Exemplo n.º 1
0
        private static NyARMat makeMatAtB(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtB)
        {
            double v0, v1, v2, v3, v4, v5, v6, v7;
            v0 = v1 = v2 = v3 = v4 = v5 = v6 = v7 = 0;
            for (int i = 0; i < i_num; i++)
            {
                double wx = worldCoord[i].x;
                double wy = worldCoord[i].y;
                double sx = screenCoord[i].x;
                double sy = screenCoord[i].y;
                v0 += wx * sx;
                v1 += wy * sx;
                v2 += sx;
                v3 += wx * sy;
                v4 += wy * sy;
                v5 += sy;
                v6 += -wx * sx * sx - wx * sy * sy;
                v7 += -wy * sx * sx - wy * sy * sy;
            }
            double[][] t = o_matAtB.getArray();
            t[0][0] = v0;
            t[1][0] = v1;
            t[2][0] = v2;
            t[3][0] = v3;
            t[4][0] = v4;
            t[5][0] = v5;
            t[6][0] = v6;
            t[7][0] = v7;
            return o_matAtB;

        }
Exemplo n.º 2
0
 private static NyARMat makeMatAtA(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtA)
 {
     o_matAtA.loadZero();
     double[][] t = o_matAtA.getArray();
     for (int i = 0; i < i_num; i++)
     {
         //0
         double wx = worldCoord[i].x;
         double wy = worldCoord[i].y;
         double sx = screenCoord[i].x;
         double sy = screenCoord[i].y;
         double wxwx = wx * wx;
         double wywy = wy * wy;
         double wxwy = wx * wy;
         t[0][0] += wxwx;
         t[0][1] += wxwy;
         t[0][2] += wx;
         t[1][2] += wy;
         t[0][6] += (-wxwx) * (sx);
         t[0][7] += (-wxwy) * (sx);
         t[1][1] += wywy;
         t[1][7] += (-wywy) * (sx);
         t[2][6] += (-wx) * (sx);
         t[2][7] += (-wy) * (sx);
         t[3][6] += (-wxwx) * (sy);
         t[3][7] += (-wxwy) * (sy);
         t[4][7] += (-wywy) * (sy);
         t[5][6] += (-wx) * (sy);
         t[5][7] += (-wy) * (sy);
         t[6][6] += (wxwx) * (sx) * (sx) + (wxwx) * (sy) * (sy);
         t[6][7] += (wxwy) * (sx) * (sx) + (wxwy) * (sy) * (sy);
         t[7][7] += (wywy) * (sx) * (sx) + (wywy) * (sy) * (sy);
     }
     t[1][0] = t[3][4] = t[4][3] = t[0][1];
     t[2][0] = t[3][5] = t[5][3] = t[0][2];
     t[2][1] = t[5][4] = t[4][5] = t[1][2];
     t[7][0] = t[6][1] = t[1][6] = t[0][7];
     t[4][6] = t[6][4] = t[7][3] = t[3][7];
     t[2][2] = t[5][5] = i_num;
     t[3][3] = t[0][0];
     t[4][4] = t[1][1];
     t[6][0] = t[0][6];
     t[6][2] = t[2][6];
     t[6][3] = t[3][6];
     t[6][5] = t[5][6];
     t[7][1] = t[1][7];
     t[7][2] = t[2][7];
     t[7][4] = t[4][7];
     t[7][5] = t[5][7];
     t[7][6] = t[6][7];
     //先頭でゼロクリアしない場合。
     //t[0][3]=t[0][4]=t[0][5]=t[1][3]=t[1][4]=t[1][5]=t[2][3]=t[2][4]=t[2][5]=t[3][0]=t[3][1]=t[3][2]=t[4][0]=t[4][1]=t[4][2]=t[5][0]=t[5][1]=t[5][2]=0;
     return o_matAtA;
 }
Exemplo n.º 3
0
        /**
         * 現在の行列に主成分分析を実行して、結果をthisと引数へ格納します。
         * 詳細は不明です。
         * @param o_evec
         * 不明
         * @param o_ev
         * 不明
         * @param o_mean
         * 不明
         * @
         */
        public void pca(NyARMat o_evec, NyARVec o_ev, NyARVec o_mean)
        {
            double l_row = this.row;// row = input->row;
            double l_clm = this.clm;// clm = input->clm;
            double check = (l_row < l_clm) ? l_row : l_clm;

            Debug.Assert(l_row >= 2 || l_clm >= 2);
            Debug.Assert(o_evec.clm == l_clm && o_evec.row == check);
            Debug.Assert(o_ev.getClm() == check);
            Debug.Assert(o_mean.getClm() == l_clm);

            double srow = Math.Sqrt((double)l_row);
            PCA_EX(o_mean);

            PCA_CENTER(this, o_mean);

            int i, j;
            // For順変更OK
            for (i = 0; i < l_row; i++)
            {
                for (j = 0; j < l_clm; j++)
                {
                    this._m[i][j] /= srow;// work->m[i] /= srow;
                }
            }

            PCA_PCA(o_evec, o_ev);

            double sum = 0.0;
            double[] ev_array = o_ev.getArray();
            int ev_clm = o_ev.getClm();
            // For順変更禁止
            for (i = 0; i < ev_clm; i++)
            {// for(int i = 0; i < ev->clm; i++ ){
                sum += ev_array[i];// sum += ev->v[i];
            }
            // For順変更禁止
            for (i = 0; i < ev_clm; i++)
            {// for(int i = 0; i < ev->clm; i++ ){
                ev_array[i] /= sum;// ev->v[i] /= sum;
            }
            return;
        }
Exemplo n.º 4
0
        public NyARRotVector(NyARPerspectiveProjectionMatrix i_cmat)
        {
            NyARMat mat_a = new NyARMat(3, 3);
            double[][] a_array = mat_a.getArray();

            a_array[0][0] = i_cmat.m00;
            a_array[0][1] = i_cmat.m01;
            a_array[0][2] = i_cmat.m02;
            a_array[1][0] = i_cmat.m10;
            a_array[1][1] = i_cmat.m11;
            a_array[1][2] = i_cmat.m12;
            a_array[2][0] = i_cmat.m20;
            a_array[2][1] = i_cmat.m21;
            a_array[2][2] = i_cmat.m22;

            mat_a.matrixSelfInv();
            this._projection_mat_ref = i_cmat;
            this._inv_cpara_array_ref = mat_a.getArray();
            //GCない言語のときは、ここで配列の所有権委譲してね!
        }
        /**
         * この関数は、射影変換後の2次元頂点座標をセットします。
         * i_number_of_vertexは4である必要があります。
         */
        public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d, int i_number_of_vertex)
        {
            Debug.Assert(i_number_of_vertex == 4);
            double[] cx      = this._cx;
            double[] cy      = this._cy;
            double   cpara02 = this._projection_mat.m02;
            double   cpara12 = this._projection_mat.m12;
            NyARMat  mat_t   = this._mat_t;

            double[][] mata  = this._mat_a.getArray();
            double[][] matat = this._mat_at.getArray();
            for (int i = 0; i < 4; i++)
            {
                cx[i] = i_ref_vertex_2d[i].x;
                cy[i] = i_ref_vertex_2d[i].y;
                int x2 = i * 2;
                mata[x2][2]     = matat[2][x2] = cpara02 - i_ref_vertex_2d[i].x;     // mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0];
                mata[x2 + 1][2] = matat[2][x2 + 1] = cpara12 - i_ref_vertex_2d[i].y; // mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];
            }
            //T(3x3行列)の作成
            mat_t.mul(this._mat_at, this._mat_a);
            mat_t.inverse();
            return;
        }
Exemplo n.º 6
0
        /**
         * ARToolKitのCENTER関数と同等な関数です。
         * 詳細は不明です。
         * @param inout
         * 不明
         * @param mean
         * 不明
         */
        private static void PCA_CENTER(NyARMat inout, NyARVec mean)
        {
            double[] v;
            int row, clm;

            row = inout.row;
            clm = inout.clm;
            if (mean.getClm() != clm)
            {
                throw new NyARException();
            }
            double[][] im = inout._m;
            double[] im_i;
            double w0, w1;
            v = mean.getArray();
            // 特にパフォーマンスが劣化するclm=1と2ときだけ、別パスで処理します。
            switch (clm)
            {
                case 1:
                    w0 = v[0];
                    for (int i = 0; i < row; i++)
                    {
                        im[i][0] -= w0;
                    }
                    break;
                case 2:
                    w0 = v[0];
                    w1 = v[1];
                    for (int i = 0; i < row; i++)
                    {
                        im_i = im[i];
                        im_i[0] -= w0;
                        im_i[1] -= w1;
                    }
                    break;
                default:
                    for (int i = 0; i < row; i++)
                    {
                        im_i = im[i];
                        for (int j = 0; j < clm; j++)
                        {
                            // *(m++) -= *(v++);
                            im_i[j] -= v[j];
                        }
                    }
                    break;
            }
            return;
        }
