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));
}
