/**
* この関数は、射影変換パラメータを計算します。
* @param i_vertex
* 変換元の４角系を定義する頂点配列。４頂点である必要がある。
* @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);
}
/**
* この関数は、射影変換後の２次元頂点座標をセットします。
* i_number_of_vertexは４である必要があります。
*/
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
* 変換元の４角系を定義する頂点配列。４頂点である必要がある。
* @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);
}