Exemplo n.º 7
0
        /**
         * ARToolKitのPCA関数と同等な関数です。
         * 詳細は不明です。
         * @param output
         * 不明
         * @param o_ev
         * 不明
         * @
         */
        private void PCA_PCA(NyARMat o_output, NyARVec o_ev)
        {

            int l_row, l_clm, min;
            double[] ev_array = o_ev.getArray();

            l_row = this.row;// row = input->row;
            l_clm = this.clm;// clm = input->clm;
            min = (l_clm < l_row) ? l_clm : l_row;
            if (l_row < 2 || l_clm < 2)
            {
                throw new NyARException();
            }
            if (o_output.clm != this.clm)
            {// if( output->clm != input->clm ){
                throw new NyARException();
            }
            if (o_output.row != min)
            {// if( output->row != min ){
                throw new NyARException();
            }
            if (o_ev.getClm() != min)
            {// if( ev->clm != min ){
                throw new NyARException();
            }

            NyARMatPca u;// u =new NyARMat( min, min );
            if (this.wk_PCA_PCA_u == null)
            {
                u = new NyARMatPca(min, min);
                this.wk_PCA_PCA_u = u;
            }
            else
            {
                u = this.wk_PCA_PCA_u;
                u.realloc(min, min);
            }

            if (l_row < l_clm)
            {
                NyARException.trap("未チェックのパス");
                PCA_x_by_xt(this, u);// if(x_by_xt( input, u ) < 0 ) {
            }
            else
            {
                PCA_xt_by_x(this, u);// if(xt_by_x( input, u ) < 0 ) {
            }
            u.PCA_QRM(o_ev);

            double[][] m1, m2;
            if (l_row < l_clm)
            {
                NyARException.trap("未チェックのパス");
                PCA_EV_create(this, u, o_output, o_ev);
            }
            else
            {
                m1 = u._m;// m1 = u->m;
                m2 = o_output._m;// m2 = output->m;
                int i;
                for (i = 0; i < min; i++)
                {
                    if (ev_array[i] < PCA_VZERO)
                    {// if( ev->v[i] < VZERO ){
                        break;
                    }
                    for (int j = 0; j < min; j++)
                    {
                        m2[i][j] = m1[i][j];// *(m2++) = *(m1++);
                    }
                }
                for (; i < min; i++)
                {// for( ; i < min; i++){
                    // コードを見た限りあってそうだからコメントアウト(2008/03/26)NyARException.trap("未チェックのパス");
                    ev_array[i] = 0.0;// ev->v[i] = 0.0;
                    for (int j = 0; j < min; j++)
                    {
                        m2[i][j] = 0.0;// *(m2++) = 0.0;
                    }
                }
            }
        }
        /**
         * 右手系の視錐台を作ります。
         * この視錐台は、ARToolKitのarglCameraViewRHの作る視錐台と同じです。
         * @param i_screen_width
         * スクリーンサイズを指定します。
         * @param i_screen_height
         * スクリーンサイズを指定します。
         * @param i_dist_min
         * near pointを指定します(mm単位)
         * @param i_dist_max
         * far pointを指定します(mm単位)
         * @param o_frustum
         * 視錐台の格納先オブジェクトを指定します。
         */
        public void makeCameraFrustumRH(double i_screen_width, double i_screen_height, double i_dist_min, double i_dist_max, NyARDoubleMatrix44 o_frustum)
        {
            NyARMat trans_mat = new NyARMat(3, 4);
            NyARMat icpara_mat = new NyARMat(3, 4);
            double[][] p = ArrayUtils.newDouble2dArray(3, 3);
            int i;

            this.decompMat(icpara_mat, trans_mat);

            double[][] icpara = icpara_mat.getArray();
            double[][] trans = trans_mat.getArray();
            for (i = 0; i < 4; i++)
            {
                icpara[1][i] = (i_screen_height - 1) * (icpara[2][i]) - icpara[1][i];
            }
            p[0][0] = icpara[0][0] / icpara[2][2];
            p[0][1] = icpara[0][1] / icpara[2][2];
            p[0][2] = icpara[0][2] / icpara[2][2];

            p[1][0] = icpara[1][0] / icpara[2][2];
            p[1][1] = icpara[1][1] / icpara[2][2];
            p[1][2] = icpara[1][2] / icpara[2][2];

            p[2][0] = icpara[2][0] / icpara[2][2];
            p[2][1] = icpara[2][1] / icpara[2][2];
            p[2][2] = icpara[2][2] / icpara[2][2];

            double q00, q01, q02, q03, q10, q11, q12, q13, q20, q21, q22, q23, q30, q31, q32, q33;

            //視錐台への変換
            q00 = (2.0 * p[0][0] / (i_screen_width - 1));
            q01 = (2.0 * p[0][1] / (i_screen_width - 1));
            q02 = -((2.0 * p[0][2] / (i_screen_width - 1)) - 1.0);
            q03 = 0.0;
            o_frustum.m00 = q00 * trans[0][0] + q01 * trans[1][0] + q02 * trans[2][0];
            o_frustum.m01 = q00 * trans[0][1] + q01 * trans[1][1] + q02 * trans[2][1];
            o_frustum.m02 = q00 * trans[0][2] + q01 * trans[1][2] + q02 * trans[2][2];
            o_frustum.m03 = q00 * trans[0][3] + q01 * trans[1][3] + q02 * trans[2][3] + q03;

            q10 = 0.0;
            q11 = -(2.0 * p[1][1] / (i_screen_height - 1));
            q12 = -((2.0 * p[1][2] / (i_screen_height - 1)) - 1.0);
            q13 = 0.0;
            o_frustum.m10 = q10 * trans[0][0] + q11 * trans[1][0] + q12 * trans[2][0];
            o_frustum.m11 = q10 * trans[0][1] + q11 * trans[1][1] + q12 * trans[2][1];
            o_frustum.m12 = q10 * trans[0][2] + q11 * trans[1][2] + q12 * trans[2][2];
            o_frustum.m13 = q10 * trans[0][3] + q11 * trans[1][3] + q12 * trans[2][3] + q13;

            q20 = 0.0;
            q21 = 0.0;
            q22 = (i_dist_max + i_dist_min) / (i_dist_min - i_dist_max);
            q23 = 2.0 * i_dist_max * i_dist_min / (i_dist_min - i_dist_max);
            o_frustum.m20 = q20 * trans[0][0] + q21 * trans[1][0] + q22 * trans[2][0];
            o_frustum.m21 = q20 * trans[0][1] + q21 * trans[1][1] + q22 * trans[2][1];
            o_frustum.m22 = q20 * trans[0][2] + q21 * trans[1][2] + q22 * trans[2][2];
            o_frustum.m23 = q20 * trans[0][3] + q21 * trans[1][3] + q22 * trans[2][3] + q23;

            q30 = 0.0;
            q31 = 0.0;
            q32 = -1.0;
            q33 = 0.0;
            o_frustum.m30 = q30 * trans[0][0] + q31 * trans[1][0] + q32 * trans[2][0];
            o_frustum.m31 = q30 * trans[0][1] + q31 * trans[1][1] + q32 * trans[2][1];
            o_frustum.m32 = q30 * trans[0][2] + q31 * trans[1][2] + q32 * trans[2][2];
            o_frustum.m33 = q30 * trans[0][3] + q31 * trans[1][3] + q32 * trans[2][3] + q33;
            return;
        }
Exemplo n.º 9
0
 /** @deprecated */
 public void copyFrom(NyARMat i_copy_from) { this.setValue(i_copy_from); }
Exemplo n.º 10
0
 /**
  * i_srcの転置行列をdestに得ます。
  * この関数は未チェックの為、実行すると例外が発生します。
  * @param dest
  * 出力先のオブジェクト
  * @param source
  * 入力元のオブジェクト
  */
 public void transpose(NyARMat i_src)
 {
     if (this.row != i_src.clm || this.clm != i_src.row)
     {
         throw new NyARException();
     }
     for (int r = 0; r < this.row; r++)
     {
         for (int c = 0; c < this.clm; c++)
         {
             this._m[r][c] = i_src._m[c][r];
         }
     }
 }
Exemplo n.º 11
0
        /**
         * 行列同士の掛け算を実行します。
         * i_mat_aとi_mat_bの積を計算して、thisへ格納します。
         * @param i_mat_a
         * 計算元の行列A
         * @param i_mat_b
         * 計算元の行列B
         * @
         */
        public void mul(NyARMat i_mat_a, NyARMat i_mat_b)
        {
            Debug.Assert(i_mat_a.clm == i_mat_b.row && this.row == i_mat_a.row && this.clm == i_mat_b.clm);

            double w;
            int r, c, i;
            double[][] am = i_mat_a._m, bm = i_mat_b._m, dm = this._m;
            // For順変更禁止
            for (r = 0; r < this.row; r++)
            {
                for (c = 0; c < this.clm; c++)
                {
                    w = 0.0;
                    for (i = 0; i < i_mat_a.clm; i++)
                    {
                        w += am[r][i] * bm[i][c];
                    }
                    dm[r][c] = w;
                }
            }
            return;
        }
