+/* \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 edition ARToolKit class library.\r
+ * Copyright (C)2008-2009 Ryo Iizuka\r
+ *\r
+ * This program is free software: you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation, either version 3 of the License, or\r
+ * (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 program. If not, see <http://www.gnu.org/licenses/>.\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.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.NyARPerspectiveProjectionMatrix;\r
import jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult;\r
import jp.nyatla.nyartoolkit.core.types.*;\r
-import jp.nyatla.nyartoolkit.core.*;\r
+import jp.nyatla.nyartoolkit.core.types.matrix.NyARDoubleMatrix33;\r
/**\r
* 回転行列計算用の、3x3行列\r
*\r
*/\r
-public class NyARRotMatrix\r
+public class NyARRotMatrix extends NyARDoubleMatrix33\r
{\r
- public double m00;\r
- public double m01;\r
- public double m02;\r
- public double m10;\r
- public double m11;\r
- public double m12;\r
- public double m20;\r
- public double m21;\r
- public double m22;\r
- \r
/**\r
* インスタンスを準備します。\r
* \r
* @param i_param\r
*/\r
- public NyARRotMatrix(NyARParam i_param) throws NyARException\r
+ public NyARRotMatrix(NyARPerspectiveProjectionMatrix i_matrix) throws NyARException\r
{\r
- this.__initRot_vec1=new NyARRotVector(i_param);\r
- this.__initRot_vec2=new NyARRotVector(i_param);\r
+ this.__initRot_vec1=new NyARRotVector(i_matrix);\r
+ this.__initRot_vec2=new NyARRotVector(i_matrix);\r
return;\r
}\r
final private NyARRotVector __initRot_vec1;\r
final private NyARRotVector __initRot_vec2;\r
-\r
- \r
-\r
+ /**\r
+ * NyARTransMatResultの内容からNyARRotMatrixを復元します。\r
+ * @param i_prev_result\r
+ */\r
public final void initRotByPrevResult(NyARTransMatResult i_prev_result)\r
{\r
- double[][] prev_array = i_prev_result.getArray();\r
- double[] pt;\r
- pt = prev_array[0];\r
- this.m00=pt[0];\r
- this.m01=pt[1];\r
- this.m02=pt[2];\r
- pt = prev_array[1];\r
- this.m10=pt[0];\r
- this.m11=pt[1];\r
- this.m12=pt[2];\r
- pt = prev_array[2];\r
- this.m20=pt[0];\r
- this.m21=pt[1];\r
- this.m22=pt[2];\r
+\r
+ this.m00=i_prev_result.m00;\r
+ this.m01=i_prev_result.m01;\r
+ this.m02=i_prev_result.m02;\r
+\r
+ this.m10=i_prev_result.m10;\r
+ this.m11=i_prev_result.m11;\r
+ this.m12=i_prev_result.m12;\r
+\r
+ this.m20=i_prev_result.m20;\r
+ this.m21=i_prev_result.m21;\r
+ this.m22=i_prev_result.m22;\r
+ return;\r
} \r
- \r
- \r
- public final void initRotBySquare(final NyARLinear[] i_linear,final NyARDoublePoint2d[] i_sqvertex) throws NyARException\r
+ /**\r
+ * \r
+ * @param i_linear\r
+ * @param i_sqvertex\r
+ * @throws NyARException\r
+ */\r
+ public void initRotBySquare(final NyARLinear[] i_linear,final NyARDoublePoint2d[] i_sqvertex) throws NyARException\r
{\r
final NyARRotVector vec1=this.__initRot_vec1;\r
final NyARRotVector vec2=this.__initRot_vec2;\r
\r
//軸２\r
vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);\r
- vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[1]);\r
+ vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);\r
\r
//回転の最適化？\r
NyARRotVector.checkRotation(vec1,vec2);\r
this.m21 =vec2.v3;\r
\r
//最後の軸を計算\r
- this.m02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2;\r
- this.m12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3;\r
- this.m22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1;\r
- final double w = Math.sqrt(this.m02 * this.m02 + this.m12 * this.m12 + this.m22 * this.m22);\r
- this.m02 /= w;\r
- this.m12 /= w;\r
- this.m22 /= w;\r
- return;\r
- }\r
-\r
- \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
- * @param o_angle\r
- * @return\r
- */\r
- public final void getAngle(final NyARDoublePoint3d o_angle)\r
- {\r
- double a,b,c;\r
- double sina, cosa, sinb,cosb,sinc, cosc;\r
- \r
- if (this.m22 > 1.0) {// <Optimize/>if( rot[2][2] > 1.0 ) {\r
- this.m22 = 1.0;// <Optimize/>rot[2][2] = 1.0;\r
- } else if (this.m22 < -1.0) {// <Optimize/>}else if( rot[2][2] < -1.0 ) {\r
- this.m22 = -1.0;// <Optimize/>rot[2][2] = -1.0;\r
- }\r
- cosb =this.m22;// <Optimize/>cosb = rot[2][2];\r
- b = Math.acos(cosb);\r
- sinb =Math.sin(b);\r
- final double rot02=this.m02;\r
- final double rot12=this.m12;\r
- if (b >= 0.000001 || b <= -0.000001) {\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
- a = Math.acos(cosa);\r
- if (sina < 0) {\r
- a = -a;\r
- }\r
- // <Optimize>\r
- // sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/\r
- // (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])/\r
- // (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
- c = Math.acos(cosc);\r
- if (sinc < 0) {\r
- c = -c;\r
- }\r
- } else {\r
- a = b = 0.0;\r
- cosa = cosb = 1.0;\r
- sina = sinb = 0.0;\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 = Math.acos(cosc);\r
- if (sinc < 0) {\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
+ final double w02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2;\r
+ final double w12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3;\r
+ final double w22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1;\r
+ final double w = Math.sqrt(w02 * w02 + w12 * w12 + w22 * w22);\r
+ this.m02 = w02/w;\r
+ this.m12 = w12/w;\r
+ this.m22 = w22/w;\r
return;\r
}\r
/**\r
out_ptr.z=this.m20 * x + this.m21 * y + this.m22 * z;\r
}\r
return;\r
- } \r
+ }\r
}\r
OSDN Git Service
