Subversion Repositories AndroidProjects

/*

 * PROJECT: NyARToolkit

 * --------------------------------------------------------------------------------

 * This work is based on the original ARToolKit developed by

 *   Hirokazu Kato

 *   Mark Billinghurst

 *   HITLab, University of Washington, Seattle

 * http://www.hitl.washington.edu/artoolkit/

 *

 * The NyARToolkit is Java version ARToolkit class library.

 * Copyright (C)2008 R.Iizuka

 *

 * This program is free software; you can redistribute it and/or

 * modify it under the terms of the GNU General Public License

 * as published by the Free Software Foundation; either version 2

 * of the License, or (at your option) any later version.

 *

 * This program is distributed in the hope that it will be useful,

 * but WITHOUT ANY WARRANTY; without even the implied warranty of

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

 * GNU General Public License for more details.

 *

 * You should have received a copy of the GNU General Public License

 * along with this framework; if not, write to the Free Software

 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

 *

 * For further information please contact.

 *      http://nyatla.jp/nyatoolkit/

 *      <airmail(at)ebony.plala.or.jp>

 *

 */

package jp.nyatla.nyartoolkit.core;

import jp.nyatla.nyartoolkit.NyARException;

import jp.nyatla.util.DoubleValue;

public class NyARTransMat_O1 implements NyARTransMat {

        private final static int AR_FITTING_TO_IDEAL = 0;// #define

        // AR_FITTING_TO_IDEAL 0

        private final static int AR_FITTING_TO_INPUT = 1;// #define

        // AR_FITTING_TO_INPUT 1

        private final static int arFittingMode = AR_FITTING_TO_INPUT;

        private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define

        // AR_GET_TRANS_MAT_MAX_LOOP_COUNT

        // 5

        private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define

        // AR_GET_TRANS_MAT_MAX_FIT_ERROR

        // 1.0

        private final static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR = 1.0;

        private final static int P_MAX = 10;// 頂点の数(4で十分だけどなんとなく10)//#define P_MAX

        // 500

        private final static int NUMBER_OF_VERTEX = 4;// 処理対象の頂点数

        private final NyARTransRot transrot;

        private final double[] center = { 0.0, 0.0 };

        private final NyARParam param;

        // private final NyARMat result_mat=new NyARMat(3,4);

        public NyARTransMat_O1(NyARParam i_param) throws NyARException {

                param = i_param;

                transrot = new NyARTransRot_O3(i_param, NUMBER_OF_VERTEX);

        }

        public void setCenter(double i_x, double i_y) {

                center[0] = i_x;

                center[1] = i_x;

        }

        // public NyARMat getTransformationMatrix()

        // {

        // return result_mat;

        // }

        private final double[][] wk_transMat_pos3d = new double[P_MAX][3];// pos3d[P_MAX][3];

        private final double[][] wk_transMat_ppos2d = new double[4][2];

        private final double[][] wk_transMat_ppos3d = new double[4][2];

        private final double[] wk_transMat_off = new double[3];

        /**

         * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double

         * width, double conv[3][4] ) 関数の置き換え。 保持している変換行列を更新する。

         *

         * @param square

         *            計算対象のNyARSquareオブジェクト

         * @param i_direction

         *            マーカーの方向

         * @param i_width

         *            マーカーのサイズ(mm)

         * @param o_result_conv

         *            変換行列を受け取るオブジェクトを指定します。

         * @return

         * @throws NyARException

         */

        public double transMat(NyARSquare square, int i_direction, double i_width,

                        NyARTransMatResult o_result_conv) throws NyARException {

                double[][] ppos2d = wk_transMat_ppos2d;

                double[][] ppos3d = wk_transMat_ppos3d;

                double[] off = wk_transMat_off;

                double[][] pos3d = wk_transMat_pos3d;

                int dir;

                double err = -1;

                transrot.initRot(square, i_direction);

                dir = i_direction;

                ppos2d[0][0] = square.sqvertex[(4 - dir) % 4][0];

                ppos2d[0][1] = square.sqvertex[(4 - dir) % 4][1];

                ppos2d[1][0] = square.sqvertex[(5 - dir) % 4][0];

                ppos2d[1][1] = square.sqvertex[(5 - dir) % 4][1];

                ppos2d[2][0] = square.sqvertex[(6 - dir) % 4][0];

                ppos2d[2][1] = square.sqvertex[(6 - dir) % 4][1];

                ppos2d[3][0] = square.sqvertex[(7 - dir) % 4][0];

                ppos2d[3][1] = square.sqvertex[(7 - dir) % 4][1];

                ppos3d[0][0] = center[0] - i_width / 2.0;

                ppos3d[0][1] = center[1] + i_width / 2.0;

                ppos3d[1][0] = center[0] + i_width / 2.0;

                ppos3d[1][1] = center[1] + i_width / 2.0;

                ppos3d[2][0] = center[0] + i_width / 2.0;

                ppos3d[2][1] = center[1] - i_width / 2.0;

                ppos3d[3][0] = center[0] - i_width / 2.0;

                ppos3d[3][1] = center[1] - i_width / 2.0;

                // arGetTransMat3の前段処理(pos3dとoffを初期化)

                arGetTransMat3_initPos3d(ppos3d, pos3d, off);

                for (int i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) {

                        err = arGetTransMat3(ppos2d, pos3d, off, o_result_conv);

                        if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR) {

                                break;

                        }

                }