Exemplo n.º 12
0
        /**
         * ARToolKitのPCA関数と同等な関数です。
         * 詳細は不明です。
         * @param output
         * 不明
         * @param o_ev
         * 不明
         * @
         */
        private void PCA_PCA(NyARMat o_output, NyARVec o_ev)
        {
            int l_row, l_clm, min;

            double[] ev_array = o_ev.getArray();

            l_row = this.row; // row = input->row;
            l_clm = this.clm; // clm = input->clm;
            min   = (l_clm < l_row) ? l_clm : l_row;
            if (l_row < 2 || l_clm < 2)
            {
                throw new NyARRuntimeException();
            }
            if (o_output.clm != this.clm)
            {// if( output->clm != input->clm ){
                throw new NyARRuntimeException();
            }
            if (o_output.row != min)
            {// if( output->row != min ){
                throw new NyARRuntimeException();
            }
            if (o_ev.getClm() != min)
            {// if( ev->clm != min ){
                throw new NyARRuntimeException();
            }

            NyARMatPca u;// u =new NyARMat( min, min );

            if (this.wk_PCA_PCA_u == null)
            {
                u = new NyARMatPca(min, min);
                this.wk_PCA_PCA_u = u;
            }
            else
            {
                u = this.wk_PCA_PCA_u;
                u.realloc(min, min);
            }

            if (l_row < l_clm)
            {
                NyARRuntimeException.trap("未チェックのパス");
                PCA_x_by_xt(this, u);// if(x_by_xt( input, u ) < 0 ) {
            }
            else
            {
                PCA_xt_by_x(this, u);// if(xt_by_x( input, u ) < 0 ) {
            }
            u.PCA_QRM(o_ev);

            double[][] m1, m2;
            if (l_row < l_clm)
            {
                NyARRuntimeException.trap("未チェックのパス");
                PCA_EV_create(this, u, o_output, o_ev);
            }
            else
            {
                m1 = u._m;        // m1 = u->m;
                m2 = o_output._m; // m2 = output->m;
                int i;
                for (i = 0; i < min; i++)
                {
                    if (ev_array[i] < PCA_VZERO)
                    {// if( ev->v[i] < VZERO ){
                        break;
                    }
                    for (int j = 0; j < min; j++)
                    {
                        m2[i][j] = m1[i][j];// *(m2++) = *(m1++);
                    }
                }
                for (; i < min; i++)
                {                      // for( ; i < min; i++){
                    // コードを見た限りあってそうだからコメントアウト(2008/03/26)NyARException.trap("未チェックのパス");
                    ev_array[i] = 0.0; // ev->v[i] = 0.0;
                    for (int j = 0; j < min; j++)
                    {
                        m2[i][j] = 0.0;// *(m2++) = 0.0;
                    }
                }
            }
        }
 private static NyARMat makeMatAtA(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtA)
 {
     o_matAtA.loadZero();
     double[][] t = o_matAtA.getArray();
     for (int i = 0; i < i_num; i++)
     {
         //0
         double wx   = worldCoord[i].x;
         double wy   = worldCoord[i].y;
         double sx   = screenCoord[i].x;
         double sy   = screenCoord[i].y;
         double wxwx = wx * wx;
         double wywy = wy * wy;
         double wxwy = wx * wy;
         t[0][0] += wxwx;
         t[0][1] += wxwy;
         t[0][2] += wx;
         t[1][2] += wy;
         t[0][6] += (-wxwx) * (sx);
         t[0][7] += (-wxwy) * (sx);
         t[1][1] += wywy;
         t[1][7] += (-wywy) * (sx);
         t[2][6] += (-wx) * (sx);
         t[2][7] += (-wy) * (sx);
         t[3][6] += (-wxwx) * (sy);
         t[3][7] += (-wxwy) * (sy);
         t[4][7] += (-wywy) * (sy);
         t[5][6] += (-wx) * (sy);
         t[5][7] += (-wy) * (sy);
         t[6][6] += (wxwx) * (sx) * (sx) + (wxwx) * (sy) * (sy);
         t[6][7] += (wxwy) * (sx) * (sx) + (wxwy) * (sy) * (sy);
         t[7][7] += (wywy) * (sx) * (sx) + (wywy) * (sy) * (sy);
     }
     t[1][0] = t[3][4] = t[4][3] = t[0][1];
     t[2][0] = t[3][5] = t[5][3] = t[0][2];
     t[2][1] = t[5][4] = t[4][5] = t[1][2];
     t[7][0] = t[6][1] = t[1][6] = t[0][7];
     t[4][6] = t[6][4] = t[7][3] = t[3][7];
     t[2][2] = t[5][5] = i_num;
     t[3][3] = t[0][0];
     t[4][4] = t[1][1];
     t[6][0] = t[0][6];
     t[6][2] = t[2][6];
     t[6][3] = t[3][6];
     t[6][5] = t[5][6];
     t[7][1] = t[1][7];
     t[7][2] = t[2][7];
     t[7][4] = t[4][7];
     t[7][5] = t[5][7];
     t[7][6] = t[6][7];
     //先頭でゼロクリアしない場合。
     //t[0][3]=t[0][4]=t[0][5]=t[1][3]=t[1][4]=t[1][5]=t[2][3]=t[2][4]=t[2][5]=t[3][0]=t[3][1]=t[3][2]=t[4][0]=t[4][1]=t[4][2]=t[5][0]=t[5][1]=t[5][2]=0;
     return(o_matAtA);
 }
        public bool icpGetInitXw2Xc_from_PlanarData(
            NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord,
            int i_num, NyARDoubleMatrix44 initMatXw2Xc)
        {
            if (i_num < 4)
            {
                throw new NyARException();
            }
            // nを元に配列の準備

            NyARMat matAtA = this.__matAtA;
            NyARMat matAtB = this.__matAtB;
            NyARMat matC   = this.__matC;

            makeMatAtA(screenCoord, worldCoord, i_num, matAtA);
            makeMatAtB(screenCoord, worldCoord, i_num, matAtB);

            if (!matAtA.inverse())
            {
                return(false);
            }

            matC.mul(matAtA, matAtB);
            double[][] bufc = matC.getArray();
            double     t0, t1, t2;

            NyARRotVector      vec0  = this.__vec0;
            NyARRotVector      vec1  = this.__vec1;
            NyARDoubleMatrix44 matxc = this._cparam;

            vec0.v3 = (bufc[6][0]);
            vec0.v2 = (bufc[3][0] - matxc.m12 * vec0.v3) / matxc.m11;
            vec0.v1 = (bufc[0][0] - matxc.m02 * vec0.v3 - matxc.m01 * vec0.v2) / matxc.m00;
            vec1.v3 = (bufc[7][0]);
            vec1.v2 = (bufc[4][0] - matxc.m12 * vec1.v3) / matxc.m11;
            vec1.v1 = (bufc[1][0] - matxc.m02 * vec1.v3 - matxc.m01 * vec1.v2) / matxc.m00;
            t2      = 1.0;
            t1      = (bufc[5][0] - matxc.m12 * t2) / matxc.m11;
            t0      = (bufc[2][0] - matxc.m02 * t2 - matxc.m01 * t1) / matxc.m00;

            double l1 = Math.Sqrt(vec0.v1 * vec0.v1 + vec0.v2 * vec0.v2 + vec0.v3 * vec0.v3);
            double l2 = Math.Sqrt(vec1.v1 * vec1.v1 + vec1.v2 * vec1.v2 + vec1.v3 * vec1.v3);

            vec0.v1 /= l1;
            vec0.v2 /= l1;
            vec0.v3 /= l1;
            vec1.v1 /= l2;
            vec1.v2 /= l2;
            vec1.v3 /= l2;
            t0      /= (l1 + l2) / 2.0;
            t1      /= (l1 + l2) / 2.0;
            t2      /= (l1 + l2) / 2.0;
            if (t2 < 0.0)
            {
                vec0.v1 = -vec0.v1;
                vec0.v2 = -vec0.v2;
                vec0.v3 = -vec0.v3;
                vec1.v1 = -vec1.v1;
                vec1.v2 = -vec1.v2;
                vec1.v3 = -vec1.v3;
                t0      = -t0;
                t1      = -t1;
                t1      = -t2;
            }
            // ここまで

            if (!NyARRotVector.checkRotation(vec0, vec1))
            {
                return(false);
            }
            double v20 = vec0.v2 * vec1.v3 - vec0.v3 * vec1.v2;
            double v21 = vec0.v3 * vec1.v1 - vec0.v1 * vec1.v3;
            double v22 = vec0.v1 * vec1.v2 - vec0.v2 * vec1.v1;

            l1   = Math.Sqrt(v20 * v20 + v21 * v21 + v22 * v22);
            v20 /= l1;
            v21 /= l1;
            v22 /= l1;

            initMatXw2Xc.m00 = vec0.v1;
            initMatXw2Xc.m10 = vec0.v2;
            initMatXw2Xc.m20 = vec0.v3;
            initMatXw2Xc.m01 = vec1.v1;
            initMatXw2Xc.m11 = vec1.v2;
            initMatXw2Xc.m21 = vec1.v3;
            initMatXw2Xc.m02 = v20;
            initMatXw2Xc.m12 = v21;
            initMatXw2Xc.m22 = v22;
            initMatXw2Xc.m03 = t0;
            initMatXw2Xc.m13 = t1;
            initMatXw2Xc.m23 = t2;
            initMatXw2Xc.m30 = initMatXw2Xc.m31 = initMatXw2Xc.m32 = 0;
            initMatXw2Xc.m33 = 1;

            icpGetInitXw2XcSub(initMatXw2Xc, screenCoord, worldCoord, i_num, initMatXw2Xc);
            return(true);
        }
Exemplo n.º 15
0
        /**
         * この関数は、ラスタのi_vertexsで定義される四角形からパターンを取得して、インスタンスに格納します。
         */
        public virtual bool pickFromRaster(INyARRgbRaster image, NyARIntPoint2d[] i_vertexs)
        {
            // パターンの切り出しに失敗することもある。
            NyARMat cpara = new NyARMat(8, 1);

            if (!get_cpara(i_vertexs, cpara))
            {
                return(false);
            }
            double[][] para   = cpara.getArray();
            double     para00 = para[0 * 3 + 0][0];
            double     para01 = para[0 * 3 + 1][0];
            double     para02 = para[0 * 3 + 2][0];
            double     para10 = para[1 * 3 + 0][0];
            double     para11 = para[1 * 3 + 1][0];
            double     para12 = para[1 * 3 + 2][0];
            double     para20 = para[2 * 3 + 0][0];
            double     para21 = para[2 * 3 + 1][0];
            double     para22 = 1.0;

            int lx1 = (int)((i_vertexs[0].x - i_vertexs[1].x) * (i_vertexs[0].x - i_vertexs[1].x) + (i_vertexs[0].y - i_vertexs[1].y) * (i_vertexs[0].y - i_vertexs[1].y));
            int lx2 = (int)((i_vertexs[2].x - i_vertexs[3].x) * (i_vertexs[2].x - i_vertexs[3].x) + (i_vertexs[2].y - i_vertexs[3].y) * (i_vertexs[2].y - i_vertexs[3].y));
            int ly1 = (int)((i_vertexs[1].x - i_vertexs[2].x) * (i_vertexs[1].x - i_vertexs[2].x) + (i_vertexs[1].y - i_vertexs[2].y) * (i_vertexs[1].y - i_vertexs[2].y));
            int ly2 = (int)((i_vertexs[3].x - i_vertexs[0].x) * (i_vertexs[3].x - i_vertexs[0].x) + (i_vertexs[3].y - i_vertexs[0].y) * (i_vertexs[3].y - i_vertexs[0].y));

            if (lx2 > lx1)
            {
                lx1 = lx2;
            }
            if (ly2 > ly1)
            {
                ly1 = ly2;
            }

            int sample_pixel_x = this._size.w;
            int sample_pixel_y = this._size.h;

            while (sample_pixel_x * sample_pixel_x < lx1 / 4)
            {
                sample_pixel_x *= 2;
            }
            while (sample_pixel_y * sample_pixel_y < ly1 / 4)
            {
                sample_pixel_y *= 2;
            }

            if (sample_pixel_x > AR_PATT_SAMPLE_NUM)
            {
                sample_pixel_x = AR_PATT_SAMPLE_NUM;
            }
            if (sample_pixel_y > AR_PATT_SAMPLE_NUM)
            {
                sample_pixel_y = AR_PATT_SAMPLE_NUM;
            }

            int xdiv = sample_pixel_x / this._size.w; // xdiv = xdiv2/Config.AR_PATT_SIZE_X;
            int ydiv = sample_pixel_y / this._size.h; // ydiv = ydiv2/Config.AR_PATT_SIZE_Y;


            int img_x = image.getWidth();
            int img_y = image.getHeight();

            double xdiv2_reciprocal = 1.0 / sample_pixel_x;
            double ydiv2_reciprocal = 1.0 / sample_pixel_y;
            int    r, g, b;

            int[] rgb_tmp = new int[3];

            //ピクセルリーダーを取得
            INyARRgbPixelDriver reader = image.getRgbPixelDriver();
            int xdiv_x_ydiv            = xdiv * ydiv;

            for (int iy = 0; iy < this._size.h; iy++)
            {
                for (int ix = 0; ix < this._size.w; ix++)
                {
                    r = g = b = 0;
                    //1ピクセルを作成
                    for (int j = 0; j < ydiv; j++)
                    {
                        double yw = 102.5 + 5.0 * (iy * ydiv + j + 0.5) * ydiv2_reciprocal;
                        for (int i = 0; i < xdiv; i++)
                        {
                            double xw = 102.5 + 5.0 * (ix * xdiv + i + 0.5) * xdiv2_reciprocal;
                            double d  = para20 * xw + para21 * yw + para22;
                            if (d == 0)
                            {
                                throw new NyARException();
                            }
                            int xc = (int)((para00 * xw + para01 * yw + para02) / d);
                            int yc = (int)((para10 * xw + para11 * yw + para12) / d);

                            if (xc >= 0 && xc < img_x && yc >= 0 && yc < img_y)
                            {
                                reader.getPixel(xc, yc, rgb_tmp);
                                r += rgb_tmp[0]; // R
                                g += rgb_tmp[1]; // G
                                b += rgb_tmp[2]; // B
                                // System.out.println(xc+":"+yc+":"+rgb_tmp[0]+":"+rgb_tmp[1]+":"+rgb_tmp[2]);
                            }
                        }
                    }
                    this._patdata[iy * this._size.w + ix] = (((r / xdiv_x_ydiv) & 0xff) << 16) | (((g / xdiv_x_ydiv) & 0xff) << 8) | (((b / xdiv_x_ydiv) & 0xff));
                }
            }
            return(true);
        }
