/** * この関数は、射影変換パラメータを計算します。 * @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); }
/** * この関数は、射影変換後の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; }
/** * この関数は、射影変換パラメータを計算します。 * @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; }
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); }