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;
}
/**
* int check_rotation( double rot[2][3] )
* 2つのベクトル引数の調整をする？
* @param i_r
* @throws NyARException
*/
public static void checkRotation(NyARRotVector io_vec1, NyARRotVector io_vec2)
{
double w;
int f;
double vec10 = io_vec1.v1;
double vec11 = io_vec1.v2;
double vec12 = io_vec1.v3;
double vec20 = io_vec2.v1;
double vec21 = io_vec2.v2;
double vec22 = io_vec2.v3;
double vec30 = vec11 * vec22 - vec12 * vec21;
double vec31 = vec12 * vec20 - vec10 * vec22;
double vec32 = vec10 * vec21 - vec11 * vec20;
w = Math.Sqrt(vec30 * vec30 + vec31 * vec31 + vec32 * vec32);
if (w == 0.0)
{
//throw new NyARException();
vec30 = 0;
vec31 = 0;
vec32 = 0;
}
else
{
vec30 /= w;
vec31 /= w;
vec32 /= w;
}
double cb = vec10 * vec20 + vec11 * vec21 + vec12 * vec22;
if (cb < 0)
{
cb = -cb;//cb *= -1.0;
}
double ca = (Math.Sqrt(cb + 1.0) + Math.Sqrt(1.0 - cb)) * 0.5;
if (vec31 * vec10 - vec11 * vec30 != 0.0)
{
f = 0;
}
else
{
if (vec32 * vec10 - vec12 * vec30 != 0.0)
{
w = vec11; vec11 = vec12; vec12 = w;
w = vec31; vec31 = vec32; vec32 = w;
f = 1;
}
else
{
w = vec10; vec10 = vec12; vec12 = w;
w = vec30; vec30 = vec32; vec32 = w;
f = 2;
}
}
if (vec31 * vec10 - vec11 * vec30 == 0.0)
{
throw new NyARException();
}
double k1, k2, k3, k4;
double a, b, c, d;
double p1, q1, r1;
double p2, q2, r2;
double p3, q3, r3;
double p4, q4, r4;
k1 = (vec11 * vec32 - vec31 * vec12) / (vec31 * vec10 - vec11 * vec30);
k2 = (vec31 * ca) / (vec31 * vec10 - vec11 * vec30);
k3 = (vec10 * vec32 - vec30 * vec12) / (vec30 * vec11 - vec10 * vec31);
k4 = (vec30 * ca) / (vec30 * vec11 - vec10 * vec31);
a = k1 * k1 + k3 * k3 + 1;
b = k1 * k2 + k3 * k4;
c = k2 * k2 + k4 * k4 - 1;
d = b * b - a * c;
if (d < 0)
{
//throw new NyARException();
d = -d; // HACK: Manually setting Camera_Calibration values caused this
}
r1 = (-b + Math.Sqrt(d)) / a;
p1 = k1 * r1 + k2;
q1 = k3 * r1 + k4;
r2 = (-b - Math.Sqrt(d)) / a;
p2 = k1 * r2 + k2;
q2 = k3 * r2 + k4;
if (f == 1)
{
w = q1; q1 = r1; r1 = w;
w = q2; q2 = r2; r2 = w;
w = vec11; vec11 = vec12; vec12 = w;
w = vec31; vec31 = vec32; vec32 = w;
f = 0;
}
if (f == 2)
{
w = p1; p1 = r1; r1 = w;
w = p2; p2 = r2; r2 = w;
w = vec10; vec10 = vec12; vec12 = w;
w = vec30; vec30 = vec32; vec32 = w;
f = 0;
}
if (vec31 * vec20 - vec21 * vec30 != 0.0)
{
f = 0;
}
else
{
if (vec32 * vec20 - vec22 * vec30 != 0.0)
{
w = vec21; vec21 = vec22; vec22 = w;
w = vec31; vec31 = vec32; vec32 = w;
f = 1;
}
else
{
w = vec20; vec20 = vec22; vec22 = w;
w = vec30; vec30 = vec32; vec32 = w;
f = 2;
}
}
if (vec31 * vec20 - vec21 * vec30 == 0.0)
{
throw new NyARException();
}
k1 = (vec21 * vec32 - vec31 * vec22) / (vec31 * vec20 - vec21 * vec30);
k2 = (vec31 * ca) / (vec31 * vec20 - vec21 * vec30);
k3 = (vec20 * vec32 - vec30 * vec22) / (vec30 * vec21 - vec20 * vec31);
k4 = (vec30 * ca) / (vec30 * vec21 - vec20 * vec31);
a = k1 * k1 + k3 * k3 + 1;
b = k1 * k2 + k3 * k4;
c = k2 * k2 + k4 * k4 - 1;
d = b * b - a * c;
if (d < 0)
{
//throw new NyARException();
d = -d; // HACK: Manually setting Camera_Calibration values caused this
}
r3 = (-b + Math.Sqrt(d)) / a;
p3 = k1 * r3 + k2;
q3 = k3 * r3 + k4;
r4 = (-b - Math.Sqrt(d)) / a;
p4 = k1 * r4 + k2;
q4 = k3 * r4 + k4;
if (f == 1)
{
w = q3; q3 = r3; r3 = w;
w = q4; q4 = r4; r4 = w;
w = vec21; vec21 = vec22; vec22 = w;
w = vec31; vec31 = vec32; vec32 = w;
f = 0;
}
if (f == 2)
{
w = p3; p3 = r3; r3 = w;
w = p4; p4 = r4; r4 = w;
w = vec20; vec20 = vec22; vec22 = w;
w = vec30; vec30 = vec32; vec32 = w;
f = 0;
}
double e1 = p1 * p3 + q1 * q3 + r1 * r3;
if (e1 < 0)
{
e1 = -e1;
}
double e2 = p1 * p4 + q1 * q4 + r1 * r4;
if (e2 < 0)
{
e2 = -e2;
}
double e3 = p2 * p3 + q2 * q3 + r2 * r3;
if (e3 < 0)
{
e3 = -e3;
}
double e4 = p2 * p4 + q2 * q4 + r2 * r4;
if (e4 < 0)
{
e4 = -e4;
}
if (e1 < e2)
{
if (e1 < e3)
{
if (e1 < e4)
{
io_vec1.v1 = p1;
io_vec1.v2 = q1;
io_vec1.v3 = r1;
io_vec2.v1 = p3;
io_vec2.v2 = q3;
io_vec2.v3 = r3;
}
else
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p4;
io_vec2.v2 = q4;
io_vec2.v3 = r4;
}
}
else
{
if (e3 < e4)
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p3;
io_vec2.v2 = q3;
io_vec2.v3 = r3;
}
else
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p4;
io_vec2.v2 = q4;
io_vec2.v3 = r4;
}
}
}
else
{
if (e2 < e3)
{
if (e2 < e4)
{
io_vec1.v1 = p1;
io_vec1.v2 = q1;
io_vec1.v3 = r1;
io_vec2.v1 = p4;
io_vec2.v2 = q4;
io_vec2.v3 = r4;
}
else
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p4;
io_vec2.v2 = q4;
io_vec2.v3 = r4;
}
}
else
{
if (e3 < e4)
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p3;
io_vec2.v2 = q3;
io_vec2.v3 = r3;
}
else
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p4;
io_vec2.v2 = q4;
io_vec2.v3 = r4;
}
}
}
return;
}
/**
* この関数は、ARToolKitのcheck_rotationに相当する計算をします。
* 詳細は不明です。(2つのベクトルの関係を調整？)
* @
*/
public static bool checkRotation(NyARRotVector io_vec1, NyARRotVector io_vec2)
{
double w;
int f;
double vec10 = io_vec1.v1;
double vec11 = io_vec1.v2;
double vec12 = io_vec1.v3;
double vec20 = io_vec2.v1;
double vec21 = io_vec2.v2;
double vec22 = io_vec2.v3;
double vec30 = vec11 * vec22 - vec12 * vec21;
double vec31 = vec12 * vec20 - vec10 * vec22;
double vec32 = vec10 * vec21 - vec11 * vec20;
w = Math.Sqrt(vec30 * vec30 + vec31 * vec31 + vec32 * vec32);
if (w == 0.0)
{
return(false);
}
vec30 /= w;
vec31 /= w;
vec32 /= w;
double cb = vec10 * vec20 + vec11 * vec21 + vec12 * vec22;
if (cb < 0)
{
cb = -cb;//cb *= -1.0;
}
double ca = (Math.Sqrt(cb + 1.0) + Math.Sqrt(1.0 - cb)) * 0.5;
if (vec31 * vec10 - vec11 * vec30 != 0.0)
{
f = 0;
}
else
{
if (vec32 * vec10 - vec12 * vec30 != 0.0)
{
w = vec11; vec11 = vec12; vec12 = w;
w = vec31; vec31 = vec32; vec32 = w;
f = 1;
}
else
{
w = vec10; vec10 = vec12; vec12 = w;
w = vec30; vec30 = vec32; vec32 = w;
f = 2;
}
}
if (vec31 * vec10 - vec11 * vec30 == 0.0)
{
return(false);
}
double k1, k2, k3, k4;
double a, b, c, d;
double p1, q1, r1;
double p2, q2, r2;
double p3, q3, r3;
double p4, q4, r4;
k1 = (vec11 * vec32 - vec31 * vec12) / (vec31 * vec10 - vec11 * vec30);
k2 = (vec31 * ca) / (vec31 * vec10 - vec11 * vec30);
k3 = (vec10 * vec32 - vec30 * vec12) / (vec30 * vec11 - vec10 * vec31);
k4 = (vec30 * ca) / (vec30 * vec11 - vec10 * vec31);
a = k1 * k1 + k3 * k3 + 1;
b = k1 * k2 + k3 * k4;
c = k2 * k2 + k4 * k4 - 1;
d = b * b - a * c;
if (d < 0)
{
return(false);
}
r1 = (-b + Math.Sqrt(d)) / a;
p1 = k1 * r1 + k2;
q1 = k3 * r1 + k4;
r2 = (-b - Math.Sqrt(d)) / a;
p2 = k1 * r2 + k2;
q2 = k3 * r2 + k4;
if (f == 1)
{
w = q1; q1 = r1; r1 = w;
w = q2; q2 = r2; r2 = w;
w = vec11; vec11 = vec12; vec12 = w;
w = vec31; vec31 = vec32; vec32 = w;
f = 0;
}
if (f == 2)
{
w = p1; p1 = r1; r1 = w;
w = p2; p2 = r2; r2 = w;
w = vec10; vec10 = vec12; vec12 = w;
w = vec30; vec30 = vec32; vec32 = w;
f = 0;
}
if (vec31 * vec20 - vec21 * vec30 != 0.0)
{
f = 0;
}
else
{
if (vec32 * vec20 - vec22 * vec30 != 0.0)
{
w = vec21; vec21 = vec22; vec22 = w;
w = vec31; vec31 = vec32; vec32 = w;
f = 1;
}
else
{
w = vec20; vec20 = vec22; vec22 = w;
w = vec30; vec30 = vec32; vec32 = w;
f = 2;
}
}
if (vec31 * vec20 - vec21 * vec30 == 0.0)
{
return(false);
}
k1 = (vec21 * vec32 - vec31 * vec22) / (vec31 * vec20 - vec21 * vec30);
k2 = (vec31 * ca) / (vec31 * vec20 - vec21 * vec30);
k3 = (vec20 * vec32 - vec30 * vec22) / (vec30 * vec21 - vec20 * vec31);
k4 = (vec30 * ca) / (vec30 * vec21 - vec20 * vec31);
a = k1 * k1 + k3 * k3 + 1;
b = k1 * k2 + k3 * k4;
c = k2 * k2 + k4 * k4 - 1;
d = b * b - a * c;
if (d < 0)
{
return(false);
}
r3 = (-b + Math.Sqrt(d)) / a;
p3 = k1 * r3 + k2;
q3 = k3 * r3 + k4;
r4 = (-b - Math.Sqrt(d)) / a;
p4 = k1 * r4 + k2;
q4 = k3 * r4 + k4;
if (f == 1)
{
w = q3; q3 = r3; r3 = w;
w = q4; q4 = r4; r4 = w;
w = vec21; vec21 = vec22; vec22 = w;
w = vec31; vec31 = vec32; vec32 = w;
f = 0;
}
if (f == 2)
{
w = p3; p3 = r3; r3 = w;
w = p4; p4 = r4; r4 = w;
w = vec20; vec20 = vec22; vec22 = w;
w = vec30; vec30 = vec32; vec32 = w;
f = 0;
}
double e1 = p1 * p3 + q1 * q3 + r1 * r3;
if (e1 < 0)
{
e1 = -e1;
}
double e2 = p1 * p4 + q1 * q4 + r1 * r4;
if (e2 < 0)
{
e2 = -e2;
}
double e3 = p2 * p3 + q2 * q3 + r2 * r3;
if (e3 < 0)
{
e3 = -e3;
}
double e4 = p2 * p4 + q2 * q4 + r2 * r4;
if (e4 < 0)
{
e4 = -e4;
}
if (e1 < e2)
{
if (e1 < e3)
{
if (e1 < e4)
{
io_vec1.v1 = p1;
io_vec1.v2 = q1;
io_vec1.v3 = r1;
io_vec2.v1 = p3;
io_vec2.v2 = q3;
io_vec2.v3 = r3;
}
else
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p4;
io_vec2.v2 = q4;
io_vec2.v3 = r4;
}
}
else
{
if (e3 < e4)
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p3;
io_vec2.v2 = q3;
io_vec2.v3 = r3;
}
else
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p4;
io_vec2.v2 = q4;
io_vec2.v3 = r4;
}
}
}
else
{
if (e2 < e3)
{
if (e2 < e4)
{
io_vec1.v1 = p1;
io_vec1.v2 = q1;
io_vec1.v3 = r1;
io_vec2.v1 = p4;
io_vec2.v2 = q4;
io_vec2.v3 = r4;
}
else
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p4;
io_vec2.v2 = q4;
io_vec2.v3 = r4;
}
}
else
{
if (e3 < e4)
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p3;
io_vec2.v2 = q3;
io_vec2.v3 = r3;
}
else
{
io_vec1.v1 = p2;
io_vec1.v2 = q2;
io_vec1.v3 = r2;
io_vec2.v1 = p4;
io_vec2.v2 = q4;
io_vec2.v3 = r4;
}
}
}
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);
}