Exemplo n.º 16
0
 public NyARMat matrixAllocDup()
 {
     NyARMat result = new NyARMat(this.row, this.clm);
     // コピー
     int r, c;
     double[][] dest_m, src_m;
     dest_m = result._m;
     src_m = this._m;
     // コピーはFor順を変えてもOK
     for (r = this.row - 1; r >= 0; r--)
     {
         for (c = this.clm - 1; c >= 0; c--)
         {
             dest_m[r][c] = src_m[r][c];
         }
     }
     return result;
 }
Exemplo n.º 17
0
 /**
  * sourceの内容を自身に複製する。 Optimized 2008.04.19
  * 
  * @param i_source
  * @return
  */
 public void matrixDup(NyARMat i_source)
 {
     // 自身の配列サイズを相手のそれより大きいことを保障する。
     this.realloc(i_source.row, i_source.clm);
     // 内容を転写
     int r, c;
     double[][] src_m, dest_m;
     src_m = i_source._m;
     dest_m = this._m;
     // コピーはFor順を変えてもOK
     for (r = this.row - 1; r >= 0; r--)
     {
         for (c = this.clm - 1; c >= 0; c--)
         {
             dest_m[r][c] = src_m[r][c];
         }
     }
 }
Exemplo n.º 18
0
 /**
  * sourceの転置行列をdestに得る。arMatrixTrans()の代替品
  * 
  * @param dest
  * @param source
  * @return
  */
 public static void matrixTrans(NyARMat dest, NyARMat source)
 {
     if (dest.row != source.clm || dest.clm != source.row)
     {
         throw new NyARException();
     }
     NyARException.trap("未チェックのパス");
     // For順変更禁止
     for (int r = 0; r < dest.row; r++)
     {
         for (int c = 0; c < dest.clm; c++)
         {
             dest._m[r][c] = source._m[c][r];
         }
     }
 }
Exemplo n.º 19
0
        private Matrix GetProjectionMatrix(NyARParam i_arparam, float near, float far)
        {
            NyARMat trans_mat = new NyARMat(3, 4);
            NyARMat icpara_mat = new NyARMat(3, 4);
            double[,] p = new double[3, 3], q = new double[4, 4];
            int width, height;
            int i, j;

            NyARIntSize size = i_arparam.getScreenSize();
            width = size.w;
            height = size.h;

            i_arparam.getPerspectiveProjectionMatrix().decompMat(icpara_mat, trans_mat);

            double[][] icpara = icpara_mat.getArray();
            double[][] trans = trans_mat.getArray();

            for (i = 0; i < 3; i++)
            {
                for (j = 0; j < 3; j++)
                {
                    p[i, j] = icpara[i][j] / icpara[2][2];
                }
            }

            q[0, 0] = (2.0 * p[0, 0] / (width));
            q[0, 1] = (2.0 * p[0, 1] / (width));
            q[0, 2] = ((2.0 * p[0, 2] / (width)) - 1.0);
            q[0, 3] = 0.0;

            q[1, 0] = 0.0;
            q[1, 1] = (2.0 * p[1, 1] / (height));
            q[1, 2] = ((2.0 * p[1, 2] / (height)) - 1.0);
            q[1, 3] = 0.0;

            q[2, 0] = 0.0;
            q[2, 1] = 0.0;
            q[2, 2] = (far + near) / (far - near);
            q[2, 3] = -2.0 * far * near / (far - near);

            q[3, 0] = 0.0;
            q[3, 1] = 0.0;
            q[3, 2] = 1.0;
            q[3, 3] = 0.0;

            Matrix mat = Matrix.Identity;
            mat.M11 = (float)(q[0, 0] * trans[0][0] + q[0, 1] * trans[1][0] + q[0, 2] * trans[2][0]);
            mat.M12 = (float)(q[1, 0] * trans[0][0] + q[1, 1] * trans[1][0] + q[1, 2] * trans[2][0]);
            mat.M13 = (float)(q[2, 0] * trans[0][0] + q[2, 1] * trans[1][0] + q[2, 2] * trans[2][0]);
            mat.M14 = (float)(q[3, 0] * trans[0][0] + q[3, 1] * trans[1][0] + q[3, 2] * trans[2][0]);
            mat.M21 = (float)(q[0, 1] * trans[0][1] + q[0, 1] * trans[1][1] + q[0, 2] * trans[2][1]);
            mat.M22 = (float)(q[1, 1] * trans[0][1] + q[1, 1] * trans[1][1] + q[1, 2] * trans[2][1]);
            mat.M23 = (float)(q[2, 1] * trans[0][1] + q[2, 1] * trans[1][1] + q[2, 2] * trans[2][1]);
            mat.M24 = (float)(q[3, 1] * trans[0][1] + q[3, 1] * trans[1][1] + q[3, 2] * trans[2][1]);
            mat.M31 = (float)(q[0, 2] * trans[0][2] + q[0, 1] * trans[1][2] + q[0, 2] * trans[2][2]);
            mat.M32 = (float)(q[1, 2] * trans[0][2] + q[1, 1] * trans[1][2] + q[1, 2] * trans[2][2]);
            mat.M33 = -(float)(q[2, 2] * trans[0][2] + q[2, 1] * trans[1][2] + q[2, 2] * trans[2][2]);
            mat.M34 = -(float)(q[3, 2] * trans[0][2] + q[3, 1] * trans[1][2] + q[3, 2] * trans[2][2]);
            mat.M41 = (float)(q[0, 3] * trans[0][3] + q[0, 1] * trans[1][3] + q[0, 2] * trans[2][3] + q[0, 3]);
            mat.M42 = (float)(q[1, 3] * trans[0][3] + q[1, 1] * trans[1][3] + q[1, 2] * trans[2][3] + q[1, 3]);
            mat.M43 = (float)(q[2, 3] * trans[0][3] + q[2, 1] * trans[1][3] + q[2, 2] * trans[2][3] + q[2, 3]);
            mat.M44 = (float)(q[3, 3] * trans[0][3] + q[3, 1] * trans[1][3] + q[3, 2] * trans[2][3] + q[3, 3]);

            return mat;
        }
Exemplo n.º 20
0
        /**
         * ARToolKitのxt_by_x関数と同等な関数です。
         * 詳細は不明です。
         * @param input
         * 不明
         * @param i_output
         * 不明
         * @
         */
        private static void PCA_xt_by_x(NyARMat input, NyARMat i_output)
        {
            double[] in_;
            int row, clm;

            row = input.row;
            clm = input.clm;
            if (i_output.row != clm || i_output.clm != clm)
            {
                throw new NyARException();
            }

            int k, j;
            double[][] out_m = i_output._m;
            double w;
            for (int i = 0; i < clm; i++)
            {
                for (j = 0; j < clm; j++)
                {
                    if (j < i)
                    {
                        out_m[i][j] = out_m[j][i];// *out = output->m[j*clm+i];
                    }
                    else
                    {
                        w = 0.0;// *out = 0.0;
                        for (k = 0; k < row; k++)
                        {
                            in_ = input._m[k];// in=input.getRowArray(k);
                            w += (in_[i] * in_[j]);// *out += *in1 * *in2;
                        }
                        out_m[i][j] = w;
                    }
                }
            }
        }
Exemplo n.º 21
0
 /**
  * dim x dim の単位行列を作る。
  * 
  * @param dim
  * @return
  * @throws NyARException
  */
 public static NyARMat matrixAllocUnit(int dim)
 {
     NyARException.trap("未チェックのパス");
     NyARMat result = new NyARMat(dim, dim);
     NyARException.trap("未チェックのパス");
     NyARMat.matrixUnit(result);
     return result;
 }
        private static NyARMat makeMatAtB(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtB)
        {
            double v0, v1, v2, v3, v4, v5, v6, v7;

            v0 = v1 = v2 = v3 = v4 = v5 = v6 = v7 = 0;
            for (int i = 0; i < i_num; i++)
            {
                double wx = worldCoord[i].x;
                double wy = worldCoord[i].y;
                double sx = screenCoord[i].x;
                double sy = screenCoord[i].y;
                v0 += wx * sx;
                v1 += wy * sx;
                v2 += sx;
                v3 += wx * sy;
                v4 += wy * sy;
                v5 += sy;
                v6 += -wx * sx * sx - wx * sy * sy;
                v7 += -wy * sx * sx - wy * sy * sy;
            }
            double[][] t = o_matAtB.getArray();
            t[0][0] = v0;
            t[1][0] = v1;
            t[2][0] = v2;
            t[3][0] = v3;
            t[4][0] = v4;
            t[5][0] = v5;
            t[6][0] = v6;
            t[7][0] = v7;
            return(o_matAtB);
        }
