--- /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 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.optimize.artoolkit;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.*;\r
+import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+\r
+/**\r
+ * 処理構造がわかる程度に展開したNyARRotTransOptimize\r
+ * \r
+ */\r
+public class NyARRotMatrixOptimize_Base implements INyARRotMatrixOptimize\r
+{\r
+ private final NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
+\r
+ public NyARRotMatrixOptimize_Base(NyARPerspectiveProjectionMatrix i_projection_mat_ref)\r
+ {\r
+ this._projection_mat_ref = i_projection_mat_ref;\r
+ return;\r
+ }\r
+ private double[] __createRotationMap_b_map=new double[6];\r
+ private double[] __createRotationMap_c_map=new double[6];\r
+ private double[] __createRotationMap_f=new double[3];\r
+ private void createRotationMap(NyARDoublePoint3d i_angle,double i_factor,NyARDoubleMatrix33[] i_rot_matrix)\r
+ {\r
+ double sina,cosa,sinb,cosb,sinc,cosc;\r
+ double CACA,SASA,SACA,SASB,CASB,SACACB,CACACB,SASACB;\r
+\r
+ \r
+ final double[] f=this.__createRotationMap_f;\r
+ final double[] b_map=this.__createRotationMap_b_map;\r
+ final double[] c_map=this.__createRotationMap_c_map;\r
+ f[0]=-i_factor;\r
+ f[1]=0;\r
+ f[2]=i_factor;\r
+ double ang1,ang2;\r
+ //BとCのsinマップを先に作成\r
+ for(int i=0;i<3;i++)\r
+ {\r
+ ang1=i_angle.y + f[i];\r
+ b_map[i] =Math.sin(ang1);\r
+ b_map[i+3]=Math.cos(ang1);\r
+ ang2=i_angle.z + f[i];\r
+ c_map[i] =Math.sin(ang2);\r
+ c_map[i+3]=Math.cos(ang2);\r
+ }\r
+ int idx=0;\r
+ int t1,t2,t3;\r
+ for (t1 = 0; t1 < 3; t1++){\r
+ ang1=i_angle.x + f[t1];\r
+ sina = Math.sin(ang1);\r
+ cosa = Math.cos(ang1); \r
+ CACA = cosa * cosa;\r
+ SASA = sina * sina;\r
+ SACA = sina * cosa;\r
+\r
+ for (t2=0;t2<3;t2++){\r
+ sinb = b_map[t2];\r
+ cosb = b_map[t2+3];\r
+ SASB = sina * sinb;\r
+ CASB = cosa * sinb;\r
+ SACACB = SACA * cosb;\r
+ CACACB = CACA * cosb;\r
+ SASACB = SASA * cosb; \r
+ for (t3=0;t3<3;t3++) {\r
+ sinc = c_map[t3];\r
+ cosc = c_map[t3+3];\r
+ final NyARDoubleMatrix33 mat_ptr=i_rot_matrix[idx];\r
+ mat_ptr.m00 = CACACB * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;\r
+ mat_ptr.m01 = -CACACB * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;\r
+ mat_ptr.m02 = CASB;\r
+ mat_ptr.m10 = SACACB * cosc - SACA * cosc + SASACB * sinc + CACA * sinc;\r
+ mat_ptr.m11 = -SACACB * sinc + SACA * sinc + SASACB * cosc + CACA * cosc;\r
+ mat_ptr.m12 = SASB;\r
+ mat_ptr.m20 = -CASB * cosc - SASB * sinc;\r
+ mat_ptr.m21 = CASB * sinc - SASB * cosc;\r
+ mat_ptr.m22 = cosb;\r
+ idx++;\r
+ }\r
+ }\r
+ }\r
+ return;\r
+ }\r
+ private final void getNewMatrix(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans, NyARDoubleMatrix34 o_combo)\r
+ {\r
+ double cp0,cp1,cp2,cp3;\r
+ NyARPerspectiveProjectionMatrix cp=this._projection_mat_ref;\r
+\r
+ cp3=cp.m03;\r
+ cp0=cp.m00;cp1=cp.m01;cp2=cp.m02;\r
+ o_combo.m00=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;\r
+ o_combo.m01=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;\r
+ o_combo.m02=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;\r
+ o_combo.m03=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;\r
+\r
+ cp0=cp.m10;cp1=cp.m11;cp2=cp.m12;\r
+ o_combo.m10=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;\r
+ o_combo.m11=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;\r
+ o_combo.m12=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;\r
+ o_combo.m13=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;\r
+\r
+ cp0=cp.m20;cp1=cp.m21;cp2=cp.m22;\r
+ o_combo.m20=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;\r
+ o_combo.m21=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;\r
+ o_combo.m22=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;\r
+ o_combo.m23=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;\r
+ return;\r
+ }\r
+ private final NyARDoublePoint3d __modifyMatrix_angle = new NyARDoublePoint3d();\r
+ private final NyARDoubleMatrix34 __modifyMatrix_combo=new NyARDoubleMatrix34();\r
+ private final NyARDoubleMatrix33[] __modifyMatrix_next_rot_matrix=NyARDoubleMatrix33.createArray(27); \r
+ public double modifyMatrix(NyARRotMatrix_ARToolKit io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d) throws NyARException\r
+ {\r
+ final NyARDoublePoint3d angle = this.__modifyMatrix_angle;\r
+ final NyARDoubleMatrix34 combo=this.__modifyMatrix_combo;\r
+ final NyARDoubleMatrix33[] next_rot_matrix=this.__modifyMatrix_next_rot_matrix;\r
+ double factor;\r
+ double hx, hy, h, x, y;\r
+ double err, minerr = 0;\r
+ int i,i2;\r
+ int best_idx=0;\r
+ angle.setValue(io_rot.refAngle());// arGetAngle( rot, &a, &b, &c );\r
+ factor = 10.0 * Math.PI / 180.0;\r
+ for (int j = 0; j < 10; j++){\r
+ minerr = 1000000000.0;\r
+ //評価用の角度マップ作成\r
+ createRotationMap(angle,factor,next_rot_matrix);\r
+ //評価して一番宜しいIDを保存\r
+ best_idx=(1+1*3+1*9);\r
+ for(i2=0;i2<27;i2++){\r
+ this.getNewMatrix(next_rot_matrix[i2],i_trans,combo);\r
+ err = 0.0;\r
+ for (i = 0; i < 4; i++) {\r
+ hx = combo.m00 * i_vertex3d[i].x + combo.m01 * i_vertex3d[i].y + combo.m02 * i_vertex3d[i].z + combo.m03;\r
+ hy = combo.m10 * i_vertex3d[i].x + combo.m11 *i_vertex3d[i].y + combo.m12 * i_vertex3d[i].z + combo.m13;\r
+ h = combo.m20 * i_vertex3d[i].x + combo.m21 * i_vertex3d[i].y + combo.m22 * i_vertex3d[i].z + combo.m23;\r
+ x = i_vertex2d[i].x-(hx / h);\r
+ y = i_vertex2d[i].y-(hy / h);\r
+ err += x*x+y*y;\r
+ }\r
+ if (err < minerr){\r
+ minerr = err;\r
+ best_idx=i2;\r
+ }\r
+ \r
+ }\r
+ if (best_idx==(1+1*3+1*9)){\r
+ factor *= 0.5; \r
+ }else{\r
+ angle.z+=factor*(best_idx%3-1);\r
+ angle.y+=factor*((best_idx/3)%3-1);\r
+ angle.x+=factor*((best_idx/9)%3-1); \r
+ }\r
+ }\r
+ io_rot.setAngle(angle.x,angle.y,angle.z);\r
+ /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
+ return minerr / 4;\r
+ }\r
+\r
+\r
+\r
+\r
+}\r
OSDN Git Service
