/** * この関数は、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); }
/** * この関数は、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); }