Exemplo n.º 23
0
        private const double PCA_VZERO = 1e-16; // #define VZERO 1e-16

        /**
         * ARToolKitのEV_create関数と同等な関数です。
         * 詳細は不明です。
         * @param input
         * 不明。
         * @param u
         * 不明。
         * @param output
         * 不明。
         * @param ev
         * 不明。
         * @
         */
        private static void PCA_EV_create(NyARMat input, NyARMat u, NyARMat output, NyARVec ev)
        {
            NyARRuntimeException.trap("未チェックのパス");
            int row, clm;

            row = input.row; // row = input->row;
            clm = input.clm; // clm = input->clm;
            if (row <= 0 || clm <= 0)
            {
                throw new NyARRuntimeException();
            }
            if (u.row != row || u.clm != row)
            {// if( u->row != row || u->clm !=
                // row ){
                throw new NyARRuntimeException();
            }
            if (output.row != row || output.clm != clm)
            {// if( output->row !=
                // row || output->clm !=
                // clm ){
                throw new NyARRuntimeException();
            }
            if (ev.getClm() != row)
            {// if( ev->clm != row ){
                throw new NyARRuntimeException();
            }
            double[][] m, in_;
            double[]   m1, ev_array;
            double     sum, work;

            NyARRuntimeException.trap("未チェックのパス");
            m   = output._m;// m = output->m;
            in_ = input._m;
            int i;

            ev_array = ev.getArray();
            for (i = 0; i < row; i++)
            {
                NyARRuntimeException.trap("未チェックのパス");
                if (ev_array[i] < PCA_VZERO)
                {// if( ev->v[i] < VZERO ){
                    break;
                }
                NyARRuntimeException.trap("未チェックのパス");
                work = 1 / Math.Sqrt(Math.Abs(ev_array[i]));// work = 1 /
                // sqrt(fabs(ev->v[i]));
                for (int j = 0; j < clm; j++)
                {
                    sum = 0.0;
                    m1  = u._m[i];// m1 = &(u->m[i*row]);
                    // m2=input.getPointer(j);//m2 = &(input->m[j]);
                    for (int k = 0; k < row; k++)
                    {
                        sum += m1[k] + in_[k][j];// sum += *m1 * *m2;
                        // m1.incPtr(); //m1++;
                        // m2.addPtr(clm);//m2 += clm;
                    }
                    m1[j] = sum * work;// *(m++) = sum * work;
                    // {//*(m++) = sum * work;
                    // m.set(sum * work);
                    // m.incPtr();}
                }
            }
            for (; i < row; i++)
            {
                NyARRuntimeException.trap("未チェックのパス");
                ev_array[i] = 0.0;// ev->v[i] = 0.0;
                for (int j = 0; j < clm; j++)
                {
                    m[i][j] = 0.0;
                    // m.set(0.0);//*(m++) = 0.0;
                    // m.incPtr();
                }
            }
        }
Exemplo n.º 24
0
 // public void getRowVec(int i_row,NyARVec o_vec)
 // {
 // o_vec.set(this.m[i_row],this.clm);
 // }
 /**
  * aとbの積を自分自身に格納する。arMatrixMul()の代替品
  * 
  * @param a
  * @param b
  * @throws NyARException
  */
 public void matrixMul(NyARMat a, NyARMat b)
 {
     if (a.clm != b.row || this.row != a.row || this.clm != b.clm)
     {
         throw new NyARException();
     }
     double w;
     int r, c, i;
     double[][] am = a.m, bm = b.m, dm = this.m;
     // For順変更禁止
     for (r = 0; r < this.row; r++)
     {
         for (c = 0; c < this.clm; c++)
         {
             w = 0.0;// dest.setARELEM0(r, c,0.0);
             for (i = 0; i < a.clm; i++)
             {
                 w += am[r][i] * bm[i][c];// ARELEM0(dest, r, c) +=
                 // ARELEM0(a, r, i) * ARELEM0(b,
                 // i, c);
             }
             dm[r][c] = w;
         }
     }
 }
Exemplo n.º 25
0
 /**
  * i_copy_fromの内容を、thisへコピーします。 
  * @param i_copy_from
  * コピー元の行列です。
  * この行列のサイズは、thisと同じでなければなりません。
  */
 public void setValue(NyARMat i_copy_from)
 {
     // サイズ確認
     if (this.row != i_copy_from.row || this.clm != i_copy_from.clm)
     {
         throw new NyARException();
     }
     // 値コピー
     for (int r = this.row - 1; r >= 0; r--)
     {
         for (int c = this.clm - 1; c >= 0; c--)
         {
             this._m[r][c] = i_copy_from._m[r][c];
         }
     }
 }
Exemplo n.º 26
0
 /* double arMatrixDet(ARMat *m); */
 public static double arMatrixDet(NyARMat m)
 {
     NyARException.trap("動作未チェック/配列化未チェック");
     if (m.row != m.clm)
     {
         return 0.0;
     }
     return Det_mdet(m.getArray(), m.row, m.clm);// return mdet(m->m, m->row,m->row);
 }
Exemplo n.º 27
0
 /**
  * ARToolKitの、arMatrixUnit関数と同等な関数です。unitを単位行列に初期化します。
  * この関数は未チェックの為、実行すると例外が発生します。
  * @param unit
  * 操作するオブジェクト。
  */
 public static void matrixUnit(NyARMat unit)
 {
     if (unit.row != unit.clm)
     {
         throw new NyARException();
     }
     NyARException.trap("未チェックのパス");
     // For順変更禁止
     for (int r = 0; r < unit.getRow(); r++)
     {
         for (int c = 0; c < unit.getClm(); c++)
         {
             if (r == c)
             {
                 unit._m[r][c] = 1.0;
             }
             else
             {
                 unit._m[r][c] = 0.0;
             }
         }
     }
 }
        /**
         * この関数は、射影変換パラメータを計算します。
         * @param i_vertex
         * 変換元の4角系を定義する頂点配列。4頂点である必要がある。
         * @param o_para
         * 計算したパラメータの出力先配列
         * @return
         * 計算に成功するとtrueです。
         * @
         */
        private bool get_cpara(NyARIntPoint2d[] i_vertex, NyARMat o_para)
        {
            double[][] world = CPARAM_WORLD;
            NyARMat a = new NyARMat(8, 8);// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );
            double[][] a_array = a.getArray();
            NyARMat b = new NyARMat(8, 1);// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );
            double[][] b_array = b.getArray();
            double[] a_pt0, a_pt1;
            double[] world_pti;

            for (int i = 0; i < 4; i++)
            {
                a_pt0 = a_array[i * 2];
                a_pt1 = a_array[i * 2 + 1];
                world_pti = world[i];

                a_pt0[0] = (double)world_pti[0];// a->m[i*16+0] = world[i][0];
                a_pt0[1] = (double)world_pti[1];// a->m[i*16+1] = world[i][1];
                a_pt0[2] = 1.0;// a->m[i*16+2] = 1.0;
                a_pt0[3] = 0.0;// a->m[i*16+3] = 0.0;
                a_pt0[4] = 0.0;// a->m[i*16+4] = 0.0;
                a_pt0[5] = 0.0;// a->m[i*16+5] = 0.0;
                a_pt0[6] = (double)(-world_pti[0] * i_vertex[i].x);// a->m[i*16+6]= -world[i][0]*vertex[i][0];
                a_pt0[7] = (double)(-world_pti[1] * i_vertex[i].x);// a->m[i*16+7]=-world[i][1]*vertex[i][0];
                a_pt1[0] = 0.0;// a->m[i*16+8] = 0.0;
                a_pt1[1] = 0.0;// a->m[i*16+9] = 0.0;
                a_pt1[2] = 0.0;// a->m[i*16+10] = 0.0;
                a_pt1[3] = (double)world_pti[0];// a->m[i*16+11] = world[i][0];
                a_pt1[4] = (double)world_pti[1];// a->m[i*16+12] = world[i][1];
                a_pt1[5] = 1.0;// a->m[i*16+13] = 1.0;
                a_pt1[6] = (double)(-world_pti[0] * i_vertex[i].y);// a->m[i*16+14]=-world[i][0]*vertex[i][1];
                a_pt1[7] = (double)(-world_pti[1] * i_vertex[i].y);// a->m[i*16+15]=-world[i][1]*vertex[i][1];
                b_array[i * 2 + 0][0] = (double)i_vertex[i].x;// b->m[i*2+0] =vertex[i][0];
                b_array[i * 2 + 1][0] = (double)i_vertex[i].y;// b->m[i*2+1] =vertex[i][1];
            }
            if (!a.inverse())
            {
                return false;
            }

            o_para.mul(a, b);
            return true;
        }