                return err;

        }

        /**

         * transMatContinue用のワーク

         */

        private final NyARTransMatResult wk_transMatContinue_result = new NyARTransMatResult();

        /**

         * double arGetTransMatCont( ARMarkerInfo *marker_info, double

         * prev_conv[3][4],double center[2], double width, double conv[3][4] )

         *

         * @param i_square

         * @param i_direction

         *            マーカーの方位を指定する。

         * @param i_width

         * @param io_result_conv

         *            計算履歴を持つNyARTransMatResultオブジェクトを指定する。

         *            履歴を持たない場合は、transMatと同じ処理を行う。

         * @return

         * @throws NyARException

         */

        public double transMatContinue(NyARSquare i_square, int i_direction,

                        double i_width, NyARTransMatResult io_result_conv)

                        throws NyARException {

                // io_result_convが初期値なら、transMatで計算する。

                if (!io_result_conv.hasValue()) {

                        return this

                                        .transMat(i_square, i_direction, i_width, io_result_conv);

                }

                double err1, err2;

                int i, dir;

                double[][] ppos2d = wk_transMat_ppos2d;

                double[][] ppos3d = wk_transMat_ppos3d;

                double[] off = wk_transMat_off;

                double[][] pos3d = wk_transMat_pos3d;

                // arGetTransMatContSub計算部分

                transrot.initRotByPrevResult(io_result_conv);

                dir = i_direction;

                ppos2d[0][0] = i_square.sqvertex[(4 - dir) % 4][0];

                ppos2d[0][1] = i_square.sqvertex[(4 - dir) % 4][1];

                ppos2d[1][0] = i_square.sqvertex[(5 - dir) % 4][0];

                ppos2d[1][1] = i_square.sqvertex[(5 - dir) % 4][1];

                ppos2d[2][0] = i_square.sqvertex[(6 - dir) % 4][0];

                ppos2d[2][1] = i_square.sqvertex[(6 - dir) % 4][1];

                ppos2d[3][0] = i_square.sqvertex[(7 - dir) % 4][0];

                ppos2d[3][1] = i_square.sqvertex[(7 - dir) % 4][1];

                ppos3d[0][0] = center[0] - i_width / 2.0;

                ppos3d[0][1] = center[1] + i_width / 2.0;

                ppos3d[1][0] = center[0] + i_width / 2.0;

                ppos3d[1][1] = center[1] + i_width / 2.0;

                ppos3d[2][0] = center[0] + i_width / 2.0;

                ppos3d[2][1] = center[1] - i_width / 2.0;

                ppos3d[3][0] = center[0] - i_width / 2.0;

                ppos3d[3][1] = center[1] - i_width / 2.0;

                // arGetTransMat3の前段処理(pos3dとoffを初期化)

                arGetTransMat3_initPos3d(ppos3d, pos3d, off);

                err1 = err2 = -1;

                for (i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) {

                        err1 = arGetTransMat3(ppos2d, pos3d, off, io_result_conv);

                        if (err1 < AR_GET_TRANS_MAT_MAX_FIT_ERROR) {

                                // 十分な精度を達成できたらブレーク

                                break;

                        }

                }

                // エラー値が許容範囲でなければTransMatをやり直し

                if (err1 > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) {

                        NyARTransMatResult result2 = this.wk_transMatContinue_result;

                        // transMatを実行

                        transrot.initRot(i_square, i_direction);

                        err2 = transMat(i_square, i_direction, i_width, result2);

                        // transmMatここまで

                        if (err2 < err1) {

                                io_result_conv.copyFrom(result2);

                                err1 = err2;

                        }

                }

                return err1;

        }

        private final double[] wk_arGetTransMat3_initPos3d_pmax = new double[3];

        private final double[] wk_arGetTransMat3_initPos3d_pmin = new double[3];

        /**

         * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。

         * 計算結果から再帰的に変更される可能性が無いので、切り離し。

         *

         * @param i_ppos3d

         *            入力配列[num][3]

         * @param o_pos3d

         *            出力配列[P_MAX][3]

         * @param o_off

         *            [3]

         * @throws NyARException

         */

        private final void arGetTransMat3_initPos3d(double i_ppos3d[][],

                        double[][] o_pos3d, double[] o_off) throws NyARException {

                double[] pmax = wk_arGetTransMat3_initPos3d_pmax;// new double[3];

                double[] pmin = wk_arGetTransMat3_initPos3d_pmin;// new

                // double[3];//double

                // off[3], pmax[3],

                // pmin[3];

                int i;

                pmax[0] = pmax[1] = pmax[2] = -10000000000.0;

                pmin[0] = pmin[1] = pmin[2] = 10000000000.0;

                for (i = 0; i < NUMBER_OF_VERTEX; i++) {

                        if (i_ppos3d[i][0] > pmax[0]) {

                                pmax[0] = i_ppos3d[i][0];

                        }

                        if (i_ppos3d[i][0] < pmin[0]) {

                                pmin[0] = i_ppos3d[i][0];

                        }

                        if (i_ppos3d[i][1] > pmax[1]) {

                                pmax[1] = i_ppos3d[i][1];

                        }

                        if (i_ppos3d[i][1] < pmin[1]) {

                                pmin[1] = i_ppos3d[i][1];

                        }

                        /*

                         * オリジナルでもコメントアウト if( ppos3d[i][2] > pmax[2] ) pmax[2] =

                         * ppos3d[i][2]; if( ppos3d[i][2] < pmin[2] ) pmin[2] =

                         * ppos3d[i][2];

                         */

                }

                o_off[0] = -(pmax[0] + pmin[0]) / 2.0;

                o_off[1] = -(pmax[1] + pmin[1]) / 2.0;

                o_off[2] = -(pmax[2] + pmin[2]) / 2.0;

                double[] o_pos3d_pt;

                double[] i_pos_pd_pt;

                for (i = 0; i < NUMBER_OF_VERTEX; i++) {

                        o_pos3d_pt = o_pos3d[i];

                        i_pos_pd_pt = i_ppos3d[i];

                        o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0];

                        o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1];

                        o_pos3d_pt[2] = 0.0;

                }

        }

        /**

         * double arGetTransMat3( double rot[3][3], double ppos2d[][2],double

         * ppos3d[][2], int num, double conv[3][4],double *dist_factor, double

         * cpara[3][4] ) STEP 414->

         *

         * @param ppos2d

         * @param i_pos3d

         * @param i_off

         * @param num

         * @return

         * @throws NyARException

         */

        private final double arGetTransMat3(double ppos2d[][],

                        final double i_pos3d[][], final double i_off[],

                        NyARTransMatResult o_result_conv) throws NyARException {

                double ret;

                ret = arGetTransMatSub(ppos2d, i_pos3d, i_off, o_result_conv);

                // double[][] conv=o_result_conv.getArray();

                // double[] rot=transrot.getArray();

                // for(int i=0;i<3;i++){

                // conv[i][0] = rot[i*3+0];

                // conv[i][1] = rot[i*3+1];

                // conv[i][2] = rot[i*3+2];

                // }

                // conv[0][3] = conv[0][0]*i_off[0] + conv[0][1]*i_off[1] +

                // conv[0][2]*i_off[2] + conv[0][3];

                // conv[1][3] = conv[1][0]*i_off[0] + conv[1][1]*i_off[1] +

                // conv[1][2]*i_off[2] + conv[1][3];

                // conv[2][3] = conv[2][0]*i_off[0] + conv[2][1]*i_off[1] +

                // conv[2][2]*i_off[2] + conv[2][3];

                return ret;

        }

        private final NyARMat wk_arGetTransMatSub_mat_a = new NyARMat(

                        NUMBER_OF_VERTEX * 2, 3);

        private final NyARMat wk_arGetTransMatSub_mat_b = new NyARMat(3,

                        NUMBER_OF_VERTEX * 2);

        private final NyARMat wk_arGetTransMatSub_mat_c = new NyARMat(

                        NUMBER_OF_VERTEX * 2, 1);

        private final NyARMat wk_arGetTransMatSub_mat_d = new NyARMat(3, 3);

        private final NyARMat wk_arGetTransMatSub_mat_e = new NyARMat(3, 1);

        private final NyARMat wk_arGetTransMatSub_mat_f = new NyARMat(3, 1);

        private final double[] wk_arGetTransMatSub_trans = new double[3];

        private final double[][] wk_arGetTransMatSub_pos2d = new double[P_MAX][2];// pos2d[P_MAX][2];

        private final DoubleValue wk_arGetTransMatSub_a1 = new DoubleValue();

        private final DoubleValue wk_arGetTransMatSub_a2 = new DoubleValue();

        /**

         * static double arGetTransMatSub( double rot[3][3], double

         * ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double

         * *dist_factor, double cpara[3][4] ) Optimize:2008.04.20:STEP[1033→1004]

         *

         * @param i_ppos2d

         * @param i_pos3d

         * @return

         * @throws NyARException

         */

        private final double arGetTransMatSub(double i_ppos2d[][],

                        double i_pos3d[][], double[] i_off, NyARTransMatResult o_result_conv)

                        throws NyARException {

                double[][] pos2d = wk_arGetTransMatSub_pos2d;

                double cpara[] = param.get34Array();

                NyARMat mat_a, mat_b, mat_c, mat_d, mat_e, mat_f;// ARMat *mat_a,

                // *mat_b, *mat_c,

                // *mat_d, *mat_e,

                // *mat_f;

                double wx, wy, wz;

                double ret;

                int i;

                double[] po2d_pt;

                mat_a = this.wk_arGetTransMatSub_mat_a;

                // mat_a.realloc(NUMBER_OF_VERTEX*2,3);

                double[][] a_array = mat_a.getArray();

                mat_b = this.wk_arGetTransMatSub_mat_b;

                // mat_b.realloc(3,NUMBER_OF_VERTEX*2);

                double[][] b_array = mat_b.getArray();

                DoubleValue a1 = wk_arGetTransMatSub_a1;

                DoubleValue a2 = wk_arGetTransMatSub_a2;

                if (arFittingMode == AR_FITTING_TO_INPUT) {

                        for (i = 0; i < NUMBER_OF_VERTEX; i++) {

                                param.ideal2Observ(i_ppos2d[i][0], i_ppos2d[i][1], a1, a2);// arParamIdeal2Observ(dist_factor,

                                // ppos2d[i][0],

                                // ppos2d[i][1],&pos2d[i][0],

                                // &pos2d[i][1]);

                                po2d_pt = pos2d[i];

                                po2d_pt[0] = a1.value;

                                po2d_pt[1] = a2.value;

                        }

                } else {

                        for (i = 0; i < NUMBER_OF_VERTEX; i++) {

                                po2d_pt = pos2d[i];

                                pos2d[i][0] = i_ppos2d[i][0];

                                pos2d[i][1] = i_ppos2d[i][1];

                        }

                }

                mat_c = this.wk_arGetTransMatSub_mat_c;// 次処理で値をもらうので、初期化の必要は無い。

                // mat_c.realloc(NUMBER_OF_VERTEX*2,1);

                double[][] c_array = mat_c.getArray();

                double[] rot = transrot.getArray();

                double[] i_pos3d_pt;

                int x2;

                for (i = 0; i < NUMBER_OF_VERTEX; i++) {

                        x2 = i * 2;

                        i_pos3d_pt = i_pos3d[i];

                        po2d_pt = pos2d[i];

                        wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2]

                                        * i_pos3d_pt[2];

                        wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5]

                                        * i_pos3d_pt[2];

                        wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8]

                                        * i_pos3d_pt[2];

                        // </Optimize>

                        a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0]

                        // =

                        // mat_b->m[num*0+j*2]

                        // =

                        // cpara[0][0];

                        a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1]

                        // =

                        // mat_b->m[num*2+j*2]

                        // =

                        // cpara[0][1];

                        a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - po2d_pt[0];// mat_a->m[j*6+2]

                        // =

                        // mat_b->m[num*4+j*2]

                        // =

                        // cpara[0][2]

                        // -

                        // pos2d[j][0];

                        a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] =

                        // mat_b->m[num*0+j*2+1]

                        // = 0.0;

                        a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4]

                        // =

                        // mat_b->m[num*2+j*2+1]

                        // =

                        // cpara[1][1];

                        a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2]

                                        - po2d_pt[1];// mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] =

                        // cpara[1][2] - pos2d[j][1];

                        c_array[x2][0] = wz * po2d_pt[0] - cpara[0 * 4 + 0] * wx

                                        - cpara[0 * 4 + 1] * wy - cpara[0 * 4 + 2] * wz;// mat_c->m[j*2+0]

                        // = wz *

                        // pos2d[j][0]-

                        // cpara[0][0]*wx

                        // -

                        // cpara[0][1]*wy

                        // -

                        // cpara[0][2]*wz;

                        c_array[x2 + 1][0] = wz * po2d_pt[1] - cpara[1 * 4 + 1] * wy

                                        - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] = wz *

                        // pos2d[j][1]- cpara[1][1]*wy -

                        // cpara[1][2]*wz;

                }

                mat_d = this.wk_arGetTransMatSub_mat_d;// 次処理で値をもらうので、初期化の必要は無い。

                mat_e = this.wk_arGetTransMatSub_mat_e;// 次処理で値をもらうので、初期化の必要は無い。

                mat_f = this.wk_arGetTransMatSub_mat_f;// 次処理で値をもらうので、初期化の必要は無い。

                double[][] f_array = mat_f.getArray();

                mat_d.matrixMul(mat_b, mat_a);

                mat_e.matrixMul(mat_b, mat_c);

                mat_d.matrixSelfInv();

                mat_f.matrixMul(mat_d, mat_e);

                double[] trans = wk_arGetTransMatSub_trans;// double trans[3];

                trans[0] = f_array[0][0];// trans[0] = mat_f->m[0];

                trans[1] = f_array[1][0];

                trans[2] = f_array[2][0];// trans[2] = mat_f->m[2];

                ret = transrot.modifyMatrix(trans, i_pos3d, pos2d);

                for (i = 0; i < NUMBER_OF_VERTEX; i++) {

                        x2 = i * 2;

                        i_pos3d_pt = i_pos3d[i];

                        po2d_pt = pos2d[i];

                        // <Optimize>

                        // wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2]

                        // * pos3d[j][2];

                        // wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2]

                        // * pos3d[j][2];

                        // wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2]

                        // * pos3d[j][2];

                        wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2]

                                        * i_pos3d_pt[2];

                        wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5]

                                        * i_pos3d_pt[2];

                        wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8]

                                        * i_pos3d_pt[2];

                        // </Optimize>

                        a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0]

                        // =

                        // mat_b->m[num*0+j*2]

                        // =

                        // cpara[0][0];

                        a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1]

                        // =

                        // mat_b->m[num*2+j*2]

                        // =

                        // cpara[0][1];

                        a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - po2d_pt[0];// mat_a->m[j*6+2]

                        // =

                        // mat_b->m[num*4+j*2]

                        // =

                        // cpara[0][2]

                        // -

                        // pos2d[j][0];

                        a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] =

                        // mat_b->m[num*0+j*2+1]

                        // = 0.0;

                        a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4]

                        // =

                        // mat_b->m[num*2+j*2+1]

                        // =

                        // cpara[1][1];

                        a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2]

                                        - po2d_pt[1];// mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] =

                        // cpara[1][2] - pos2d[j][1];

                        c_array[x2][0] = wz * po2d_pt[0] - cpara[0 * 4 + 0] * wx

                                        - cpara[0 * 4 + 1] * wy - cpara[0 * 4 + 2] * wz;// mat_c->m[j*2+0]

                        // = wz *

                        // pos2d[j][0]-

                        // cpara[0][0]*wx

                        // -

                        // cpara[0][1]*wy

                        // -

                        // cpara[0][2]*wz;

                        c_array[x2 + 1][0] = wz * po2d_pt[1] - cpara[1 * 4 + 1] * wy

                                        - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] = wz *

                        // pos2d[j][1]- cpara[1][1]*wy -

                        // cpara[1][2]*wz;

                }

                mat_d.matrixMul(mat_b, mat_a);

                mat_e.matrixMul(mat_b, mat_c);

                mat_d.matrixSelfInv();

                mat_f.matrixMul(mat_d, mat_e);

                trans[0] = f_array[0][0];// trans[0] = mat_f->m[0];

                trans[1] = f_array[1][0];

                trans[2] = f_array[2][0];// trans[2] = mat_f->m[2];

                ret = transrot.modifyMatrix(trans, i_pos3d, pos2d);

                // 変換行列を保存

                o_result_conv.updateMatrixValue(this.transrot, i_off, trans);

                // double[][] conv=o_result_conv.getArray();

                // for( i = 2; i >=0; i-- ) {//<Optimize/>for( j = 0; j < 3; j++ ) {

                // //<Optimize>

                // //for( i = 0; i < 3; i++ ){

                // // conv[j][i] = rot[j][i];

                // //}

                // conv[i][0] = rot[i*3+0];

                // conv[i][1] = rot[i*3+1];

                // conv[i][2] = rot[i*3+2];

                // //</Optimize>

                // conv[i][3] = trans[i];

                // }

                return ret;

        }

}