public virtual void initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex)
{
NyARRotVector vec1 = this.__initRot_vec1;
NyARRotVector vec2 = this.__initRot_vec2;
//向かい合った辺から、２本のベクトルを計算
//軸１
vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);
vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);
//軸２
vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);
vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);
//回転の最適化？
NyARRotVector.checkRotation(vec1, vec2);
this.m00 = vec1.v1;
this.m10 = vec1.v2;
this.m20 = vec1.v3;
this.m01 = vec2.v1;
this.m11 = vec2.v2;
this.m21 = vec2.v3;
//最後の軸を計算
double w02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2;
double w12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3;
double w22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1;
double w = Math.Sqrt(w02 * w02 + w12 * w12 + w22 * w22);
this.m02 = w02 / w;
this.m12 = w12 / w;
this.m22 = w22 / w;
//Matrixからangleをロード
return;
}
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);
}