2 * PROJECT: NyARToolkit
\r
3 * --------------------------------------------------------------------------------
\r
4 * This work is based on the original ARToolKit developed by
\r
7 * HITLab, University of Washington, Seattle
\r
8 * http://www.hitl.washington.edu/artoolkit/
\r
10 * The NyARToolkit is Java version ARToolkit class library.
\r
11 * Copyright (C)2008 R.Iizuka
\r
13 * This program is free software; you can redistribute it and/or
\r
14 * modify it under the terms of the GNU General Public License
\r
15 * as published by the Free Software Foundation; either version 2
\r
16 * of the License, or (at your option) any later version.
\r
18 * This program is distributed in the hope that it will be useful,
\r
19 * but WITHOUT ANY WARRANTY; without even the implied warranty of
\r
20 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
\r
21 * GNU General Public License for more details.
\r
23 * You should have received a copy of the GNU General Public License
\r
24 * along with this framework; if not, write to the Free Software
\r
25 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
\r
27 * For further information please contact.
\r
28 * http://nyatla.jp/nyatoolkit/
\r
29 * <airmail(at)ebony.plala.or.jp>
\r
32 package jp.nyatla.nyartoolkit.core.transmat.optimize;
\r
34 import jp.nyatla.nyartoolkit.NyARException;
\r
35 import jp.nyatla.nyartoolkit.core.param.*;
\r
36 import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.NyARRotMatrix;
\r
37 import jp.nyatla.nyartoolkit.core.types.matrix.*;
\r
38 import jp.nyatla.nyartoolkit.core.types.*;
\r
41 * 処理構造がわかる程度に展開したNyARRotTransOptimize
\r
44 public class NyARRotMatrixOptimize_Base implements INyARRotMatrixOptimize
\r
46 private final NyARPerspectiveProjectionMatrix _projection_mat_ref;
\r
48 public NyARRotMatrixOptimize_Base(NyARPerspectiveProjectionMatrix i_projection_mat_ref)
\r
50 this._projection_mat_ref = i_projection_mat_ref;
\r
53 private double[] __createRotationMap_b_map=new double[6];
\r
54 private double[] __createRotationMap_c_map=new double[6];
\r
55 private double[] __createRotationMap_f=new double[3];
\r
56 private void createRotationMap(NyARDoublePoint3d i_angle,double i_factor,NyARDoubleMatrix33[] i_rot_matrix)
\r
58 double sina,cosa,sinb,cosb,sinc,cosc;
\r
59 double CACA,SASA,SACA,SASB,CASB,SACACB,CACACB,SASACB;
\r
62 final double[] f=this.__createRotationMap_f;
\r
63 final double[] b_map=this.__createRotationMap_b_map;
\r
64 final double[] c_map=this.__createRotationMap_c_map;
\r
70 for(int i=0;i<3;i++)
\r
72 ang1=i_angle.y + f[i];
\r
73 b_map[i] =Math.sin(ang1);
\r
74 b_map[i+3]=Math.cos(ang1);
\r
75 ang2=i_angle.z + f[i];
\r
76 c_map[i] =Math.sin(ang2);
\r
77 c_map[i+3]=Math.cos(ang2);
\r
81 for (t1 = 0; t1 < 3; t1++){
\r
82 ang1=i_angle.x + f[t1];
\r
83 sina = Math.sin(ang1);
\r
84 cosa = Math.cos(ang1);
\r
89 for (t2=0;t2<3;t2++){
\r
94 SACACB = SACA * cosb;
\r
95 CACACB = CACA * cosb;
\r
96 SASACB = SASA * cosb;
\r
97 for (t3=0;t3<3;t3++) {
\r
100 final NyARDoubleMatrix33 mat_ptr=i_rot_matrix[idx];
\r
101 mat_ptr.m00 = CACACB * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;
\r
102 mat_ptr.m01 = -CACACB * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;
\r
103 mat_ptr.m02 = CASB;
\r
104 mat_ptr.m10 = SACACB * cosc - SACA * cosc + SASACB * sinc + CACA * sinc;
\r
105 mat_ptr.m11 = -SACACB * sinc + SACA * sinc + SASACB * cosc + CACA * cosc;
\r
106 mat_ptr.m12 = SASB;
\r
107 mat_ptr.m20 = -CASB * cosc - SASB * sinc;
\r
108 mat_ptr.m21 = CASB * sinc - SASB * cosc;
\r
109 mat_ptr.m22 = cosb;
\r
116 private final void getNewMatrix(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans, NyARDoubleMatrix34 o_combo)
\r
118 double cp0,cp1,cp2,cp3;
\r
119 NyARPerspectiveProjectionMatrix cp=this._projection_mat_ref;
\r
122 cp0=cp.m00;cp1=cp.m01;cp2=cp.m02;
\r
123 o_combo.m00=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;
\r
124 o_combo.m01=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;
\r
125 o_combo.m02=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;
\r
126 o_combo.m03=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;
\r
128 cp0=cp.m10;cp1=cp.m11;cp2=cp.m12;
\r
129 o_combo.m10=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;
\r
130 o_combo.m11=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;
\r
131 o_combo.m12=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;
\r
132 o_combo.m13=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;
\r
134 cp0=cp.m20;cp1=cp.m21;cp2=cp.m22;
\r
135 o_combo.m20=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;
\r
136 o_combo.m21=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;
\r
137 o_combo.m22=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;
\r
138 o_combo.m23=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;
\r
141 private final NyARDoublePoint3d __modifyMatrix_angle = new NyARDoublePoint3d();
\r
142 private final NyARDoubleMatrix34 __modifyMatrix_combo=new NyARDoubleMatrix34();
\r
143 private final NyARDoubleMatrix33[] __modifyMatrix_next_rot_matrix=NyARDoubleMatrix33.createArray(27);
\r
144 public double modifyMatrix(NyARRotMatrix io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d) throws NyARException
\r
146 final NyARDoublePoint3d angle = this.__modifyMatrix_angle;
\r
147 final NyARDoubleMatrix34 combo=this.__modifyMatrix_combo;
\r
148 final NyARDoubleMatrix33[] next_rot_matrix=this.__modifyMatrix_next_rot_matrix;
\r
150 double hx, hy, h, x, y;
\r
151 double err, minerr = 0;
\r
154 angle.setValue(io_rot.refAngle());// arGetAngle( rot, &a, &b, &c );
\r
155 factor = 10.0 * Math.PI / 180.0;
\r
156 for (int j = 0; j < 10; j++){
\r
157 minerr = 1000000000.0;
\r
159 createRotationMap(angle,factor,next_rot_matrix);
\r
161 best_idx=(1+1*3+1*9);
\r
162 for(i2=0;i2<27;i2++){
\r
163 this.getNewMatrix(next_rot_matrix[i2],i_trans,combo);
\r
165 for (i = 0; i < 4; i++) {
\r
166 hx = combo.m00 * i_vertex3d[i].x + combo.m01 * i_vertex3d[i].y + combo.m02 * i_vertex3d[i].z + combo.m03;
\r
167 hy = combo.m10 * i_vertex3d[i].x + combo.m11 *i_vertex3d[i].y + combo.m12 * i_vertex3d[i].z + combo.m13;
\r
168 h = combo.m20 * i_vertex3d[i].x + combo.m21 * i_vertex3d[i].y + combo.m22 * i_vertex3d[i].z + combo.m23;
\r
169 x = i_vertex2d[i].x-(hx / h);
\r
170 y = i_vertex2d[i].y-(hy / h);
\r
179 if (best_idx==(1+1*3+1*9)){
\r
182 angle.z+=factor*(best_idx%3-1);
\r
183 angle.y+=factor*((best_idx/3)%3-1);
\r
184 angle.x+=factor*((best_idx/9)%3-1);
\r
187 io_rot.setAngle(angle.x,angle.y,angle.z);
\r
188 /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */
\r