Exemplo n.º 29
0
 /** @deprecated */
 public void matrixMul(NyARMat i_mat_a, NyARMat i_mat_b) { this.mul(i_mat_a, i_mat_b); }
        /**
         * この関数は、ラスタのi_vertexsで定義される四角形からパターンを取得して、インスタンスに格納します。
         */
        public virtual bool pickFromRaster(INyARRgbRaster image, NyARIntPoint2d[] i_vertexs)
        {
            // パターンの切り出しに失敗することもある。
            NyARMat cpara = new NyARMat(8, 1);
            if (!get_cpara(i_vertexs, cpara))
            {
                return false;
            }
            double[][] para = cpara.getArray();
            double para00 = para[0 * 3 + 0][0];
            double para01 = para[0 * 3 + 1][0];
            double para02 = para[0 * 3 + 2][0];
            double para10 = para[1 * 3 + 0][0];
            double para11 = para[1 * 3 + 1][0];
            double para12 = para[1 * 3 + 2][0];
            double para20 = para[2 * 3 + 0][0];
            double para21 = para[2 * 3 + 1][0];
            double para22 = 1.0;

            int lx1 = (int)((i_vertexs[0].x - i_vertexs[1].x) * (i_vertexs[0].x - i_vertexs[1].x) + (i_vertexs[0].y - i_vertexs[1].y) * (i_vertexs[0].y - i_vertexs[1].y));
            int lx2 = (int)((i_vertexs[2].x - i_vertexs[3].x) * (i_vertexs[2].x - i_vertexs[3].x) + (i_vertexs[2].y - i_vertexs[3].y) * (i_vertexs[2].y - i_vertexs[3].y));
            int ly1 = (int)((i_vertexs[1].x - i_vertexs[2].x) * (i_vertexs[1].x - i_vertexs[2].x) + (i_vertexs[1].y - i_vertexs[2].y) * (i_vertexs[1].y - i_vertexs[2].y));
            int ly2 = (int)((i_vertexs[3].x - i_vertexs[0].x) * (i_vertexs[3].x - i_vertexs[0].x) + (i_vertexs[3].y - i_vertexs[0].y) * (i_vertexs[3].y - i_vertexs[0].y));
            if (lx2 > lx1)
            {
                lx1 = lx2;
            }
            if (ly2 > ly1)
            {
                ly1 = ly2;
            }

            int sample_pixel_x = this._size.w;
            int sample_pixel_y = this._size.h;
            while (sample_pixel_x * sample_pixel_x < lx1 / 4)
            {
                sample_pixel_x *= 2;
            }
            while (sample_pixel_y * sample_pixel_y < ly1 / 4)
            {
                sample_pixel_y *= 2;
            }

            if (sample_pixel_x > AR_PATT_SAMPLE_NUM)
            {
                sample_pixel_x = AR_PATT_SAMPLE_NUM;
            }
            if (sample_pixel_y > AR_PATT_SAMPLE_NUM)
            {
                sample_pixel_y = AR_PATT_SAMPLE_NUM;
            }

            int xdiv = sample_pixel_x / this._size.w;// xdiv = xdiv2/Config.AR_PATT_SIZE_X;
            int ydiv = sample_pixel_y / this._size.h;// ydiv = ydiv2/Config.AR_PATT_SIZE_Y;


            int img_x = image.getWidth();
            int img_y = image.getHeight();

            double xdiv2_reciprocal = 1.0 / sample_pixel_x;
            double ydiv2_reciprocal = 1.0 / sample_pixel_y;
            int r, g, b;
            int[] rgb_tmp = new int[3];

            //ピクセルリーダーを取得
            INyARRgbPixelDriver reader = image.getRgbPixelDriver();
            int xdiv_x_ydiv = xdiv * ydiv;

            for (int iy = 0; iy < this._size.h; iy++)
            {
                for (int ix = 0; ix < this._size.w; ix++)
                {
                    r = g = b = 0;
                    //1ピクセルを作成
                    for (int j = 0; j < ydiv; j++)
                    {
                        double yw = 102.5 + 5.0 * (iy * ydiv + j + 0.5) * ydiv2_reciprocal;
                        for (int i = 0; i < xdiv; i++)
                        {
                            double xw = 102.5 + 5.0 * (ix * xdiv + i + 0.5) * xdiv2_reciprocal;
                            double d = para20 * xw + para21 * yw + para22;
                            if (d == 0)
                            {
                                throw new NyARException();
                            }
                            int xc = (int)((para00 * xw + para01 * yw + para02) / d);
                            int yc = (int)((para10 * xw + para11 * yw + para12) / d);

                            if (xc >= 0 && xc < img_x && yc >= 0 && yc < img_y)
                            {
                                reader.getPixel(xc, yc, rgb_tmp);
                                r += rgb_tmp[0];// R
                                g += rgb_tmp[1];// G
                                b += rgb_tmp[2];// B
                                // System.out.println(xc+":"+yc+":"+rgb_tmp[0]+":"+rgb_tmp[1]+":"+rgb_tmp[2]);
                            }
                        }
                    }
                    this._patdata[iy * this._size.w + ix] = (((r / xdiv_x_ydiv) & 0xff) << 16) | (((g / xdiv_x_ydiv) & 0xff) << 8) | (((b / xdiv_x_ydiv) & 0xff));
                }
            }
            return true;
        }
        /**
         * この関数は、ARToolKitのarParamDecompMatと同じです。
         * 動作はよくわかりません…。
         * @param o_cpara
         * 詳細不明。3x4のマトリクスを指定すること。
         * @param o_trans
         * 詳細不明。3x4のマトリクスを指定すること。
         */
        public void decompMat(NyARMat o_cpara, NyARMat o_trans)
        {
            double rem1, rem2, rem3;
            double c00, c01, c02, c03, c10, c11, c12, c13, c20, c21, c22, c23;
            if (this.m23 >= 0)
            {// if( source[2][3] >= 0 ) {
                c00 = this.m00;
                c01 = this.m01;
                c02 = this.m02;
                c03 = this.m03;
                c10 = this.m10;
                c11 = this.m11;
                c12 = this.m12;
                c13 = this.m13;
                c20 = this.m20;
                c21 = this.m21;
                c22 = this.m22;
                c23 = this.m23;
            }
            else
            {
                // <Optimize>
                // for(int r = 0; r < 3; r++ ){
                // for(int c = 0; c < 4; c++ ){
                // Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]);
                // }
                // }
                c00 = -this.m00;
                c01 = -this.m01;
                c02 = -this.m02;
                c03 = -this.m03;
                c10 = -this.m10;
                c11 = -this.m11;
                c12 = -this.m12;
                c13 = -this.m13;
                c20 = -this.m20;
                c21 = -this.m21;
                c22 = -this.m22;
                c23 = -this.m23;
            }

            double[][] cpara = o_cpara.getArray();
            double[][] trans = o_trans.getArray();
            for (int r = 0; r < 3; r++)
            {
                for (int c = 0; c < 4; c++)
                {
                    cpara[r][c] = 0.0;// cpara[r][c] = 0.0;
                }
            }
            cpara[2][2] = norm(c20, c21, c22);// cpara[2][2] =norm( Cpara[2][0],Cpara[2][1],Cpara[2][2]);
            trans[2][0] = c20 / cpara[2][2];// trans[2][0] = Cpara[2][0] /cpara[2][2];
            trans[2][1] = c21 / cpara[2][2];// trans[2][1] = Cpara[2][1] / cpara[2][2];
            trans[2][2] = c22 / cpara[2][2];// trans[2][2] =Cpara[2][2] /cpara[2][2];
            trans[2][3] = c23 / cpara[2][2];// trans[2][3] =Cpara[2][3] /cpara[2][2];

            cpara[1][2] = dot(trans[2][0], trans[2][1], trans[2][2], c10, c11, c12);// cpara[1][2]=dot(trans[2][0],trans[2][1],trans[2][2],Cpara[1][0],Cpara[1][1],Cpara[1][2]);
            rem1 = c10 - cpara[1][2] * trans[2][0];// rem1 =Cpara[1][0] -cpara[1][2] *trans[2][0];
            rem2 = c11 - cpara[1][2] * trans[2][1];// rem2 =Cpara[1][1] -cpara[1][2] *trans[2][1];
            rem3 = c12 - cpara[1][2] * trans[2][2];// rem3 =Cpara[1][2] -cpara[1][2] *trans[2][2];
            cpara[1][1] = norm(rem1, rem2, rem3);// cpara[1][1] = norm( rem1,// rem2, rem3 );
            trans[1][0] = rem1 / cpara[1][1];// trans[1][0] = rem1 / cpara[1][1];
            trans[1][1] = rem2 / cpara[1][1];// trans[1][1] = rem2 / cpara[1][1];
            trans[1][2] = rem3 / cpara[1][1];// trans[1][2] = rem3 / cpara[1][1];

            cpara[0][2] = dot(trans[2][0], trans[2][1], trans[2][2], c00, c01, c02);// cpara[0][2] =dot(trans[2][0], trans[2][1],trans[2][2],Cpara[0][0],Cpara[0][1],Cpara[0][2]);
            cpara[0][1] = dot(trans[1][0], trans[1][1], trans[1][2], c00, c01, c02);// cpara[0][1]=dot(trans[1][0],trans[1][1],trans[1][2],Cpara[0][0],Cpara[0][1],Cpara[0][2]);
            rem1 = c00 - cpara[0][1] * trans[1][0] - cpara[0][2] * trans[2][0];// rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0]- cpara[0][2]*trans[2][0];
            rem2 = c01 - cpara[0][1] * trans[1][1] - cpara[0][2] * trans[2][1];// rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1]- cpara[0][2]*trans[2][1];
            rem3 = c02 - cpara[0][1] * trans[1][2] - cpara[0][2] * trans[2][2];// rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];
            cpara[0][0] = norm(rem1, rem2, rem3);// cpara[0][0] = norm( rem1,rem2, rem3 );
            trans[0][0] = rem1 / cpara[0][0];// trans[0][0] = rem1 / cpara[0][0];
            trans[0][1] = rem2 / cpara[0][0];// trans[0][1] = rem2 / cpara[0][0];
            trans[0][2] = rem3 / cpara[0][0];// trans[0][2] = rem3 / cpara[0][0];

            trans[1][3] = (c13 - cpara[1][2] * trans[2][3]) / cpara[1][1];// trans[1][3] = (Cpara[1][3] -cpara[1][2]*trans[2][3]) / cpara[1][1];
            trans[0][3] = (c03 - cpara[0][1] * trans[1][3] - cpara[0][2] * trans[2][3]) / cpara[0][0];// trans[0][3] = (Cpara[0][3] -cpara[0][1]*trans[1][3]-cpara[0][2]*trans[2][3]) / cpara[0][0];

            for (int r = 0; r < 3; r++)
            {
                for (int c = 0; c < 3; c++)
                {
                    cpara[r][c] /= cpara[2][2];// cpara[r][c] /= cpara[2][2];
                }
            }
            return;
        }
Exemplo n.º 32
0
      /// <summary>
      /// Returns a right-handed perspective transformation matrix built from the camera calibration data.
      /// </summary>
      /// <param name="arParameters">The camera calibration data.</param>
      /// <param name="nearPlane">The near view plane of the frustum.</param>
      /// <param name="farPlane">The far view plane of the frustum.</param>
      /// <returns>The projection matrix.</returns>
      internal static Matrix3D GetCameraFrustumRH(this NyARParam arParameters, double nearPlane, double farPlane)
      {
         NyARMat transformation = new NyARMat(3, 4);
         NyARMat icParameters = new NyARMat(3, 4);
         double[,] p = new double[3, 3];
         double[,] q = new double[4, 4];

         NyARIntSize size = arParameters.getScreenSize();
         int width = size.w;
         int height = size.h;

         arParameters.getPerspectiveProjectionMatrix().decompMat(icParameters, transformation);

         double[][] icpara = icParameters.getArray();
         double[][] trans = transformation.getArray();
         for (int i = 0; i < 4; i++)
         {
            icpara[1][i] = (height - 1) * (icpara[2][i]) - icpara[1][i];
         }

         for (int i = 0; i < 3; i++)
         {
            for (int j = 0; j < 3; j++)
            {
               p[i, j] = icpara[i][j] / icpara[2][2];
            }
         }

         q[0, 0] = (2.0 * p[0, 0] / (width - 1));
         q[0, 1] = (2.0 * p[0, 1] / (width - 1));
         q[0, 2] = ((2.0 * p[0, 2] / (width - 1)) - 1.0);
         q[0, 3] = 0.0;

         q[1, 0] = 0.0;
         q[1, 1] = -(2.0 * p[1, 1] / (height - 1));
         q[1, 2] = -((2.0 * p[1, 2] / (height - 1)) - 1.0);
         q[1, 3] = 0.0;

         q[2, 0] = 0.0;
         q[2, 1] = 0.0;
         q[2, 2] = (farPlane + nearPlane) / (nearPlane - farPlane);
         q[2, 3] = 2.0 * farPlane * nearPlane / (nearPlane - farPlane);

         q[3, 0] = 0.0;
         q[3, 1] = 0.0;
         q[3, 2] = -1.0;
         q[3, 3] = 0.0;

         return new Matrix3D( q[0, 0] * trans[0][0] + q[0, 1] * trans[1][0] + q[0, 2] * trans[2][0],
                              q[1, 0] * trans[0][0] + q[1, 1] * trans[1][0] + q[1, 2] * trans[2][0],
                              q[2, 0] * trans[0][0] + q[2, 1] * trans[1][0] + q[2, 2] * trans[2][0],
                              q[3, 0] * trans[0][0] + q[3, 1] * trans[1][0] + q[3, 2] * trans[2][0],
                              q[0, 0] * trans[0][1] + q[0, 1] * trans[1][1] + q[0, 2] * trans[2][1],
                              q[1, 0] * trans[0][1] + q[1, 1] * trans[1][1] + q[1, 2] * trans[2][1],
                              q[2, 0] * trans[0][1] + q[2, 1] * trans[1][1] + q[2, 2] * trans[2][1],
                              q[3, 0] * trans[0][1] + q[3, 1] * trans[1][1] + q[3, 2] * trans[2][1],
                              q[0, 0] * trans[0][2] + q[0, 1] * trans[1][2] + q[0, 2] * trans[2][2],
                              q[1, 0] * trans[0][2] + q[1, 1] * trans[1][2] + q[1, 2] * trans[2][2],
                              q[2, 0] * trans[0][2] + q[2, 1] * trans[1][2] + q[2, 2] * trans[2][2],
                              q[3, 0] * trans[0][2] + q[3, 1] * trans[1][2] + q[3, 2] * trans[2][2],
                              q[0, 0] * trans[0][3] + q[0, 1] * trans[1][3] + q[0, 2] * trans[2][3] + q[0, 3],
                              q[1, 0] * trans[0][3] + q[1, 1] * trans[1][3] + q[1, 2] * trans[2][3] + q[1, 3],
                              q[2, 0] * trans[0][3] + q[2, 1] * trans[1][3] + q[2, 2] * trans[2][3] + q[2, 3],
                              q[3, 0] * trans[0][3] + q[3, 1] * trans[1][3] + q[3, 2] * trans[2][3] + q[3, 3]);
      }
