public static Vector FlattenRow(this MatrixByArr mat) /* <summary> flatten toward row direction </summary>*/ { return(mat.Flatten()); }
public static Vector FlattenCol(this MatrixByArr mat) /* <summary> flatten toward column direction </summary>*/ { return(mat.Tr().Flatten()); }
static public MatrixByArr Inv4x4(MatrixByArr _this) { if (HDebug.Selftest()) { /// >> A=[ 1,2,3,4 ; 5,7,9,10 ; 13,24,52,14 ; 12,43,73,28 ] /// >> invA = inv(A) /// -0.5599 0.2942 0.0557 -0.0529 /// -0.8416 0.2638 -0.1128 0.0824 /// 0.3886 -0.1754 0.0576 -0.0216 /// 0.5193 -0.0739 -0.0007 -0.0117 /// >> A*invA /// 1.0000 0.0000 0 -0.0000 /// -0.0000 1.0000 -0.0000 0.0000 /// 0 0.0000 1.0000 0.0000 /// 0 0.0000 0.0000 1.0000 MatrixByArr _A = new double[4, 4] { { 1, 2, 3, 4 }, { 5, 7, 9, 10 }, { 13, 24, 52, 14 }, { 12, 43, 73, 28 } }; MatrixByArr _invA_sol = new double[4, 4] { { -0.5599, 0.2942, 0.0557, -0.0529 } , { -0.8416, 0.2638, -0.1128, 0.0824 } , { 0.3886, -0.1754, 0.0576, -0.0216 } , { 0.5193, -0.0739, -0.0007, -0.0117 } }; MatrixByArr _invA = Inv4x4(_A); double err1 = (_invA - _invA_sol).HAbsMax(); HDebug.Assert(err1 < 0.0001); MatrixByArr _I = LinAlg.Eye(4); MatrixByArr _AinvA = _A * _invA; double err2 = (_I - _AinvA).HAbsMax(); HDebug.Assert(err2 < 0.000000001); MatrixByArr _invAA = _invA * _A; double err3 = (_I - _invAA).HAbsMax(); HDebug.Assert(err3 < 0.000000001); } ////////////////////////////////////////////////////////////////////////// // http://www.koders.com/cpp/fidFB7C4F93FDDB86E33EB66D177335BA81D86E58B5.aspx // Matrix.cpp // bool idMat4::InverseFastSelf( void ) ////////////////////////////////////////////////////////////////////////// // // 6*8+2*6 = 60 multiplications // // 2*1 = 2 divisions // idMat2 r0, r1, r2, r3; // float a, det, invDet; // float *mat = reinterpret_cast<float *>(this); // // // r0 = m0.Inverse(); // det = mat[0*4+0] * mat[1*4+1] - mat[0*4+1] * mat[1*4+0]; // // if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { // return false; // } // // invDet = 1.0f / det; // // r0[0][0] = mat[1*4+1] * invDet; // r0[0][1] = - mat[0*4+1] * invDet; // r0[1][0] = - mat[1*4+0] * invDet; // r0[1][1] = mat[0*4+0] * invDet; // // // r1 = r0 * m1; // r1[0][0] = r0[0][0] * mat[0*4+2] + r0[0][1] * mat[1*4+2]; // r1[0][1] = r0[0][0] * mat[0*4+3] + r0[0][1] * mat[1*4+3]; // r1[1][0] = r0[1][0] * mat[0*4+2] + r0[1][1] * mat[1*4+2]; // r1[1][1] = r0[1][0] * mat[0*4+3] + r0[1][1] * mat[1*4+3]; // // // r2 = m2 * r1; // r2[0][0] = mat[2*4+0] * r1[0][0] + mat[2*4+1] * r1[1][0]; // r2[0][1] = mat[2*4+0] * r1[0][1] + mat[2*4+1] * r1[1][1]; // r2[1][0] = mat[3*4+0] * r1[0][0] + mat[3*4+1] * r1[1][0]; // r2[1][1] = mat[3*4+0] * r1[0][1] + mat[3*4+1] * r1[1][1]; // // // r3 = r2 - m3; // r3[0][0] = r2[0][0] - mat[2*4+2]; // r3[0][1] = r2[0][1] - mat[2*4+3]; // r3[1][0] = r2[1][0] - mat[3*4+2]; // r3[1][1] = r2[1][1] - mat[3*4+3]; // // // r3.InverseSelf(); // det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0]; // // if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { // return false; // } // // invDet = 1.0f / det; // // a = r3[0][0]; // r3[0][0] = r3[1][1] * invDet; // r3[0][1] = - r3[0][1] * invDet; // r3[1][0] = - r3[1][0] * invDet; // r3[1][1] = a * invDet; // // // r2 = m2 * r0; // r2[0][0] = mat[2*4+0] * r0[0][0] + mat[2*4+1] * r0[1][0]; // r2[0][1] = mat[2*4+0] * r0[0][1] + mat[2*4+1] * r0[1][1]; // r2[1][0] = mat[3*4+0] * r0[0][0] + mat[3*4+1] * r0[1][0]; // r2[1][1] = mat[3*4+0] * r0[0][1] + mat[3*4+1] * r0[1][1]; // // // m2 = r3 * r2; // mat[2*4+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0]; // mat[2*4+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1]; // mat[3*4+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0]; // mat[3*4+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1]; // // // m0 = r0 - r1 * m2; // mat[0*4+0] = r0[0][0] - r1[0][0] * mat[2*4+0] - r1[0][1] * mat[3*4+0]; // mat[0*4+1] = r0[0][1] - r1[0][0] * mat[2*4+1] - r1[0][1] * mat[3*4+1]; // mat[1*4+0] = r0[1][0] - r1[1][0] * mat[2*4+0] - r1[1][1] * mat[3*4+0]; // mat[1*4+1] = r0[1][1] - r1[1][0] * mat[2*4+1] - r1[1][1] * mat[3*4+1]; // // // m1 = r1 * r3; // mat[0*4+2] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0]; // mat[0*4+3] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1]; // mat[1*4+2] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0]; // mat[1*4+3] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1]; // // // m3 = -r3; // mat[2*4+2] = -r3[0][0]; // mat[2*4+3] = -r3[0][1]; // mat[3*4+2] = -r3[1][0]; // mat[3*4+3] = -r3[1][1]; // // return true; if (_this.RowSize != 4 || _this.ColSize != 4) { return(null); } MatrixByArr mat = new MatrixByArr(_this); const double MATRIX_INVERSE_EPSILON = 0.000000001; // 6*8+2*6 = 60 multiplications // 2*1 = 2 divisions double det, invDet; // r0 = m0.Inverse(); det = mat[0, 0] * mat[1, 1] - mat[0, 1] * mat[1, 0]; if (Math.Abs(det) < MATRIX_INVERSE_EPSILON) { Debug.Assert(false); return(null); } invDet = 1.0f / det; double r0_00 = mat[1, 1] * invDet; double r0_01 = -mat[0, 1] * invDet; double r0_10 = -mat[1, 0] * invDet; double r0_11 = mat[0, 0] * invDet; // r1 = r0 * m1; double r1_00 = r0_00 * mat[0, 2] + r0_01 * mat[1, 2]; double r1_01 = r0_00 * mat[0, 3] + r0_01 * mat[1, 3]; double r1_10 = r0_10 * mat[0, 2] + r0_11 * mat[1, 2]; double r1_11 = r0_10 * mat[0, 3] + r0_11 * mat[1, 3]; // r2 = m2 * r1; double r2_00 = mat[2, 0] * r1_00 + mat[2, 1] * r1_10; double r2_01 = mat[2, 0] * r1_01 + mat[2, 1] * r1_11; double r2_10 = mat[3, 0] * r1_00 + mat[3, 1] * r1_10; double r2_11 = mat[3, 0] * r1_01 + mat[3, 1] * r1_11; // r3 = r2 - m3; double r3_00 = r2_00 - mat[2, 2]; double r3_01 = r2_01 - mat[2, 3]; double r3_10 = r2_10 - mat[3, 2]; double r3_11 = r2_11 - mat[3, 3]; // r3.InverseSelf(); det = r3_00 * r3_11 - r3_01 * r3_10; if (Math.Abs(det) < MATRIX_INVERSE_EPSILON) { Debug.Assert(false); return(null); } invDet = 1.0f / det; double r3_00_prv = r3_00; r3_00 = r3_11 * invDet; r3_01 = -r3_01 * invDet; r3_10 = -r3_10 * invDet; r3_11 = r3_00_prv * invDet; // r2 = m2 * r0; r2_00 = mat[2, 0] * r0_00 + mat[2, 1] * r0_10; r2_01 = mat[2, 0] * r0_01 + mat[2, 1] * r0_11; r2_10 = mat[3, 0] * r0_00 + mat[3, 1] * r0_10; r2_11 = mat[3, 0] * r0_01 + mat[3, 1] * r0_11; // m2 = r3 * r2; mat[2, 0] = r3_00 * r2_00 + r3_01 * r2_10; mat[2, 1] = r3_00 * r2_01 + r3_01 * r2_11; mat[3, 0] = r3_10 * r2_00 + r3_11 * r2_10; mat[3, 1] = r3_10 * r2_01 + r3_11 * r2_11; // m0 = r0 - r1 * m2; mat[0, 0] = r0_00 - r1_00 * mat[2, 0] - r1_01 * mat[3, 0]; mat[0, 1] = r0_01 - r1_00 * mat[2, 1] - r1_01 * mat[3, 1]; mat[1, 0] = r0_10 - r1_10 * mat[2, 0] - r1_11 * mat[3, 0]; mat[1, 1] = r0_11 - r1_10 * mat[2, 1] - r1_11 * mat[3, 1]; // m1 = r1 * r3; mat[0, 2] = r1_00 * r3_00 + r1_01 * r3_10; mat[0, 3] = r1_00 * r3_01 + r1_01 * r3_11; mat[1, 2] = r1_10 * r3_00 + r1_11 * r3_10; mat[1, 3] = r1_10 * r3_01 + r1_11 * r3_11; // m3 = -r3; mat[2, 2] = -r3_00; mat[2, 3] = -r3_01; mat[3, 2] = -r3_10; mat[3, 3] = -r3_11; return(mat); }
static public MatrixByArr Inv3x3(MatrixByArr _this) { if (HDebug.Selftest()) { MatrixByArr tA = new double[, ] { { 1, 2, 3 }, { 2, 9, 5 }, { 3, 5, 6 } }; MatrixByArr tB0 = new double[, ] { { -1.8125, -0.1875, 1.0625 }, { -0.1875, 0.1875, -0.0625 }, { 1.0625, -0.0625, -0.3125 } }; MatrixByArr tI = LinAlg.Eye(3); MatrixByArr tB1 = Inv3x3(tA); HDebug.AssertTolerance(0.0001, tB0 - tB1); HDebug.AssertTolerance(0.0001, tI - tA * tB1); HDebug.AssertTolerance(0.0001, tI - tB1 * tA); } // http://www.cvl.iis.u-tokyo.ac.jp/~miyazaki/tech/teche23.html if (_this.RowSize != 3 || _this.ColSize != 3) { return(null); } double a11 = _this[0, 0]; double a12 = _this[0, 1]; double a13 = _this[0, 2]; double a21 = _this[1, 0]; double a22 = _this[1, 1]; double a23 = _this[1, 2]; double a31 = _this[2, 0]; double a32 = _this[2, 1]; double a33 = _this[2, 2]; double detA = a11 * a22 * a33 + a21 * a32 * a13 + a31 * a12 * a23 - a11 * a32 * a23 - a31 * a22 * a13 - a21 * a12 * a33; MatrixByArr inv = new MatrixByArr(3, 3); inv[0, 0] = a22 * a33 - a23 * a32; inv[0, 1] = a13 * a32 - a12 * a33; inv[0, 2] = a12 * a23 - a13 * a22; inv[1, 0] = a23 * a31 - a21 * a33; inv[1, 1] = a11 * a33 - a13 * a31; inv[1, 2] = a13 * a21 - a11 * a23; inv[2, 0] = a21 * a32 - a22 * a31; inv[2, 1] = a12 * a31 - a11 * a32; inv[2, 2] = a11 * a22 - a12 * a21; inv /= detA; if (HDebug.IsDebuggerAttached) { MatrixByArr I33 = _this * inv; for (int r = 0; r < 3; r++) { for (int c = 0; c < 3; c++) { if (r == c) { Debug.Assert(Math.Abs(I33[r, c] - 1) < 0.00001); } else { Debug.Assert(Math.Abs(I33[r, c] - 0) < 0.00001); } } } I33 = inv * _this; for (int r = 0; r < 3; r++) { for (int c = 0; c < 3; c++) { if (r == c) { Debug.Assert(Math.Abs(I33[r, c] - 1) < 0.00001); } else { Debug.Assert(Math.Abs(I33[r, c] - 0) < 0.00001); } } } } return(inv); }
public static MatrixByArr Average(this IList <MatrixByArr> vecs) { MatrixByArr avg = vecs.Sum(); avg /= vecs.Count; return(avg); }
public static void Derivative2(Func <Vector, double> func, Vector x, double dx, ref MatrixByArr dy2) { // [df/dxdx, df/dxdy] // [df/dydx, df/dydy] // // df/dx = ( f(x+dx,y)-f(x-dx,y) ) // ----------------------- // (x+dx) - (x-dx) // = ( f(x+dx,y)-f(x-dx,y) ) / 2dx // = dfx(y) // // df/dxdy = ( dfx(y+dy) - dfx(y-dy) ) // ------------------------- // (y+dy) - (y-dy) // = [ (f(x+dx,y+dy)-f(x-dx,y+dy))/2dx - (f(x+dx,y-dy)-f(x-dx,y-dy))/2dx ] // --------------------------------------------------------------------- // 2dy // = [ f(x+dx,y+dy) - f(x-dx,y+dy) - f(x+dx,y-dy) + f(x-dx,y-dy) ] / (2dx * 2dy) // = [ f(x0 ) - f(x1 ) - f(x2 ) + f(x3 ) ] / (2dx * 2dy) // // df/dxdx = [ f((x+dx)+dx) - f((x+dx)-dx) - f((x-dx)+dx) + f((x-dx)-dx) ] / (2dx * 2dx) // = [ f(x+2dx) - f(x) - f(x) + f(x-2dx) ] / (2dx * 2dx) // int size = x.Size; HDebug.Assert(dy2.RowSize == size, dy2.ColSize == size); double dx2 = dx * dx; for (int i = 0; i < size; i++) { for (int j = 0; j < size; j++) { Vector x0 = x.Clone(); x0[i] += dx; x0[j] += dx; double y0 = func(x0); Vector x1 = x.Clone(); x1[i] -= dx; x1[j] += dx; double y1 = func(x1); Vector x2 = x.Clone(); x2[i] += dx; x2[j] -= dx; double y2 = func(x2); Vector x3 = x.Clone(); x3[i] -= dx; x3[j] -= dx; double y3 = func(x3); double d2f_didj = (y0 - y1 - y2 + y3) / (4 * dx2); dy2[i, j] = d2f_didj; } } }
public static Quaternion GetQuaternion(MatrixByArr mat, double scale) { // [a b c] [q0*q0 + q1*q1 - q2*q2 - q3*q3, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2) ] // s*[d e f] = [2*(q1*q2 + q0*q3), q0*q0 + q2*q2 - q1*q1 - q3*q3, 2*(q2*q3 - q0*q1) ] // [g h i] [2*(q1*q3 - q0*q2), 2*(q2*q3 + q0*q1), q0*q0 + q3*q3 - q1*q1 - q2*q2] // // q0*q0 + q1*q1 + q2*q2 + q3*q3 = 1 // s(a+e) = 2*(q0*q0 - q3*q3) // s(a+i) = 2*(q0*q0 - q2*q2) // s(e+i) = 2*(q0*q0 - q1*q1) // s(d+b) = 4 * q1*q2 // s(d-b) = 4 * q0*q3 // s(c+g) = 4 * q1*q3 // s(c-g) = 4 * q0*q2 // s(h+f) = 4 * q2*q3 // s(h-f) = 4 * q0*q1 // // if we assume 's == 1', // then (a+e+i) = 3*q0*q0 - (q1*q1 + q2*q2 + q3*q3) = 3*q0*q0 - (1-q0*q0) = 4*q0*q0 - 1 // q0 = sqrt((a + e + i + 4)/4) // q1 = (h - f)/(4 * q0) // q2 = (c - g)/(4 * q0) // q3 = (d - b)/(4 * q0) HDebug.Assert(scale == 1); HDebug.Assert(mat.ColSize == 3, mat.RowSize == 3); double q0 = Math.Sqrt((mat[0, 0] + mat[1, 1] + mat[2, 2] + 4) / 4); HDebug.Assert(double.IsNaN(q0) == false); HDebug.Assert(q0 != 0); double q1 = (mat[2, 1] - mat[1, 2]) / (4 * q0); double q2 = (mat[0, 2] - mat[2, 0]) / (4 * q0); double q3 = (mat[1, 0] - mat[0, 1]) / (4 * q0); HDebug.AssertSimilar((mat[0, 0] + mat[1, 1]), 2 * (q0 * q0 - q3 * q3), 0.001); HDebug.AssertSimilar((mat[0, 0] + mat[2, 2]), 2 * (q0 * q0 - q2 * q2), 0.001); HDebug.AssertSimilar((mat[1, 1] + mat[2, 2]), 2 * (q0 * q0 - q1 * q1), 0.001); HDebug.AssertSimilar((mat[1, 0] + mat[0, 1]), (4 * q1 * q2), 0.001); HDebug.AssertSimilar((mat[1, 0] - mat[0, 1]), (4 * q0 * q3), 0.001); HDebug.AssertSimilar((mat[0, 2] + mat[2, 0]), (4 * q1 * q3), 0.001); HDebug.AssertSimilar((mat[0, 2] - mat[2, 0]), (4 * q0 * q2), 0.001); HDebug.AssertSimilar((mat[2, 1] + mat[1, 2]), (4 * q2 * q3), 0.001); HDebug.AssertSimilar((mat[2, 1] - mat[1, 2]), (4 * q0 * q1), 0.001); Quaternion quater = new Quaternion(q0, q1, q2, q3); if (HDebug.IsDebuggerAttached) { MatrixByArr rotmat = quater.RotationMatrix; const double tolerance = 0.001; HDebug.Assert(Math.Abs(rotmat[0, 0] - mat[0, 0]) < tolerance); HDebug.Assert(Math.Abs(rotmat[0, 1] - mat[0, 1]) < tolerance); HDebug.Assert(Math.Abs(rotmat[0, 2] - mat[0, 2]) < tolerance); HDebug.Assert(Math.Abs(rotmat[1, 0] - mat[1, 0]) < tolerance); HDebug.Assert(Math.Abs(rotmat[1, 1] - mat[1, 1]) < tolerance); HDebug.Assert(Math.Abs(rotmat[1, 2] - mat[1, 2]) < tolerance); HDebug.Assert(Math.Abs(rotmat[2, 0] - mat[2, 0]) < tolerance); HDebug.Assert(Math.Abs(rotmat[2, 1] - mat[2, 1]) < tolerance); HDebug.Assert(Math.Abs(rotmat[2, 2] - mat[2, 2]) < tolerance); } return(quater); ////////////////////////////////////////////////////////////////////////// // (sqrt(3)*q0 + q1 + q2 + q3) * (sqrt(3)*q0 - q1 - q2 - q3) // = (3*q0*q0 - q1*q1 - q2*q2 - q3*q3) - 2*(q1*q2 + q2*q3 + q1*q3) // = (a+e+i) - (d+b + c+g + h+f)/2 // // (q0 - q1 + q2 - q3) * (a0 - q1 - q2 + q3) // = (q0*q0 + q1*q1 - q2*q2 - q3*q3) + 2*(q0*q1 + q2*q3) // = a + h/2 }
public static Matrix InvOfSubMatrixOfInv(this Matrix mat, IList <int> idxs, ILinAlg ila, string invtype, params object[] invopt) { if (HDebug.Selftest()) { MatrixByArr tH = new double[, ] { { 1, 2, 3, 4 } , { 2, 5, 6, 7 } , { 3, 6, 8, 9 } , { 4, 7, 9, 10 } }; MatrixByArr tInvH = new double[, ] { { 0.5, -0.5, -1.5, 1.5 } // = inv(tH) , { -0.5, 1.5, -1.5, 0.5 } , { -1.5, -1.5, 3.5, -1.5 } , { 1.5, 0.5, -1.5, 0.5 } }; MatrixByArr tInvH12 = new double[, ] { { 0.5, -0.5 } // = block matrix of tInvH , { -0.5, 1.5 } }; MatrixByArr tInvtInvH12 = new double[, ] { { 3, 1 } // = inv([ 0.5, -0.5]) , { 1, 1 } }; // [-0.5, 1.5] MatrixByArr ttInvtInvH12 = tH.InvOfSubMatrixOfInv(new int[] { 0, 1 }, ila, null).ToArray(); HDebug.AssertTolerance(0.00000001, tInvtInvH12 - ttInvtInvH12); } int ColSize = mat.ColSize; int RowSize = mat.RowSize; if (ColSize != RowSize) { HDebug.Assert(false); return(null); } if (idxs.Count != idxs.HToHashSet().Count) { HDebug.Assert(false); return(null); } List <int> idxs0 = idxs.ToList(); List <int> idxs1 = new List <int>(); for (int i = 0; i < ColSize; i++) { if (idxs.Contains(i) == false) { idxs1.Add(i); } } Matrix InvInvA; { Matrix A = mat.SubMatrix(idxs0, idxs0); Matrix B = mat.SubMatrix(idxs0, idxs1); // [A B] Matrix C = mat.SubMatrix(idxs1, idxs0); Matrix D = mat.SubMatrix(idxs1, idxs1); // [C D] /// http://en.wikipedia.org/wiki/Invertable_matrix#Blockwise_inversion /// /// M^-1 = [M_00 M_01] = [A B]-1 = [ (A - B D^-1 C)^-1 ... ] /// = [M_10 M_11] = [C D] [ ... ... ] /// /// (inv(M))_00 = inv(A - B inv(D) C) /// inv((inv(M))_00) = (A - B inv(D) C) var AA = ila.ToILMat(A); var BB = ila.ToILMat(B); var CC = ila.ToILMat(C); var DD = ila.ToILMat(D); var InvDD = ila.HInv(DD, invtype, invopt); var InvInvAA = AA - BB * InvDD * CC; //var xx = BB * InvDD * CC; InvInvA = InvInvAA.ToArray(); AA.Dispose(); BB.Dispose(); CC.Dispose(); DD.Dispose(); InvDD.Dispose(); InvInvAA.Dispose(); } GC.Collect(); return(InvInvA); }
public static Tuple <MatrixByArr, Vector> Eig(MatrixByArr A) { if (HDebug.Selftest()) { MatrixByArr tA = new double[, ] { { 1, 2, 3 }, { 2, 9, 5 }, { 3, 5, 6 } }; MatrixByArr tV = new double[, ] { { -0.8879, 0.3782, 0.2618 }, { -0.0539, -0.6508, 0.7573 }, { 0.4568, 0.6583, 0.5983 } }; Vector tD = new double[] { -0.4219, 2.7803, 13.6416 }; Tuple <MatrixByArr, Vector> tVD = Eig(tA); Vector tV0 = tVD.Item1.GetColVector(0); double tD0 = tVD.Item2[0]; Vector tV1 = tVD.Item1.GetColVector(1); double tD1 = tVD.Item2[1]; Vector tV2 = tVD.Item1.GetColVector(2); double tD2 = tVD.Item2[2]; HDebug.AssertTolerance(0.00000001, 1 - LinAlg.VtV(tV0, tV0)); HDebug.AssertTolerance(0.00000001, 1 - LinAlg.VtV(tV1, tV1)); HDebug.AssertTolerance(0.00000001, 1 - LinAlg.VtV(tV2, tV2)); MatrixByArr tAA = tVD.Item1 * LinAlg.Diag(tVD.Item2) * tVD.Item1.Tr(); HDebug.AssertTolerance(0.00000001, tA - tAA); //HDebug.AssertTolerance(0.0001, VD.Item1-tV); HDebug.AssertTolerance(0.0001, tVD.Item2 - tD); } HDebug.Assert(A.ColSize == A.RowSize); double[] eigval; double[,] eigvec; #region bool alglib.smatrixevd(double[,] a, int n, int zneeded, bool isupper, out double[] d, out double[,] z) /// Finding the eigenvalues and eigenvectors of a symmetric matrix /// /// The algorithm finds eigen pairs of a symmetric matrix by reducing it to /// tridiagonal form and using the QL/QR algorithm. /// /// Input parameters: /// A - symmetric matrix which is given by its upper or lower /// triangular part. /// Array whose indexes range within [0..N-1, 0..N-1]. /// N - size of matrix A. /// ZNeeded - flag controlling whether the eigenvectors are needed or not. /// If ZNeeded is equal to: /// * 0, the eigenvectors are not returned; /// * 1, the eigenvectors are returned. /// IsUpper - storage format. /// /// Output parameters: /// D - eigenvalues in ascending order. /// Array whose index ranges within [0..N-1]. /// Z - if ZNeeded is equal to: /// * 0, Z hasn’t changed; /// * 1, Z contains the eigenvectors. /// Array whose indexes range within [0..N-1, 0..N-1]. /// The eigenvectors are stored in the matrix columns. /// /// Result: /// True, if the algorithm has converged. /// False, if the algorithm hasn't converged (rare case). /// /// -- ALGLIB -- /// Copyright 2005-2008 by Bochkanov Sergey /// /// public static bool alglib.smatrixevd( /// double[,] a, /// int n, /// int zneeded, /// bool isupper, /// out double[] d, /// out double[,] z) #endregion bool success = alglib.smatrixevd(A, A.ColSize, 1, false, out eigval, out eigvec); if (success == false) { HDebug.Assert(false); return(null); } return(new Tuple <MatrixByArr, Vector>(eigvec, eigval)); }
protected static string _ToString(string format, object obj) { try{ if (obj == null) { return(""); } string name = obj.GetType().FullName; if (obj is System.Runtime.CompilerServices.ITuple) { return(_ToString(format, obj as System.Runtime.CompilerServices.ITuple)); } if (obj is System.Collections.IDictionary) { return(_ToString(format, obj as System.Collections.IDictionary)); } //if(name==typeof(Tuple<DoubleVector3,Tuple<double,DoubleVector3>[]> ).FullName) return _ToString (format, (Tuple<DoubleVector3,Tuple<double,DoubleVector3>[]> )obj); //if(name==typeof(Tuple<double,DoubleVector3>[] ).FullName) return _ToString (format, (Tuple<double,DoubleVector3>[] )obj); //if(name==typeof(Tuple<double,DoubleVector3> ).FullName) return _ToString (format, (Tuple<double,DoubleVector3> )obj); //if(name==typeof(Tuple<DoubleVector3,Tuple<DoubleVector3,DoubleVector3>>).FullName) return _ToString (format, (Tuple<DoubleVector3,Tuple<DoubleVector3,DoubleVector3>>)obj); //if(name==typeof(Tuple<DoubleVector3,DoubleVector3> ).FullName) return _ToString (format, (Tuple<DoubleVector3,DoubleVector3> )obj); //if(name==typeof(Tuple<double[],double[]> ).FullName) return _ToString (format, (Tuple<double[],double[]> )obj); //if(name==typeof(Tuple<double,double> ).FullName) return _ToString (format, (Tuple<double,double> )obj); //if(name==typeof(Tuple<double,int> ).FullName) return _ToString (format, (Tuple<double,int> )obj); //if(name==typeof(Tuple<int,double> ).FullName) return _ToString (format, (Tuple<int,double> )obj); //if(name==typeof(Tuple<double,Vector> ).FullName) return _ToString (format, (Tuple<double,Vector> )obj); //if(name==typeof(Tuple<Vector,Vector> ).FullName) return _ToString (format, (Tuple<Vector,Vector> )obj); if (name == typeof(List <Tuple <double, int> >).FullName) { return(_ToString(format, (List <Tuple <double, int> >)obj)); } if (name == typeof(List <Tuple <int, double> >).FullName) { return(_ToString(format, (List <Tuple <int, double> >)obj)); } if (name == typeof(List <Tuple <double, double> >).FullName) { return(_ToString(format, (List <Tuple <double, double> >)obj)); } if (name == typeof(List <double>).FullName) { return(_ToString(format, (List <double>)obj)); } if (name == typeof(List <Vector>).FullName) { return(_ToString(format, (List <Vector>)obj)); } //if(name==typeof(List<DoubleVector3> ).FullName) return _ToString (format, (List<DoubleVector3> )obj); if (name == typeof(List <double[]>).FullName) { return(_ToString(format, (List <double[]>)obj)); } if (name == typeof(List <double[, ]>).FullName) { return(_ToString(format, (List <double[, ]>)obj)); } if (name == typeof(MatrixByArr[]).FullName) { return(_ToString <MatrixByArr> (format, (MatrixByArr[] )obj)); } if (name == typeof(Vector[]).FullName) { return(_ToString <Vector> (format, (Vector[] )obj)); } if (name == typeof(string[]).FullName) { return(_ToString <string> (format, (string[] )obj)); } if (name == typeof(string[, ]).FullName) { return(_ToString(format, (string[, ])obj)); } if (name == typeof(double[]).FullName) { return(_ToString(format, (double[] )obj)); } if (name == typeof(int[]).FullName) { return(_ToString(format, (int[] )obj)); } if (name == typeof(double[, ]).FullName) { return(_ToString(format, (double[, ])obj)); } if (name == typeof(double[, , ]).FullName) { return(_ToString(format, (double[, , ])obj)); } //if(name==typeof(DoubleVector3[] ).FullName) return _ToString (format, (DoubleVector3[] )obj); if (name == typeof(double[][]).FullName) { return(_ToString <double[]> (format, (double[][] )obj)); } if (name == typeof(double[][, ]).FullName) { return(_ToString(format, new List <double[, ]>((double[][, ])obj))); } if (name == typeof(double[, ][]).FullName) { return(_ToString(format, (double[, ][])obj)); } if (name == typeof(List <int>).FullName) { return(_ToString <int> (format, (IEnumerable <int>)obj)); } if (name == typeof(List <string>).FullName) { return(_ToString <string> (format, (IEnumerable <string>)obj)); } if (name == typeof(List <string[]>).FullName) { return(_ToString <string[]> (format, (IEnumerable <string[]>)obj)); } if (name == typeof(List <List <int> >).FullName) { return(_ToString <List <int> > (format, (IEnumerable <List <int> >)obj)); } if (name == typeof(List <List <double> >).FullName) { return(_ToString <List <double> > (format, (IEnumerable <List <double> >)obj)); } if (name == typeof(List <List <string> >).FullName) { return(_ToString <List <string> > (format, (IEnumerable <List <string> >)obj)); } if (name == typeof(List <List <Vector> >).FullName) { return(_ToString <List <Vector> > (format, (IEnumerable <List <Vector> >)obj)); } //if(name==typeof(Dictionary<double,double> ).FullName) return _ToString<double,double> (format, (Dictionary<double,double> )obj); //if(name==typeof(KeyValuePair<double,Vector[]> ).FullName) return _ToString<double,Vector[]> (format, (KeyValuePair<double,Vector[]> )obj); //if(name==typeof(KeyValuePair<double,double[]> ).FullName) return _ToString<double,double[]> (format, (KeyValuePair<double,double[]> )obj); //if(name==typeof(KeyValuePair<double,string> ).FullName) return _ToString<double,string> (format, (KeyValuePair<double,string> )obj); //if(name==typeof(KeyValuePair<int, Tuple<Vector, Vector>> ).FullName) return _ToString<int, Tuple<Vector,Vector>>(format, (KeyValuePair<int, Tuple<Vector, Vector>> )obj); //if(name==typeof(Dictionary<double,double[]> ).FullName) return _ToString<double,double[]> (format, (Dictionary<double,double[]> )obj); //if(name==typeof(Tuple<int , int >).FullName) { var val=(Tuple<int , int >)obj; return _ToString(format, val.Item1, val.Item2); } //if(name==typeof(Tuple<int , double >).FullName) { var val=(Tuple<int , double >)obj; return _ToString(format, val.Item1, val.Item2); } //if(name==typeof(Tuple<int , double, double>).FullName) { var val=(Tuple<int , double, double>)obj; return _ToString(format, val.Item1, val.Item2, val.Item3); } //if(name==typeof(Tuple<double, double >).FullName) { var val=(Tuple<double, double >)obj; return _ToString(format, val.Item1, val.Item2); } //if(name==typeof(Tuple<double, double, double>).FullName) { var val=(Tuple<double, double, double>)obj; return _ToString(format, val.Item1, val.Item2, val.Item3); } //if(name==typeof(Tuple<double, int, int, int >).FullName) { var val=(Tuple<double, int, int, int >)obj; return _ToString(format, val.Item1, val.Item2, val.Item3, val.Item4); } //if(name==typeof(Tuple<int ,Vector >).FullName) { var val=(Tuple<int ,Vector >)obj; return _ToString(format, val.Item1, val.Item2); } //if(name==typeof(Tuple<string,string >).FullName) { var val=(Tuple<string,string >)obj; return _ToString(format, val.Item1, val.Item2); } //if(name==typeof(Tuple<string,int >).FullName) { var val=(Tuple<string,int >)obj; return _ToString(format, val.Item1, val.Item2); } if (name == typeof(KeyValuePair <double, double>).FullName) { KeyValuePair <double, double> kvp = (KeyValuePair <double, double>)obj; return(_ToString(format, kvp.Key, kvp.Value)); } //if(name == typeof(MatrixDouble).FullName) //{ // MatrixDouble data = (MatrixDouble)obj; // return _ToString(data.ToArray()); //} if (name == typeof(MatrixByArr).FullName) { MatrixByArr data = (MatrixByArr)obj; return(_ToString(format, data.ToArray())); } if (name == typeof(Vector).FullName) { Vector data = (Vector)obj; return(_ToString(format, data._data)); } //if(name == typeof(DoubleVector3).FullName) //{ // DoubleVector3 data = (DoubleVector3)obj; // return _ToString(data.v0, data.v1, data.v2); //} //if(name == typeof(DoubleMatrix3).FullName) //{ // DoubleMatrix3 data = (DoubleMatrix3)obj; // return _ToString(data.ToArray()); //} if (name == typeof(int).FullName) { if (format == null) { return(obj.ToString()); } else { return(string.Format(format, (int)obj)); } } if (name == typeof(double).FullName) { string text; if (format == null) { text = ((double)obj).ToString(); } else { text = string.Format(format, (double)obj); } if (text.Contains("E") || text.Contains("e")) { text = text.Replace("e", "*10^"); text = text.Replace("E", "*10^"); } return(text); } if (name == typeof(string).FullName) { string str = obj.ToString(); str = str.Replace("\\", "\\\\"); str = "\"" + str + "\""; return(str); } if (name == typeof(object[]).FullName) { return(_ToString(format, (object[])obj)); } HDebug.Assert(false); HDebug.Break(); return(null); } catch (Exception) { HDebug.Break(); return(null); } }
static public MatrixByArr Inverse3x3(MatrixByArr _this) { // http://www.cvl.iis.u-tokyo.ac.jp/~miyazaki/tech/teche23.html if (_this.RowSize != 3 || _this.ColSize != 3) { return(null); } double a11 = _this[0, 0]; double a12 = _this[0, 1]; double a13 = _this[0, 2]; double a21 = _this[1, 0]; double a22 = _this[1, 1]; double a23 = _this[1, 2]; double a31 = _this[2, 0]; double a32 = _this[2, 1]; double a33 = _this[2, 2]; double detA = a11 * a22 * a33 + a21 * a32 * a13 + a31 * a12 * a23 - a11 * a32 * a23 - a31 * a22 * a13 - a21 * a12 * a33; MatrixByArr inv = new MatrixByArr(3, 3); inv[0, 0] = a22 * a33 - a23 * a32; inv[0, 1] = a13 * a32 - a12 * a33; inv[0, 2] = a12 * a23 - a13 * a22; inv[1, 0] = a23 * a31 - a21 * a33; inv[1, 1] = a11 * a33 - a13 * a31; inv[1, 2] = a13 * a21 - a11 * a23; inv[2, 0] = a21 * a32 - a22 * a31; inv[2, 1] = a12 * a31 - a11 * a32; inv[2, 2] = a11 * a22 - a12 * a21; inv /= detA; if (HDebug.IsDebuggerAttached) { MatrixByArr I33 = _this * inv; for (int r = 0; r < 3; r++) { for (int c = 0; c < 3; c++) { if (r == c) { HDebug.Assert(Math.Abs(I33[r, c] - 1) < 0.00001); } else { HDebug.Assert(Math.Abs(I33[r, c] - 0) < 0.00001); } } } I33 = inv * _this; for (int r = 0; r < 3; r++) { for (int c = 0; c < 3; c++) { if (r == c) { HDebug.Assert(Math.Abs(I33[r, c] - 1) < 0.00001); } else { HDebug.Assert(Math.Abs(I33[r, c] - 0) < 0.00001); } } } } return(inv); }
static public MatrixByArr Inverse4x4(MatrixByArr _this) { ////////////////////////////////////////////////////////////////////////// // http://www.koders.com/cpp/fidFB7C4F93FDDB86E33EB66D177335BA81D86E58B5.aspx // Matrix.cpp // bool idMat4::InverseFastSelf( void ) ////////////////////////////////////////////////////////////////////////// // // 6*8+2*6 = 60 multiplications // // 2*1 = 2 divisions // idMat2 r0, r1, r2, r3; // float a, det, invDet; // float *mat = reinterpret_cast<float *>(this); // // // r0 = m0.Inverse(); // det = mat[0*4+0] * mat[1*4+1] - mat[0*4+1] * mat[1*4+0]; // // if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { // return false; // } // // invDet = 1.0f / det; // // r0[0][0] = mat[1*4+1] * invDet; // r0[0][1] = - mat[0*4+1] * invDet; // r0[1][0] = - mat[1*4+0] * invDet; // r0[1][1] = mat[0*4+0] * invDet; // // // r1 = r0 * m1; // r1[0][0] = r0[0][0] * mat[0*4+2] + r0[0][1] * mat[1*4+2]; // r1[0][1] = r0[0][0] * mat[0*4+3] + r0[0][1] * mat[1*4+3]; // r1[1][0] = r0[1][0] * mat[0*4+2] + r0[1][1] * mat[1*4+2]; // r1[1][1] = r0[1][0] * mat[0*4+3] + r0[1][1] * mat[1*4+3]; // // // r2 = m2 * r1; // r2[0][0] = mat[2*4+0] * r1[0][0] + mat[2*4+1] * r1[1][0]; // r2[0][1] = mat[2*4+0] * r1[0][1] + mat[2*4+1] * r1[1][1]; // r2[1][0] = mat[3*4+0] * r1[0][0] + mat[3*4+1] * r1[1][0]; // r2[1][1] = mat[3*4+0] * r1[0][1] + mat[3*4+1] * r1[1][1]; // // // r3 = r2 - m3; // r3[0][0] = r2[0][0] - mat[2*4+2]; // r3[0][1] = r2[0][1] - mat[2*4+3]; // r3[1][0] = r2[1][0] - mat[3*4+2]; // r3[1][1] = r2[1][1] - mat[3*4+3]; // // // r3.InverseSelf(); // det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0]; // // if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) { // return false; // } // // invDet = 1.0f / det; // // a = r3[0][0]; // r3[0][0] = r3[1][1] * invDet; // r3[0][1] = - r3[0][1] * invDet; // r3[1][0] = - r3[1][0] * invDet; // r3[1][1] = a * invDet; // // // r2 = m2 * r0; // r2[0][0] = mat[2*4+0] * r0[0][0] + mat[2*4+1] * r0[1][0]; // r2[0][1] = mat[2*4+0] * r0[0][1] + mat[2*4+1] * r0[1][1]; // r2[1][0] = mat[3*4+0] * r0[0][0] + mat[3*4+1] * r0[1][0]; // r2[1][1] = mat[3*4+0] * r0[0][1] + mat[3*4+1] * r0[1][1]; // // // m2 = r3 * r2; // mat[2*4+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0]; // mat[2*4+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1]; // mat[3*4+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0]; // mat[3*4+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1]; // // // m0 = r0 - r1 * m2; // mat[0*4+0] = r0[0][0] - r1[0][0] * mat[2*4+0] - r1[0][1] * mat[3*4+0]; // mat[0*4+1] = r0[0][1] - r1[0][0] * mat[2*4+1] - r1[0][1] * mat[3*4+1]; // mat[1*4+0] = r0[1][0] - r1[1][0] * mat[2*4+0] - r1[1][1] * mat[3*4+0]; // mat[1*4+1] = r0[1][1] - r1[1][0] * mat[2*4+1] - r1[1][1] * mat[3*4+1]; // // // m1 = r1 * r3; // mat[0*4+2] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0]; // mat[0*4+3] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1]; // mat[1*4+2] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0]; // mat[1*4+3] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1]; // // // m3 = -r3; // mat[2*4+2] = -r3[0][0]; // mat[2*4+3] = -r3[0][1]; // mat[3*4+2] = -r3[1][0]; // mat[3*4+3] = -r3[1][1]; // // return true; if (_this.RowSize != 4 || _this.ColSize != 4) { return(null); } double[,] mat = new double[, ] { { _this[0, 0], _this[0, 1], _this[0, 2], _this[0, 3] }, { _this[1, 0], _this[1, 1], _this[1, 2], _this[1, 3] }, { _this[2, 0], _this[2, 1], _this[2, 2], _this[2, 3] }, { _this[3, 0], _this[3, 1], _this[3, 2], _this[3, 3] }, }; const double MATRIX_INVERSE_EPSILON = 0.000000001; // 6*8+2*6 = 60 multiplications // 2*1 = 2 divisions double det, invDet; // r0 = m0.Inverse(); det = mat[0, 0] * mat[1, 1] - mat[0, 1] * mat[1, 0]; if (Math.Abs(det) < MATRIX_INVERSE_EPSILON) { HDebug.Assert(false); return(null); } invDet = 1.0f / det; double r0_00 = mat[1, 1] * invDet; double r0_01 = -mat[0, 1] * invDet; double r0_10 = -mat[1, 0] * invDet; double r0_11 = mat[0, 0] * invDet; // r1 = r0 * m1; double r1_00 = r0_00 * mat[0, 2] + r0_01 * mat[1, 2]; double r1_01 = r0_00 * mat[0, 3] + r0_01 * mat[1, 3]; double r1_10 = r0_10 * mat[0, 2] + r0_11 * mat[1, 2]; double r1_11 = r0_10 * mat[0, 3] + r0_11 * mat[1, 3]; // r2 = m2 * r1; double r2_00 = mat[2, 0] * r1_00 + mat[2, 1] * r1_10; double r2_01 = mat[2, 0] * r1_01 + mat[2, 1] * r1_11; double r2_10 = mat[3, 0] * r1_00 + mat[3, 1] * r1_10; double r2_11 = mat[3, 0] * r1_01 + mat[3, 1] * r1_11; // r3 = r2 - m3; double r3_00 = r2_00 - mat[2, 2]; double r3_01 = r2_01 - mat[2, 3]; double r3_10 = r2_10 - mat[3, 2]; double r3_11 = r2_11 - mat[3, 3]; // r3.InverseSelf(); det = r3_00 * r3_11 - r3_01 * r3_10; if (Math.Abs(det) < MATRIX_INVERSE_EPSILON) { HDebug.Assert(false); return(null); } invDet = 1.0f / det; double r3_00_prv = r3_00; r3_00 = r3_11 * invDet; r3_01 = -r3_01 * invDet; r3_10 = -r3_10 * invDet; r3_11 = r3_00_prv * invDet; // r2 = m2 * r0; r2_00 = mat[2, 0] * r0_00 + mat[2, 1] * r0_10; r2_01 = mat[2, 0] * r0_01 + mat[2, 1] * r0_11; r2_10 = mat[3, 0] * r0_00 + mat[3, 1] * r0_10; r2_11 = mat[3, 0] * r0_01 + mat[3, 1] * r0_11; // m2 = r3 * r2; mat[2, 0] = r3_00 * r2_00 + r3_01 * r2_10; mat[2, 1] = r3_00 * r2_01 + r3_01 * r2_11; mat[3, 0] = r3_10 * r2_00 + r3_11 * r2_10; mat[3, 1] = r3_10 * r2_01 + r3_11 * r2_11; // m0 = r0 - r1 * m2; mat[0, 0] = r0_00 - r1_00 * mat[2, 0] - r1_01 * mat[3, 0]; mat[0, 1] = r0_01 - r1_00 * mat[2, 1] - r1_01 * mat[3, 1]; mat[1, 0] = r0_10 - r1_10 * mat[2, 0] - r1_11 * mat[3, 0]; mat[1, 1] = r0_11 - r1_10 * mat[2, 1] - r1_11 * mat[3, 1]; // m1 = r1 * r3; mat[0, 2] = r1_00 * r3_00 + r1_01 * r3_10; mat[0, 3] = r1_00 * r3_01 + r1_01 * r3_11; mat[1, 2] = r1_10 * r3_00 + r1_11 * r3_10; mat[1, 3] = r1_10 * r3_01 + r1_11 * r3_11; // m3 = -r3; mat[2, 2] = -r3_00; mat[2, 3] = -r3_01; mat[3, 2] = -r3_10; mat[3, 3] = -r3_11; return(mat); }
public static double[] LeastSquare (double[] As, double[] bs , double[] mean_square_err = null ) { /// => A x = b /// /// => [A1, 1] * [ x ] = [b1] /// [A2, 1] [ t ] [b2] /// [A3, 1] [b3] /// [... ] [..] /// /// => At A xt = At b /// /// => [A1 A2 A3] * [A1, 1] * [ x ] = [A1 A2 A3] * [b1] /// [ 1 1 1] [A2, 1] [ t ] [ 1 1 1] [b2] /// [A3, 1] [b3] /// [... ] [..] /// /// => [A1^2 + A2^2 + A3^2 + ..., A1+A2+A3+...] * [ x ] = [A1*b1 + A2*b2 + A3*b3 + ...] /// [A1+A2+A3+... , 1+1+1+... ] [ t ] = [b1+b2+b3+... ] /// /// => [sumA2, sumA ] * [ x ] = [sumAb] /// [sumA , sum1 ] [ t ] = [sumb ] /// /// => AA * xt = Ab /// => xt = inv(AA) * Ab double[,] AA = new double[2, 2]; double[] Ab = new double[2]; int n = As.Length; HDebug.Assert(n == As.Length); HDebug.Assert(n == bs.Length); for (int i = 0; i < n; i++) { double ai = As[i]; double bi = bs[i]; double Ai2 = ai * ai; AA[0, 0] += Ai2; AA[0, 1] += ai; AA[1, 0] += ai; AA[1, 1] += 1; Ab[0] += ai * bi; } MatrixByArr invA = LinAlg.Inv2x2(AA); Vector xt = LinAlg.MV(invA, Ab); if (mean_square_err != null) { HDebug.Assert(mean_square_err.Length == 1); double err2 = 0; double x = xt[0]; double t = xt[1]; for (int i = 0; i < n; i++) { double nbi = As[i] * x + t; double erri = (nbi - bs[i]); err2 += erri * erri; } mean_square_err[0] = err2 / n; } return(xt); }
public static Vector OptimalTransWeighted(Vector up, Vector ux, MatrixByArr qR, double[] w) { return(ux - qR * up); }
public static Trans3 OptimalTransformWeighted(IList <Vector> source, IList <Vector> target, IList <double> weight , double?rotateRatio = null ) { if (OptimalTransformWeighted_selftest) #region selftest { OptimalTransformWeighted_selftest = false; { Vector[] _source = new Vector[] { new double[] { 0, 0, 0 }, new double[] { 1, 0, 0 }, new double[] { 0, 1, 0 }, new double[] { 0, 0, 1 }, }; Vector[] _target = new Vector[] { new double[] { 0, 0, 0 }, new double[] { 0, 1, 0 }, new double[] { 0, 0, 1 }, new double[] { 1, 0, 0 }, }; double[] _weight = new double[] { 1, 1, 1, 1 }; Trans3 _trans = OptimalTransformWeighted(_source, _target, _weight, null); Vector[] _moved = _trans.GetTransformed(_source); double _err2 = 0; for (int i = 0; i < _source.Length; i++) { _err2 = (_target[i] - _moved[i]).Dist2; } HDebug.Assert(_err2 < 0.00000001); } { Vector[] _source = new Vector[] { new double[] { 1, 0, 0 }, new double[] { 0, 1, 0 }, new double[] { 0, 0, 1 }, }; Vector[] _target = new Vector[] { new double[] { 0, 1, 0 }, new double[] { 0, 0, 1 }, new double[] { 1, 0, 0 }, }; double[] _weight = new double[] { 1, 1, 1 }; Trans3 _trans = OptimalTransformWeighted(_source, _target, _weight, null); Vector[] _moved = _trans.GetTransformed(_source); double _err2 = 0; for (int i = 0; i < _source.Length; i++) { _err2 = (_target[i] - _moved[i]).Dist2; } HDebug.Assert(_err2 < 0.00000001); } } #endregion if (HDebug.IsDebuggerAttached) { foreach (double _w in weight) { HDebug.Assert(_w >= 0); } } Vector[] p = source.ToArray(); Vector[] x = target.ToArray(); double[] w = weight.ToArray(); HDebug.Assert(p.Length == x.Length); HDebug.Assert(w.Length == x.Length); Vector up = p.MeanWeighted(w); Vector ux = x.MeanWeighted(w); Quaternion quater = OptimalRotationWeighted(p, x, up, ux, w); if (rotateRatio != null) { double r = quater.RotationAngle; r = r % (2 * Math.PI); r = (r > Math.PI) ? (r - 2 * Math.PI) : r; r = r / 2; quater = new Quaternion(quater.RotationAxis, r); } MatrixByArr qR = quater.RotationMatrix; Vector qT = OptimalTransWeighted(up, ux, qR, w); return(new Trans3(qT, 1, quater)); }