{\r
this.__initRot_vec1=new NyARRotVector(i_matrix);\r
this.__initRot_vec2=new NyARRotVector(i_matrix);\r
+ this._angle=new NyARDoublePoint3d();\r
return;\r
}\r
final private NyARRotVector __initRot_vec1;\r
final private NyARRotVector __initRot_vec2;\r
+ final protected NyARDoublePoint3d _angle;\r
\r
\r
\r
this.m02 = w02/w;\r
this.m12 = w12/w;\r
this.m22 = w22/w;\r
+ //Matrixからangleをロード\r
+ this.updateAngleFromMatrix();\r
return;\r
}\r
+ public final NyARDoublePoint3d refAngle()\r
+ {\r
+ return this._angle;\r
+ }\r
+ /**\r
+ * 回転角から回転行列を計算してセットします。\r
+ * @param i_x\r
+ * @param i_y\r
+ * @param i_z\r
+ */\r
+ public void setAngle(final double i_x, final double i_y, final double i_z)\r
+ {\r
+ final double sina = Math.sin(i_x);\r
+ final double cosa = Math.cos(i_x);\r
+ final double sinb = Math.sin(i_y);\r
+ final double cosb = Math.cos(i_y);\r
+ final double sinc = Math.sin(i_z);\r
+ final double cosc = Math.cos(i_z);\r
+ // Optimize\r
+ final double CACA = cosa * cosa;\r
+ final double SASA = sina * sina;\r
+ final double SACA = sina * cosa;\r
+ final double SASB = sina * sinb;\r
+ final double CASB = cosa * sinb;\r
+ final double SACACB = SACA * cosb;\r
\r
- \r
-\r
+ this.m00 = CACA * cosb * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;\r
+ this.m01 = -CACA * cosb * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;\r
+ this.m02 = CASB;\r
+ this.m10 = SACACB * cosc - SACA * cosc + SASA * cosb * sinc + CACA * sinc;\r
+ this.m11 = -SACACB * sinc + SACA * sinc + SASA * cosb * cosc + CACA * cosc;\r
+ this.m12 = SASB;\r
+ this.m20 = -CASB * cosc - SASB * sinc;\r
+ this.m21 = CASB * sinc - SASB * cosc;\r
+ this.m22 = cosb;\r
+ updateAngleFromMatrix();\r
+ return;\r
+ }\r
/**\r
- * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc )\r
- * Optimize:2008.04.20:STEP[481→433]\r
- * 3x3変換行列から、回転角を復元して返します。\r
+ * i_in_pointを変換行列で座標変換する。\r
+ * @param i_in_point\r
+ * @param i_out_point\r
+ */\r
+ public final void getPoint3d(final NyARDoublePoint3d i_in_point,final NyARDoublePoint3d i_out_point)\r
+ {\r
+ final double x=i_in_point.x;\r
+ final double y=i_in_point.y;\r
+ final double z=i_in_point.z;\r
+ i_out_point.x=this.m00 * x + this.m01 * y + this.m02 * z;\r
+ i_out_point.y=this.m10 * x + this.m11 * y + this.m12 * z;\r
+ i_out_point.z=this.m20 * x + this.m21 * y + this.m22 * z;\r
+ return;\r
+ }\r
+ /**\r
+ * 複数の頂点を一括して変換する\r
+ * @param i_in_point\r
+ * @param i_out_point\r
+ * @param i_number_of_vertex\r
+ */\r
+ public final void getPoint3dBatch(final NyARDoublePoint3d[] i_in_point,NyARDoublePoint3d[] i_out_point,int i_number_of_vertex)\r
+ {\r
+ for(int i=i_number_of_vertex-1;i>=0;i--){\r
+ final NyARDoublePoint3d out_ptr=i_out_point[i];\r
+ final NyARDoublePoint3d in_ptr=i_in_point[i];\r
+ final double x=in_ptr.x;\r
+ final double y=in_ptr.y;\r
+ final double z=in_ptr.z;\r
+ out_ptr.x=this.m00 * x + this.m01 * y + this.m02 * z;\r
+ out_ptr.y=this.m10 * x + this.m11 * y + this.m12 * z;\r
+ out_ptr.z=this.m20 * x + this.m21 * y + this.m22 * z;\r
+ }\r
+ return;\r
+ }\r
+ /**\r
+ * 現在のMatrixからangkeを復元する。\r
* @param o_angle\r
- * @return\r
*/\r
- public final void getAngle(final NyARDoublePoint3d o_angle)\r
+ private final void updateAngleFromMatrix()\r
{\r
double a,b,c;\r
double sina, cosa, sinb,cosb,sinc, cosc;\r
cosa = rot02 / sinb;// <Optimize/>cosa = rot[0][2] / sinb;\r
sina = rot12 / sinb;// <Optimize/>sina = rot[1][2] / sinb;\r
if (cosa > 1.0) {\r
- /* printf("cos(alph) = %f\n", cosa); */\r
cosa = 1.0;\r
sina = 0.0;\r
}\r
if (cosa < -1.0) {\r
- /* printf("cos(alph) = %f\n", cosa); */\r
cosa = -1.0;\r
sina = 0.0;\r
}\r
if (sina > 1.0) {\r
- /* printf("sin(alph) = %f\n", sina); */\r
sina = 1.0;\r
cosa = 0.0;\r
}\r
if (sina < -1.0) {\r
- /* printf("sin(alph) = %f\n", sina); */\r
sina = -1.0;\r
cosa = 0.0;\r
}\r
if (sina < 0) {\r
a = -a;\r
}\r
- // <Optimize>\r
- // sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
- // cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
final double tmp = (rot02 * rot02 + rot12 * rot12);\r
sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;\r
cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;\r
- // </Optimize>\r
\r
if (cosc > 1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
cosc = 1.0;\r
sinc = 0.0;\r
}\r
if (cosc < -1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
cosc = -1.0;\r
sinc = 0.0;\r
}\r
if (sinc > 1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
sinc = 1.0;\r
cosc = 0.0;\r
}\r
if (sinc < -1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
sinc = -1.0;\r
cosc = 0.0;\r
}\r
cosc=this.m00;//cosc = rot[0];// <Optimize/>cosc = rot[0][0];\r
sinc=this.m01;//sinc = rot[1];// <Optimize/>sinc = rot[1][0];\r
if (cosc > 1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
cosc = 1.0;\r
sinc = 0.0;\r
}\r
if (cosc < -1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
cosc = -1.0;\r
sinc = 0.0;\r
}\r
if (sinc > 1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
sinc = 1.0;\r
cosc = 0.0;\r
}\r
if (sinc < -1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
sinc = -1.0;\r
cosc = 0.0;\r
}\r
c = -c;\r
}\r
}\r
- o_angle.x = a;// wa.value=a;//*wa = a;\r
- o_angle.y = b;// wb.value=b;//*wb = b;\r
- o_angle.z = c;// wc.value=c;//*wc = c;\r
- return;\r
- }\r
- /**\r
- * 回転角から回転行列を計算してセットします。\r
- * @param i_x\r
- * @param i_y\r
- * @param i_z\r
- */\r
- public final void setAngle(final double i_x, final double i_y, final double i_z)\r
- {\r
- final double sina = Math.sin(i_x);\r
- final double cosa = Math.cos(i_x);\r
- final double sinb = Math.sin(i_y);\r
- final double cosb = Math.cos(i_y);\r
- final double sinc = Math.sin(i_z);\r
- final double cosc = Math.cos(i_z);\r
- // Optimize\r
- final double CACA = cosa * cosa;\r
- final double SASA = sina * sina;\r
- final double SACA = sina * cosa;\r
- final double SASB = sina * sinb;\r
- final double CASB = cosa * sinb;\r
- final double SACACB = SACA * cosb;\r
-\r
- this.m00 = CACA * cosb * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;\r
- this.m01 = -CACA * cosb * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;\r
- this.m02 = CASB;\r
- this.m10 = SACACB * cosc - SACA * cosc + SASA * cosb * sinc + CACA * sinc;\r
- this.m11 = -SACACB * sinc + SACA * sinc + SASA * cosb * cosc + CACA * cosc;\r
- this.m12 = SASB;\r
- this.m20 = -CASB * cosc - SASB * sinc;\r
- this.m21 = CASB * sinc - SASB * cosc;\r
- this.m22 = cosb;\r
- return;\r
- }\r
- /**\r
- * i_in_pointを変換行列で座標変換する。\r
- * @param i_in_point\r
- * @param i_out_point\r
- */\r
- public final void getPoint3d(final NyARDoublePoint3d i_in_point,final NyARDoublePoint3d i_out_point)\r
- {\r
- final double x=i_in_point.x;\r
- final double y=i_in_point.y;\r
- final double z=i_in_point.z;\r
- i_out_point.x=this.m00 * x + this.m01 * y + this.m02 * z;\r
- i_out_point.y=this.m10 * x + this.m11 * y + this.m12 * z;\r
- i_out_point.z=this.m20 * x + this.m21 * y + this.m22 * z;\r
- return;\r
- }\r
- /**\r
- * 複数の頂点を一括して変換する\r
- * @param i_in_point\r
- * @param i_out_point\r
- * @param i_number_of_vertex\r
- */\r
- public final void getPoint3dBatch(final NyARDoublePoint3d[] i_in_point,NyARDoublePoint3d[] i_out_point,int i_number_of_vertex)\r
- {\r
- for(int i=i_number_of_vertex-1;i>=0;i--){\r
- final NyARDoublePoint3d out_ptr=i_out_point[i];\r
- final NyARDoublePoint3d in_ptr=i_in_point[i];\r
- final double x=in_ptr.x;\r
- final double y=in_ptr.y;\r
- final double z=in_ptr.z;\r
- out_ptr.x=this.m00 * x + this.m01 * y + this.m02 * z;\r
- out_ptr.y=this.m10 * x + this.m11 * y + this.m12 * z;\r
- out_ptr.z=this.m20 * x + this.m21 * y + this.m22 * z;\r
- }\r
+ //angleの更新\r
+ this._angle.x = a;// wa.value=a;//*wa = a;\r
+ this._angle.y = b;// wb.value=b;//*wb = b;\r
+ this._angle.z = c;// wc.value=c;//*wc = c;\r
return;\r
} \r
}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core.transmat.rotmatrix;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+/**\r
+ * 回転行列計算用の、3x3行列\r
+ * 計算方法はARToolKitと同じだが、ARToolKitにある不要な行列から角度を逆算する\r
+ * 処理を省略しているため、下位12桁目の計算値が異なる。\r
+ *\r
+ */\r
+public class NyARRotMatrix_NyARToolKit extends NyARRotMatrix_ARToolKit\r
+{ \r
+ /**\r
+ * インスタンスを準備します。\r
+ * \r
+ * @param i_param\r
+ */\r
+ public NyARRotMatrix_NyARToolKit(NyARPerspectiveProjectionMatrix i_matrix) throws NyARException\r
+ {\r
+ super(i_matrix);\r
+ return;\r
+ }\r
+ public final void setAngle(final double i_x, final double i_y, final double i_z)\r
+ {\r
+ final double sina = Math.sin(i_x);\r
+ final double cosa = Math.cos(i_x);\r
+ final double sinb = Math.sin(i_y);\r
+ final double cosb = Math.cos(i_y);\r
+ final double sinc = Math.sin(i_z);\r
+ final double cosc = Math.cos(i_z);\r
+ // Optimize\r
+ final double CACA = cosa * cosa;\r
+ final double SASA = sina * sina;\r
+ final double SACA = sina * cosa;\r
+ final double SASB = sina * sinb;\r
+ final double CASB = cosa * sinb;\r
+ final double SACACB = SACA * cosb;\r
+\r
+ this.m00 = CACA * cosb * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;\r
+ this.m01 = -CACA * cosb * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;\r
+ this.m02 = CASB;\r
+ this.m10 = SACACB * cosc - SACA * cosc + SASA * cosb * sinc + CACA * sinc;\r
+ this.m11 = -SACACB * sinc + SACA * sinc + SASA * cosb * cosc + CACA * cosc;\r
+ this.m12 = SASB;\r
+ this.m20 = -CASB * cosc - SASB * sinc;\r
+ this.m21 = CASB * sinc - SASB * cosc;\r
+ this.m22 = cosb;\r
+ //angleを逆計算せずに直接代入\r
+ this._angle.x=i_x;\r
+ this._angle.y=i_y;\r
+ this._angle.z=i_z;\r
+ return;\r
+ }\r
+ \r
+}\r
OSDN Git Service