Exemplo n.º 33
0
        private const double PCA_VZERO = 1e-16; // #define VZERO 1e-16
        /**
         * ARToolKitのEV_create関数と同等な関数です。
         * 詳細は不明です。
         * @param input
         * 不明。
         * @param u
         * 不明。
         * @param output
         * 不明。
         * @param ev
         * 不明。
         * @
         */
        private static void PCA_EV_create(NyARMat input, NyARMat u, NyARMat output, NyARVec ev)
        {
            NyARException.trap("未チェックのパス");
            int row, clm;
            row = input.row;// row = input->row;
            clm = input.clm;// clm = input->clm;
            if (row <= 0 || clm <= 0)
            {
                throw new NyARException();
            }
            if (u.row != row || u.clm != row)
            {// if( u->row != row || u->clm !=
                // row ){
                throw new NyARException();
            }
            if (output.row != row || output.clm != clm)
            {// if( output->row !=
                // row || output->clm !=
                // clm ){
                throw new NyARException();
            }
            if (ev.getClm() != row)
            {// if( ev->clm != row ){
                throw new NyARException();
            }
            double[][] m, in_;
            double[] m1, ev_array;
            double sum, work;

            NyARException.trap("未チェックのパス");
            m = output._m;// m = output->m;
            in_ = input._m;
            int i;
            ev_array = ev.getArray();
            for (i = 0; i < row; i++)
            {
                NyARException.trap("未チェックのパス");
                if (ev_array[i] < PCA_VZERO)
                {// if( ev->v[i] < VZERO ){
                    break;
                }
                NyARException.trap("未チェックのパス");
                work = 1 / Math.Sqrt(Math.Abs(ev_array[i]));// work = 1 /
                // sqrt(fabs(ev->v[i]));
                for (int j = 0; j < clm; j++)
                {
                    sum = 0.0;
                    m1 = u._m[i];// m1 = &(u->m[i*row]);
                    // m2=input.getPointer(j);//m2 = &(input->m[j]);
                    for (int k = 0; k < row; k++)
                    {
                        sum += m1[k] + in_[k][j];// sum += *m1 * *m2;
                        // m1.incPtr(); //m1++;
                        // m2.addPtr(clm);//m2 += clm;
                    }
                    m1[j] = sum * work;// *(m++) = sum * work;
                    // {//*(m++) = sum * work;
                    // m.set(sum * work);
                    // m.incPtr();}
                }
            }
            for (; i < row; i++)
            {
                NyARException.trap("未チェックのパス");
                ev_array[i] = 0.0;// ev->v[i] = 0.0;
                for (int j = 0; j < clm; j++)
                {
                    m[i][j] = 0.0;
                    // m.set(0.0);//*(m++) = 0.0;
                    // m.incPtr();
                }
            }
        }
Exemplo n.º 34
0
        /**
         * この関数は、ARToolKitのarParamDecompMatと同じです。
         * 動作はよくわかりません…。
         * @param o_cpara
         * 詳細不明。3x4のマトリクスを指定すること。
         * @param o_trans
         * 詳細不明。3x4のマトリクスを指定すること。
         */
        public void decompMat(NyARMat o_cpara, NyARMat o_trans)
        {
            double rem1, rem2, rem3;
            double c00, c01, c02, c03, c10, c11, c12, c13, c20, c21, c22, c23;

            if (this.m23 >= 0)
            {// if( source[2][3] >= 0 ) {
                c00 = this.m00;
                c01 = this.m01;
                c02 = this.m02;
                c03 = this.m03;
                c10 = this.m10;
                c11 = this.m11;
                c12 = this.m12;
                c13 = this.m13;
                c20 = this.m20;
                c21 = this.m21;
                c22 = this.m22;
                c23 = this.m23;
            }
            else
            {
                // <Optimize>
                // for(int r = 0; r < 3; r++ ){
                // for(int c = 0; c < 4; c++ ){
                // Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]);
                // }
                // }
                c00 = -this.m00;
                c01 = -this.m01;
                c02 = -this.m02;
                c03 = -this.m03;
                c10 = -this.m10;
                c11 = -this.m11;
                c12 = -this.m12;
                c13 = -this.m13;
                c20 = -this.m20;
                c21 = -this.m21;
                c22 = -this.m22;
                c23 = -this.m23;
            }

            double[][] cpara = o_cpara.getArray();
            double[][] trans = o_trans.getArray();
            for (int r = 0; r < 3; r++)
            {
                for (int c = 0; c < 4; c++)
                {
                    cpara[r][c] = 0.0;// cpara[r][c] = 0.0;
                }
            }
            cpara[2][2] = norm(c20, c21, c22);                                                         // cpara[2][2] =norm( Cpara[2][0],Cpara[2][1],Cpara[2][2]);
            trans[2][0] = c20 / cpara[2][2];                                                           // trans[2][0] = Cpara[2][0] /cpara[2][2];
            trans[2][1] = c21 / cpara[2][2];                                                           // trans[2][1] = Cpara[2][1] / cpara[2][2];
            trans[2][2] = c22 / cpara[2][2];                                                           // trans[2][2] =Cpara[2][2] /cpara[2][2];
            trans[2][3] = c23 / cpara[2][2];                                                           // trans[2][3] =Cpara[2][3] /cpara[2][2];

            cpara[1][2] = dot(trans[2][0], trans[2][1], trans[2][2], c10, c11, c12);                   // cpara[1][2]=dot(trans[2][0],trans[2][1],trans[2][2],Cpara[1][0],Cpara[1][1],Cpara[1][2]);
            rem1        = c10 - cpara[1][2] * trans[2][0];                                             // rem1 =Cpara[1][0] -cpara[1][2] *trans[2][0];
            rem2        = c11 - cpara[1][2] * trans[2][1];                                             // rem2 =Cpara[1][1] -cpara[1][2] *trans[2][1];
            rem3        = c12 - cpara[1][2] * trans[2][2];                                             // rem3 =Cpara[1][2] -cpara[1][2] *trans[2][2];
            cpara[1][1] = norm(rem1, rem2, rem3);                                                      // cpara[1][1] = norm( rem1,// rem2, rem3 );
            trans[1][0] = rem1 / cpara[1][1];                                                          // trans[1][0] = rem1 / cpara[1][1];
            trans[1][1] = rem2 / cpara[1][1];                                                          // trans[1][1] = rem2 / cpara[1][1];
            trans[1][2] = rem3 / cpara[1][1];                                                          // trans[1][2] = rem3 / cpara[1][1];

            cpara[0][2] = dot(trans[2][0], trans[2][1], trans[2][2], c00, c01, c02);                   // cpara[0][2] =dot(trans[2][0], trans[2][1],trans[2][2],Cpara[0][0],Cpara[0][1],Cpara[0][2]);
            cpara[0][1] = dot(trans[1][0], trans[1][1], trans[1][2], c00, c01, c02);                   // cpara[0][1]=dot(trans[1][0],trans[1][1],trans[1][2],Cpara[0][0],Cpara[0][1],Cpara[0][2]);
            rem1        = c00 - cpara[0][1] * trans[1][0] - cpara[0][2] * trans[2][0];                 // rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0]- cpara[0][2]*trans[2][0];
            rem2        = c01 - cpara[0][1] * trans[1][1] - cpara[0][2] * trans[2][1];                 // rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1]- cpara[0][2]*trans[2][1];
            rem3        = c02 - cpara[0][1] * trans[1][2] - cpara[0][2] * trans[2][2];                 // rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];
            cpara[0][0] = norm(rem1, rem2, rem3);                                                      // cpara[0][0] = norm( rem1,rem2, rem3 );
            trans[0][0] = rem1 / cpara[0][0];                                                          // trans[0][0] = rem1 / cpara[0][0];
            trans[0][1] = rem2 / cpara[0][0];                                                          // trans[0][1] = rem2 / cpara[0][0];
            trans[0][2] = rem3 / cpara[0][0];                                                          // trans[0][2] = rem3 / cpara[0][0];

            trans[1][3] = (c13 - cpara[1][2] * trans[2][3]) / cpara[1][1];                             // trans[1][3] = (Cpara[1][3] -cpara[1][2]*trans[2][3]) / cpara[1][1];
            trans[0][3] = (c03 - cpara[0][1] * trans[1][3] - cpara[0][2] * trans[2][3]) / cpara[0][0]; // trans[0][3] = (Cpara[0][3] -cpara[0][1]*trans[1][3]-cpara[0][2]*trans[2][3]) / cpara[0][0];

            for (int r = 0; r < 3; r++)
            {
                for (int c = 0; c < 3; c++)
                {
                    cpara[r][c] /= cpara[2][2];// cpara[r][c] /= cpara[2][2];
                }
            }
            return;
        }
Exemplo n.º 35
0
        /**
         * ARToolKitのx_by_xtと同等な関数です。
         * 詳細は不明です。
         * この関数は未チェックの為、使用すると例外が発生します。
         * @param input
         * 不明
         * @param output
         * 不明
         * @
         */
        private static void PCA_x_by_xt(NyARMat input, NyARMat output)
        {
            NyARException.trap("動作未チェック/配列化未チェック");
            int row, clm;
            // double[][] out;
            double[] in1, in2;

            NyARException.trap("未チェックのパス");
            row = input.row;
            clm = input.clm;
            NyARException.trap("未チェックのパス");
            if (output.row != row || output.clm != row)
            {
                throw new NyARException();
            }

            // out = output.getArray();
            for (int i = 0; i < row; i++)
            {
                for (int j = 0; j < row; j++)
                {
                    if (j < i)
                    {
                        NyARException.trap("未チェックのパス");
                        output._m[i][j] = output._m[j][i];// *out =
                        // output->m[j*row+i];
                    }
                    else
                    {
                        NyARException.trap("未チェックのパス");
                        in1 = input._m[i];// input.getRowArray(i);//in1 = &(input->m[clm*i]);
                        in2 = input._m[j];// input.getRowArray(j);//in2 = &(input->m[clm*j]);
                        output._m[i][j] = 0;// *out = 0.0;
                        for (int k = 0; k < clm; k++)
                        {
                            output._m[i][j] += (in1[k] * in2[k]);// *out += *(in1++)
                            // * *(in2++);
                        }
                    }
                    // out.incPtr();
                }
            }
        }
