Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 203 | chris | 1 | /* |
| 2 | * PROJECT: NyARToolkit |
||
| 3 | * -------------------------------------------------------------------------------- |
||
| 4 | * This work is based on the original ARToolKit developed by |
||
| 5 | * Hirokazu Kato |
||
| 6 | * Mark Billinghurst |
||
| 7 | * HITLab, University of Washington, Seattle |
||
| 8 | * http://www.hitl.washington.edu/artoolkit/ |
||
| 9 | * |
||
| 10 | * The NyARToolkit is Java version ARToolkit class library. |
||
| 11 | * Copyright (C)2008 R.Iizuka |
||
| 12 | * |
||
| 13 | * This program is free software; you can redistribute it and/or |
||
| 14 | * modify it under the terms of the GNU General Public License |
||
| 15 | * as published by the Free Software Foundation; either version 2 |
||
| 16 | * of the License, or (at your option) any later version. |
||
| 17 | * |
||
| 18 | * This program is distributed in the hope that it will be useful, |
||
| 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
| 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
| 21 | * GNU General Public License for more details. |
||
| 22 | * |
||
| 23 | * You should have received a copy of the GNU General Public License |
||
| 24 | * along with this framework; if not, write to the Free Software |
||
| 25 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
| 26 | * |
||
| 27 | * For further information please contact. |
||
| 28 | * http://nyatla.jp/nyatoolkit/ |
||
| 29 | * <airmail(at)ebony.plala.or.jp> |
||
| 30 | * |
||
| 31 | */ |
||
| 32 | package jp.nyatla.nyartoolkit.core; |
||
| 33 | |||
| 34 | import jp.nyatla.nyartoolkit.NyARException; |
||
| 35 | |||
| 36 | /** |
||
| 37 | * This class calculates ARMatrix from square information and holds it. -- |
||
| 38 | * 変換行列を計算して、結果を保持するクラス。 |
||
| 39 | * |
||
| 40 | */ |
||
| 41 | public class NyARTransMat_O2 implements NyARTransMat { |
||
| 42 | private final static int AR_FITTING_TO_IDEAL = 0;// #define |
||
| 43 | // AR_FITTING_TO_IDEAL 0 |
||
| 44 | private final static int AR_FITTING_TO_INPUT = 1;// #define |
||
| 45 | // AR_FITTING_TO_INPUT 1 |
||
| 46 | private final static int arFittingMode = AR_FITTING_TO_INPUT; |
||
| 47 | |||
| 48 | private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define |
||
| 49 | // AR_GET_TRANS_MAT_MAX_LOOP_COUNT |
||
| 50 | // 5 |
||
| 51 | private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define |
||
| 52 | // AR_GET_TRANS_MAT_MAX_FIT_ERROR |
||
| 53 | // 1.0 |
||
| 54 | private final static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR = 1.0; |
||
| 55 | private final static int P_MAX = 10;// 頂点の数(4で十分だけどなんとなく10)//#define P_MAX |
||
| 56 | // 500 |
||
| 57 | private final static int NUMBER_OF_VERTEX = 4;// 処理対象の頂点数 |
||
| 58 | private final NyARTransRot transrot; |
||
| 59 | private final double[] center = { 0.0, 0.0 }; |
||
| 60 | private final NyARParam param; |
||
| 61 | private final NyARMat result_mat = new NyARMat(3, 4); |
||
| 62 | |||
| 63 | public NyARTransMat_O2(NyARParam i_param) throws NyARException { |
||
| 64 | param = i_param; |
||
| 65 | transrot = new NyARTransRot_O3(i_param, NUMBER_OF_VERTEX); |
||
| 66 | |||
| 67 | } |
||
| 68 | |||
| 69 | public void setCenter(double i_x, double i_y) { |
||
| 70 | center[0] = i_x; |
||
| 71 | center[1] = i_x; |
||
| 72 | } |
||
| 73 | |||
| 74 | public NyARMat getTransformationMatrix() { |
||
| 75 | return result_mat; |
||
| 76 | } |
||
| 77 | |||
| 78 | /** |
||
| 79 | * transMat関数の初期化関数を分離したものです。 |
||
| 80 | * |
||
| 81 | * @param square |
||
| 82 | * @param i_direction |
||
| 83 | * @param i_width |
||
| 84 | * @param o_ppos2d |
||
| 85 | * @param o_ppos3d |
||
| 86 | */ |
||
| 87 | private final void init_transMat_ppos(NyARSquare square, int i_direction, |
||
| 88 | double i_width, double[][] o_ppos2d, double[][] o_ppos3d) { |
||
| 89 | o_ppos2d[0][0] = square.sqvertex[(4 - i_direction) % 4][0]; |
||
| 90 | o_ppos2d[0][1] = square.sqvertex[(4 - i_direction) % 4][1]; |
||
| 91 | o_ppos2d[1][0] = square.sqvertex[(5 - i_direction) % 4][0]; |
||
| 92 | o_ppos2d[1][1] = square.sqvertex[(5 - i_direction) % 4][1]; |
||
| 93 | o_ppos2d[2][0] = square.sqvertex[(6 - i_direction) % 4][0]; |
||
| 94 | o_ppos2d[2][1] = square.sqvertex[(6 - i_direction) % 4][1]; |
||
| 95 | o_ppos2d[3][0] = square.sqvertex[(7 - i_direction) % 4][0]; |
||
| 96 | o_ppos2d[3][1] = square.sqvertex[(7 - i_direction) % 4][1]; |
||
| 97 | |||
| 98 | double c0, c1, w_2; |
||
| 99 | c0 = center[0]; |
||
| 100 | c1 = center[1]; |
||
| 101 | w_2 = i_width / 2.0; |
||
| 102 | |||
| 103 | o_ppos3d[0][0] = c0 - w_2;// center[0] - w/2.0; |
||
| 104 | o_ppos3d[0][1] = c1 + w_2;// center[1] + w/2.0; |
||
| 105 | o_ppos3d[1][0] = c0 + w_2;// center[0] + w/2.0; |
||
| 106 | o_ppos3d[1][1] = c1 + w_2;// center[1] + w/2.0; |
||
| 107 | o_ppos3d[2][0] = c0 + w_2;// center[0] + w/2.0; |
||
| 108 | o_ppos3d[2][1] = c1 - w_2;// center[1] - w/2.0; |
||
| 109 | o_ppos3d[3][0] = c0 - w_2;// center[0] - w/2.0; |
||
| 110 | o_ppos3d[3][1] = c1 - w_2;// center[1] - w/2.0; |
||
| 111 | return; |
||
| 112 | } |
||
| 113 | |||
| 114 | private final double[][] wk_transMat_pos3d = new double[P_MAX][3];// pos3d[P_MAX][3]; |
||
| 115 | private final double[][] wk_transMat_ppos2d = new double[4][2]; |
||
| 116 | private final double[][] wk_transMat_ppos3d = new double[4][2]; |
||
| 117 | private final double[] wk_transMat_off = new double[3]; |
||
| 118 | private final double[][] wk_transMat_pos2d = new double[P_MAX][2];// pos2d[P_MAX][2]; |
||
| 119 | private final NyARMat wk_transMat_mat_b = new NyARMat(3, |
||
| 120 | NUMBER_OF_VERTEX * 2); |
||
| 121 | private final NyARMat wk_transMat_mat_d = new NyARMat(3, 3); |
||
| 122 | private final double[] wk_transMat_mat_trans = new double[3]; |
||
| 123 | |||
| 124 | /** |
||
| 125 | * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double |
||
| 126 | * width, double conv[3][4] ) 演算シーケンス最適化のため、arGetTransMat3等の関数フラグメントを含みます。 |
||
| 127 | * 保持している変換行列を更新する。 |
||
| 128 | * |
||
| 129 | * @param square |
||
| 130 | * 計算対象のNyARSquareオブジェクト |
||
| 131 | * @param i_direction |
||
| 132 | * @param width |
||
| 133 | * @return |
||
| 134 | * @throws NyARException |
||
| 135 | */ |
||
| 136 | public double transMat(NyARSquare square, int i_direction, double width, |
||
| 137 | NyARTransMatResult o_result_conv) throws NyARException { |
||
| 138 | double[][] ppos2d = wk_transMat_ppos2d; |
||
| 139 | double[][] ppos3d = wk_transMat_ppos3d; |
||
| 140 | double[] off = wk_transMat_off; |
||
| 141 | double[][] pos3d = wk_transMat_pos3d; |
||
| 142 | |||
| 143 | // rotationの初期化 |
||
| 144 | transrot.initRot(square, i_direction); |
||
| 145 | |||
| 146 | // ppos2dとppos3dの初期化 |
||
| 147 | init_transMat_ppos(square, i_direction, width, ppos2d, ppos3d); |
||
| 148 | |||
| 149 | // arGetTransMat3の前段処理(pos3dとoffを初期化) |
||
| 150 | double[][] pos2d = this.wk_transMat_pos2d; |
||
| 151 | final NyARMat mat_b = this.wk_transMat_mat_b; |
||
| 152 | final NyARMat mat_d = this.wk_transMat_mat_d; |
||
| 153 | |||
| 154 | arGetTransMat3_initTransMat(ppos3d, ppos2d, pos2d, pos3d, off, mat_b, |
||
| 155 | mat_d); |
||
| 156 | |||
| 157 | double err = -1; |
||
| 158 | double[] trans = this.wk_transMat_mat_trans; |
||
| 159 | for (int i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) { |
||
| 160 | // <arGetTransMat3> |
||
| 161 | err = arGetTransMatSub(pos2d, pos3d, mat_b, mat_d, trans); |
||
| 162 | // //</arGetTransMat3> |
||
| 163 | if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR) { |
||
| 164 | break; |
||
| 165 | } |
||
| 166 | } |
||
| 167 | // マトリクスの保存 |
||
| 168 | o_result_conv.updateMatrixValue(this.transrot, off, trans); |
||
| 169 | return err; |
||
| 170 | } |
||
| 171 | |||
| 172 | private final NyARTransMatResult wk_transMatContinue_result = new NyARTransMatResult(); |
||
| 173 | |||
| 174 | /** |
||
| 175 | * double arGetTransMatCont( ARMarkerInfo *marker_info, double |
||
| 176 | * prev_conv[3][4],double center[2], double width, double conv[3][4] ) |
||
| 177 | * |
||
| 178 | * @param i_square |
||
| 179 | * @param i_direction |
||
| 180 | * マーカーの方位を指定する。 |
||
| 181 | * @param i_width |
||
| 182 | * @param io_result_conv |
||
| 183 | * 計算履歴を持つNyARTransMatResultオブジェクトを指定する。 |
||
| 184 | * 履歴を持たない場合は、transMatと同じ処理を行う。 |
||
| 185 | * @return |
||
| 186 | * @throws NyARException |
||
| 187 | */ |
||
| 188 | public double transMatContinue(NyARSquare i_square, int i_direction, |
||
| 189 | double i_width, NyARTransMatResult io_result_conv) |
||
| 190 | throws NyARException { |
||
| 191 | // io_result_convが初期値なら、transMatで計算する。 |
||
| 192 | if (!io_result_conv.hasValue()) { |
||
| 193 | return this |
||
| 194 | .transMat(i_square, i_direction, i_width, io_result_conv); |
||
| 195 | } |
||
| 196 | |||
| 197 | double[][] ppos2d = wk_transMat_ppos2d; |
||
| 198 | double[][] ppos3d = wk_transMat_ppos3d; |
||
| 199 | double[] off = wk_transMat_off; |
||
| 200 | double[][] pos3d = wk_transMat_pos3d; |
||
| 201 | |||
| 202 | // arGetTransMatContSub計算部分 |
||
| 203 | transrot.initRotByPrevResult(io_result_conv); |
||
| 204 | |||
| 205 | // ppos2dとppos3dの初期化 |
||
| 206 | init_transMat_ppos(i_square, i_direction, i_width, ppos2d, ppos3d); |
||
| 207 | |||
| 208 | // arGetTransMat3の前段処理(pos3dとoffを初期化) |
||
| 209 | double[][] pos2d = this.wk_transMat_pos2d; |
||
| 210 | final NyARMat mat_b = this.wk_transMat_mat_b; |
||
| 211 | final NyARMat mat_d = this.wk_transMat_mat_d; |
||
| 212 | |||
| 213 | // transMatに必要な初期値を計算 |
||
| 214 | arGetTransMat3_initTransMat(ppos3d, ppos2d, pos2d, pos3d, off, mat_b, |
||
| 215 | mat_d); |
||
| 216 | |||
| 217 | double err1, err2; |
||
| 218 | int i; |
||
| 219 | |||
| 220 | err1 = err2 = -1; |
||
| 221 | double[] trans = this.wk_transMat_mat_trans; |
||
| 222 | for (i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) { |
||
| 223 | err1 = arGetTransMatSub(pos2d, pos3d, mat_b, mat_d, trans); |
||
| 224 | if (err1 < AR_GET_TRANS_MAT_MAX_FIT_ERROR) { |
||
| 225 | // 十分な精度を達成できたらブレーク |
||
| 226 | break; |
||
| 227 | } |
||
| 228 | } |
||
| 229 | // 値を保存 |
||
| 230 | io_result_conv.updateMatrixValue(this.transrot, off, trans); |
||
| 231 | |||
| 232 | // エラー値が許容範囲でなければTransMatをやり直し |
||
| 233 | if (err1 > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) { |
||
| 234 | NyARTransMatResult result2 = this.wk_transMatContinue_result; |
||
| 235 | // transMatを実行(初期化値は共用) |
||
| 236 | transrot.initRot(i_square, i_direction); |
||
| 237 | err2 = transMat(i_square, i_direction, i_width, result2); |
||
| 238 | // transmMatここまで |
||
| 239 | if (err2 < err1) { |
||
| 240 | // 良い値が取れたら、差換え |
||
| 241 | io_result_conv.copyFrom(result2); |
||
| 242 | err1 = err2; |
||
| 243 | } |
||
| 244 | } |
||
| 245 | return err1; |
||
| 246 | } |
||
| 247 | |||
| 248 | private final NyARMat wk_arGetTransMat3_mat_a = new NyARMat( |
||
| 249 | NUMBER_OF_VERTEX * 2, 3); |
||
| 250 | |||
| 251 | /** |
||
| 252 | * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。 |
||
| 253 | * 計算結果から再帰的に変更される可能性が無いので、切り離し。 |
||
| 254 | * |
||
| 255 | * @param i_ppos3d |
||
| 256 | * 入力配列[num][3] |
||
| 257 | * @param o_pos3d |
||
| 258 | * 出力配列[P_MAX][3] |
||
| 259 | * @param o_off |
||
| 260 | * [3] |
||
| 261 | * @throws NyARException |
||
| 262 | */ |
||
| 263 | private final void arGetTransMat3_initTransMat(double[][] i_ppos3d, |
||
| 264 | double[][] i_ppos2d, double[][] o_pos2d, double[][] o_pos3d, |
||
| 265 | double[] o_off, NyARMat o_mat_b, NyARMat o_mat_d) |
||
| 266 | throws NyARException { |
||
| 267 | double pmax0, pmax1, pmax2, pmin0, pmin1, pmin2; |
||
| 268 | int i; |
||
| 269 | pmax0 = pmax1 = pmax2 = -10000000000.0; |
||
| 270 | pmin0 = pmin1 = pmin2 = 10000000000.0; |
||
| 271 | for (i = 0; i < NUMBER_OF_VERTEX; i++) { |
||
| 272 | if (i_ppos3d[i][0] > pmax0) { |
||
| 273 | pmax0 = i_ppos3d[i][0]; |
||
| 274 | } |
||
| 275 | if (i_ppos3d[i][0] < pmin0) { |
||
| 276 | pmin0 = i_ppos3d[i][0]; |
||
| 277 | } |
||
| 278 | if (i_ppos3d[i][1] > pmax1) { |
||
| 279 | pmax1 = i_ppos3d[i][1]; |
||
| 280 | } |
||
| 281 | if (i_ppos3d[i][1] < pmin1) { |
||
| 282 | pmin1 = i_ppos3d[i][1]; |
||
| 283 | } |
||
| 284 | /* |
||
| 285 | * オリジナルでもコメントアウト if( ppos3d[i][2] > pmax[2] ) pmax[2] = |
||
| 286 | * ppos3d[i][2]; if( ppos3d[i][2] < pmin[2] ) pmin[2] = |
||
| 287 | * ppos3d[i][2]; |
||
| 288 | */ |
||
| 289 | } |
||
| 290 | o_off[0] = -(pmax0 + pmin0) / 2.0; |
||
| 291 | o_off[1] = -(pmax1 + pmin1) / 2.0; |
||
| 292 | o_off[2] = -(pmax2 + pmin2) / 2.0; |
||
| 293 | |||
| 294 | double[] o_pos3d_pt; |
||
| 295 | double[] i_pos_pd_pt; |
||
| 296 | for (i = 0; i < NUMBER_OF_VERTEX; i++) { |
||
| 297 | o_pos3d_pt = o_pos3d[i]; |
||
| 298 | i_pos_pd_pt = i_ppos3d[i]; |
||
| 299 | o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0]; |
||
| 300 | o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1]; |
||
| 301 | o_pos3d_pt[2] = 0.0; |
||
| 302 | } |
||
| 303 | // ココから先でarGetTransMatSubの初期化処理 |
||
| 304 | // arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。 |
||
| 305 | |||
| 306 | if (arFittingMode == AR_FITTING_TO_INPUT) { |
||
| 307 | // arParamIdeal2Observをバッチ処理 |
||
| 308 | param.ideal2ObservBatch(i_ppos2d, o_pos2d, NUMBER_OF_VERTEX); |
||
| 309 | } else { |
||
| 310 | for (i = 0; i < NUMBER_OF_VERTEX; i++) { |
||
| 311 | o_pos2d[i][0] = i_ppos2d[i][0]; |
||
| 312 | o_pos2d[i][1] = i_ppos2d[i][1]; |
||
| 313 | } |
||
| 314 | } |
||
| 315 | |||
| 316 | // 変換マトリクスdとbの準備(arGetTransMatSubの一部) |
||
| 317 | final double cpara[] = param.get34Array(); |
||
| 318 | final NyARMat mat_a = this.wk_arGetTransMat3_mat_a; |
||
| 319 | final double[][] a_array = mat_a.getArray(); |
||
| 320 | |||
| 321 | // mat_bの設定 |
||
| 322 | final double[][] b_array = o_mat_b.getArray(); |
||
| 323 | |||
| 324 | int x2; |
||
| 325 | for (i = 0; i < NUMBER_OF_VERTEX; i++) { |
||
| 326 | x2 = i * 2; |
||
| 327 | // </Optimize> |
||
| 328 | a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0] |
||
| 329 | // = |
||
| 330 | // mat_b->m[num*0+j*2] |
||
| 331 | // = |
||
| 332 | // cpara[0][0]; |
||
| 333 | a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1] |
||
| 334 | // = |
||
| 335 | // mat_b->m[num*2+j*2] |
||
| 336 | // = |
||
| 337 | // cpara[0][1]; |
||
| 338 | a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - o_pos2d[i][0];// mat_a->m[j*6+2] |
||
| 339 | // = |
||
| 340 | // mat_b->m[num*4+j*2] |
||
| 341 | // = |
||
| 342 | // cpara[0][2] |
||
| 343 | // - |
||
| 344 | // pos2d[j][0]; |
||
| 345 | a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] = |
||
| 346 | // mat_b->m[num*0+j*2+1] |
||
| 347 | // = 0.0; |
||
| 348 | a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4] |
||
| 349 | // = |
||
| 350 | // mat_b->m[num*2+j*2+1] |
||
| 351 | // = |
||
| 352 | // cpara[1][1]; |
||
| 353 | a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2] |
||
| 354 | - o_pos2d[i][1];// mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = |
||
| 355 | // cpara[1][2] - pos2d[j][1]; |
||
| 356 | } |
||
| 357 | |||
| 358 | // mat_d |
||
| 359 | o_mat_d.matrixMul(o_mat_b, mat_a); |
||
| 360 | o_mat_d.matrixSelfInv(); |
||
| 361 | } |
||
| 362 | |||
| 363 | private final NyARMat wk_arGetTransMatSub_mat_c = new NyARMat( |
||
| 364 | NUMBER_OF_VERTEX * 2, 1); |
||
| 365 | private final NyARMat wk_arGetTransMatSub_mat_e = new NyARMat(3, 1); |
||
| 366 | private final NyARMat wk_arGetTransMatSub_mat_f = new NyARMat(3, 1); |
||
| 367 | |||
| 368 | /** |
||
| 369 | * static double arGetTransMatSub( double rot[3][3], double |
||
| 370 | * ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double |
||
| 371 | * *dist_factor, double cpara[3][4] ) Optimize:2008.04.20:STEP[1033→1004] |
||
| 372 | * |
||
| 373 | * @param i_ppos2d |
||
| 374 | * @param i_pos3d |
||
| 375 | * @param i_mat_b |
||
| 376 | * 演算用行列b |
||
| 377 | * @param i_mat_d |
||
| 378 | * 演算用行列d |
||
| 379 | * @return |
||
| 380 | * @throws NyARException |
||
| 381 | */ |
||
| 382 | private final double arGetTransMatSub(double i_ppos2d[][], |
||
| 383 | double i_pos3d[][], NyARMat i_mat_b, NyARMat i_mat_d, |
||
| 384 | double[] o_trans) throws NyARException { |
||
| 385 | double cpara[] = param.get34Array(); |
||
| 386 | NyARMat mat_c, mat_e, mat_f;// ARMat *mat_a, *mat_b, *mat_c, *mat_d, |
||
| 387 | // *mat_e, *mat_f; |
||
| 388 | |||
| 389 | double wx, wy, wz; |
||
| 390 | double ret; |
||
| 391 | int i; |
||
| 392 | |||
| 393 | mat_c = this.wk_arGetTransMatSub_mat_c;// 次処理で値をもらうので、初期化の必要は無い。 |
||
| 394 | double[][] c_array = mat_c.getArray(); |
||
| 395 | double[] rot = transrot.getArray(); |
||
| 396 | double[] i_pos3d_pt; |
||
| 397 | int x2; |
||
| 398 | for (i = 0; i < NUMBER_OF_VERTEX; i++) { |
||
| 399 | x2 = i * 2; |
||
| 400 | i_pos3d_pt = i_pos3d[i]; |
||
| 401 | wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2] |
||
| 402 | * i_pos3d_pt[2]; |
||
| 403 | wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] |
||
| 404 | * i_pos3d_pt[2]; |
||
| 405 | wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] |
||
| 406 | * i_pos3d_pt[2]; |
||
| 407 | c_array[x2][0] = wz * i_ppos2d[i][0] - cpara[0 * 4 + 0] * wx |
||
| 408 | - cpara[0 * 4 + 1] * wy - cpara[0 * 4 + 2] * wz;// mat_c->m[j*2+0] |
||
| 409 | // = wz * |
||
| 410 | // pos2d[j][0]- |
||
| 411 | // cpara[0][0]*wx |
||
| 412 | // - |
||
| 413 | // cpara[0][1]*wy |
||
| 414 | // - |
||
| 415 | // cpara[0][2]*wz; |
||
| 416 | c_array[x2 + 1][0] = wz * i_ppos2d[i][1] - cpara[1 * 4 + 1] * wy |
||
| 417 | - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] = wz * |
||
| 418 | // pos2d[j][1]- cpara[1][1]*wy - |
||
| 419 | // cpara[1][2]*wz; |
||
| 420 | } |
||
| 421 | mat_e = this.wk_arGetTransMatSub_mat_e;// 次処理で値をもらうので、初期化の必要は無い。 |
||
| 422 | mat_f = this.wk_arGetTransMatSub_mat_f;// 次処理で値をもらうので、初期化の必要は無い。 |
||
| 423 | double[][] f_array = mat_f.getArray(); |
||
| 424 | |||
| 425 | mat_e.matrixMul(i_mat_b, mat_c); |
||
| 426 | mat_f.matrixMul(i_mat_d, mat_e); |
||
| 427 | |||
| 428 | // double[] trans=wk_arGetTransMatSub_trans;//double trans[3]; |
||
| 429 | o_trans[0] = f_array[0][0];// trans[0] = mat_f->m[0]; |
||
| 430 | o_trans[1] = f_array[1][0]; |
||
| 431 | o_trans[2] = f_array[2][0];// trans[2] = mat_f->m[2]; |
||
| 432 | ret = transrot.modifyMatrix(o_trans, i_pos3d, i_ppos2d); |
||
| 433 | for (i = 0; i < NUMBER_OF_VERTEX; i++) { |
||
| 434 | x2 = i * 2; |
||
| 435 | i_pos3d_pt = i_pos3d[i]; |
||
| 436 | wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2] |
||
| 437 | * i_pos3d_pt[2]; |
||
| 438 | wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] |
||
| 439 | * i_pos3d_pt[2]; |
||
| 440 | wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] |
||
| 441 | * i_pos3d_pt[2]; |
||
| 442 | c_array[x2][0] = wz * i_ppos2d[i][0] - cpara[0 * 4 + 0] * wx |
||
| 443 | - cpara[0 * 4 + 1] * wy - cpara[0 * 4 + 2] * wz;// mat_c->m[j*2+0] |
||
| 444 | // = wz * |
||
| 445 | // pos2d[j][0]- |
||
| 446 | // cpara[0][0]*wx |
||
| 447 | // - |
||
| 448 | // cpara[0][1]*wy |
||
| 449 | // - |
||
| 450 | // cpara[0][2]*wz; |
||
| 451 | c_array[x2 + 1][0] = wz * i_ppos2d[i][1] - cpara[1 * 4 + 1] * wy |
||
| 452 | - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] = wz * |
||
| 453 | // pos2d[j][1]- cpara[1][1]*wy - |
||
| 454 | // cpara[1][2]*wz; |
||
| 455 | } |
||
| 456 | |||
| 457 | mat_e.matrixMul(i_mat_b, mat_c); |
||
| 458 | mat_f.matrixMul(i_mat_d, mat_e); |
||
| 459 | o_trans[0] = f_array[0][0];// trans[0] = mat_f->m[0]; |
||
| 460 | o_trans[1] = f_array[1][0]; |
||
| 461 | o_trans[2] = f_array[2][0];// trans[2] = mat_f->m[2]; |
||
| 462 | ret = transrot.modifyMatrix(o_trans, i_pos3d, i_ppos2d); |
||
| 463 | return ret; |
||
| 464 | } |
||
| 465 | } |