Exemplo n.º 36
0
 public static NyARMat matrixAllocMul(NyARMat a, NyARMat b)
 {
     NyARException.trap("未チェックのパス");
     NyARMat dest = new NyARMat(a.row, b.clm);
     NyARException.trap("未チェックのパス");
     dest.matrixMul(a, b);
     return dest;
 }
Exemplo n.º 37
0
        /**
         * int arMatrixPCA( ARMat *input, ARMat *evec, ARVec *ev, ARVec *mean );
         * 関数の置き換え。input引数がthisになる。 Optimize:2008.04.19
         *
         * @param o_evec
         * @param o_ev
         *
         * @param mean
         * @throws NyARException
         */
        public void matrixPCA(NyARMat o_evec, NyARVec o_ev, NyARVec mean)
        {
            double srow, sum;
            int    l_row, l_clm;
            int    check;

            l_row = this.row; // row = input->row;
            l_clm = this.clm; // clm = input->clm;
            check = (l_row < l_clm) ? l_row : l_clm;
            if (l_row < 2 || l_clm < 2)
            {
                throw new NyARException();
            }
            if (o_evec.clm != l_clm || o_evec.row != check)
            {// if( evec->clm !=
                // input->clm ||
                // evec->row !=
                // check ){
                throw new NyARException();
            }
            if (o_ev.getClm() != check)
            {// if( ev->clm != check ){
                throw new NyARException();
            }
            if (mean.getClm() != l_clm)
            {// if( mean->clm != input->clm ){
                throw new NyARException();
            }

            // 自分の内容をワークにコピー(高速化の為に、1度作ったインスタンスは使いまわす)
            NyARMat work;

            if (this.wk_work_matrixPCA == null)
            {
                work = this.matrixAllocDup();
                this.wk_work_matrixPCA = work;
            }
            else
            {
                work = this.wk_work_matrixPCA;
                work.matrixDup(this);// arMatrixAllocDup( input );work =
                // arMatrixAllocDup( input );
            }

            srow = Math.Sqrt((double)l_row);
            work.PCA_EX(mean);

            PCA_CENTER(work, mean);

            int i, j;

            // For順変更OK
            for (i = 0; i < l_row; i++)
            {
                for (j = 0; j < l_clm; j++)
                {
                    work.m[i][j] /= srow;// work->m[i] /= srow;
                }
            }

            work.PCA_PCA(o_evec, o_ev);

            sum = 0.0;
            double[] ev_array = o_ev.getArray();
            int      ev_clm   = o_ev.getClm();

            // For順変更禁止
            for (i = 0; i < ev_clm; i++)
            {                       // for(int i = 0; i < ev->clm; i++ ){
                sum += ev_array[i]; // sum += ev->v[i];
            }
            // For順変更禁止
            for (i = 0; i < ev_clm; i++)
            {                       // for(int i = 0; i < ev->clm; i++ ){
                ev_array[i] /= sum; // ev->v[i] /= sum;
            }
        }
Exemplo n.º 38
0
        /**
         * arMatrixInv関数の代替品です。 destにsourceの逆行列を返します。
         * 
         * @param dest
         * @param source
         * @throws NyARException
         */
        public static void matrixInv(NyARMat dest, NyARMat source)
        {
            NyARException.trap("未チェックのパス");
            dest.matrixDup(source);

            NyARException.trap("未チェックのパス");
            dest.matrixSelfInv();
        }
Exemplo n.º 39
0
        /**
         * 右手系の視錐台を作ります。
         * この視錐台は、ARToolKitのarglCameraViewRHの作る視錐台と同じです。
         * @param i_screen_width
         * スクリーンサイズを指定します。
         * @param i_screen_height
         * スクリーンサイズを指定します。
         * @param i_dist_min
         * near pointを指定します(mm単位)
         * @param i_dist_max
         * far pointを指定します(mm単位)
         * @param o_frustum
         * 視錐台の格納先オブジェクトを指定します。
         */
        public void makeCameraFrustumRH(double i_screen_width, double i_screen_height, double i_dist_min, double i_dist_max, NyARDoubleMatrix44 o_frustum)
        {
            NyARMat trans_mat  = new NyARMat(3, 4);
            NyARMat icpara_mat = new NyARMat(3, 4);

            double[][] p = ArrayUtils.newDouble2dArray(3, 3);
            int        i;

            this.decompMat(icpara_mat, trans_mat);

            double[][] icpara = icpara_mat.getArray();
            double[][] trans  = trans_mat.getArray();
            for (i = 0; i < 4; i++)
            {
                icpara[1][i] = (i_screen_height - 1) * (icpara[2][i]) - icpara[1][i];
            }
            p[0][0] = icpara[0][0] / icpara[2][2];
            p[0][1] = icpara[0][1] / icpara[2][2];
            p[0][2] = icpara[0][2] / icpara[2][2];

            p[1][0] = icpara[1][0] / icpara[2][2];
            p[1][1] = icpara[1][1] / icpara[2][2];
            p[1][2] = icpara[1][2] / icpara[2][2];

            p[2][0] = icpara[2][0] / icpara[2][2];
            p[2][1] = icpara[2][1] / icpara[2][2];
            p[2][2] = icpara[2][2] / icpara[2][2];

            double q00, q01, q02, q03, q10, q11, q12, q13, q20, q21, q22, q23, q30, q31, q32, q33;

            //視錐台への変換
            q00           = (2.0 * p[0][0] / (i_screen_width - 1));
            q01           = (2.0 * p[0][1] / (i_screen_width - 1));
            q02           = -((2.0 * p[0][2] / (i_screen_width - 1)) - 1.0);
            q03           = 0.0;
            o_frustum.m00 = q00 * trans[0][0] + q01 * trans[1][0] + q02 * trans[2][0];
            o_frustum.m01 = q00 * trans[0][1] + q01 * trans[1][1] + q02 * trans[2][1];
            o_frustum.m02 = q00 * trans[0][2] + q01 * trans[1][2] + q02 * trans[2][2];
            o_frustum.m03 = q00 * trans[0][3] + q01 * trans[1][3] + q02 * trans[2][3] + q03;

            q10           = 0.0;
            q11           = -(2.0 * p[1][1] / (i_screen_height - 1));
            q12           = -((2.0 * p[1][2] / (i_screen_height - 1)) - 1.0);
            q13           = 0.0;
            o_frustum.m10 = q10 * trans[0][0] + q11 * trans[1][0] + q12 * trans[2][0];
            o_frustum.m11 = q10 * trans[0][1] + q11 * trans[1][1] + q12 * trans[2][1];
            o_frustum.m12 = q10 * trans[0][2] + q11 * trans[1][2] + q12 * trans[2][2];
            o_frustum.m13 = q10 * trans[0][3] + q11 * trans[1][3] + q12 * trans[2][3] + q13;

            q20           = 0.0;
            q21           = 0.0;
            q22           = (i_dist_max + i_dist_min) / (i_dist_min - i_dist_max);
            q23           = 2.0 * i_dist_max * i_dist_min / (i_dist_min - i_dist_max);
            o_frustum.m20 = q20 * trans[0][0] + q21 * trans[1][0] + q22 * trans[2][0];
            o_frustum.m21 = q20 * trans[0][1] + q21 * trans[1][1] + q22 * trans[2][1];
            o_frustum.m22 = q20 * trans[0][2] + q21 * trans[1][2] + q22 * trans[2][2];
            o_frustum.m23 = q20 * trans[0][3] + q21 * trans[1][3] + q22 * trans[2][3] + q23;

            q30           = 0.0;
            q31           = 0.0;
            q32           = -1.0;
            q33           = 0.0;
            o_frustum.m30 = q30 * trans[0][0] + q31 * trans[1][0] + q32 * trans[2][0];
            o_frustum.m31 = q30 * trans[0][1] + q31 * trans[1][1] + q32 * trans[2][1];
            o_frustum.m32 = q30 * trans[0][2] + q31 * trans[1][2] + q32 * trans[2][2];
            o_frustum.m33 = q30 * trans[0][3] + q31 * trans[1][3] + q32 * trans[2][3] + q33;
            return;
        }
Exemplo n.º 40
0
 /** @deprecated */
 public void copyFrom(NyARMat i_copy_from)
 {
     this.setValue(i_copy_from);
 }
Exemplo n.º 41
0
 /** @deprecated */
 public void matrixMul(NyARMat i_mat_a, NyARMat i_mat_b)
 {
     this.mul(i_mat_a, i_mat_b);
 }
Exemplo n.º 42
0
        /* int arMatrixPCA2( ARMat *input, ARMat *evec, ARVec *ev ); */
        public static void arMatrixPCA2(NyARMat input, NyARMat evec, NyARVec ev)
        {
            NyARException.trap("未チェックのパス");
            NyARMat work;
            // double srow; // unreferenced
            double sum;
            int row, clm;
            int check;

            row = input.row;// row = input->row;
            clm = input.clm;// clm = input->clm;
            check = (row < clm) ? row : clm;
            if (row < 2 || clm < 2)
            {
                throw new NyARException();
            }
            if (evec.getClm() != input.clm || evec.row != check)
            {// if( evec->clm!= input->clm|| evec->row!= check ){
                throw new NyARException();
            }
            if (ev.getClm() != check)
            {// if( ev->clm != check ){
                throw new NyARException();
            }

            NyARException.trap("未チェックのパス");
            work = input.matrixAllocDup();

            NyARException.trap("未チェックパス");
            work.PCA_PCA(evec, ev);// rval = PCA( work, evec, ev );
            sum = 0.0;
            double[] ev_array = ev.getArray();
            for (int i = 0; i < ev.getClm(); i++)
            {// for( i = 0; i < ev->clm; i++
                // ){
                NyARException.trap("未チェックパス");
                sum += ev_array[i];// sum += ev->v[i];
            }
            for (int i = 0; i < ev.getClm(); i++)
            {// for(int i = 0; i < ev->clm;i++ ){
                NyARException.trap("未チェックパス");
                ev_array[i] /= sum;// ev->v[i] /= sum;
            }
            return;
        }