public static bool IsRotMatrix(MatrixByArr rot)
{
// http://en.wikipedia.org/wiki/Rotation_matrix#Properties_of_a_rotation_matrix
if (rot.ColSize != 3)
{
return(false);
}
if (rot.RowSize != 3)
{
return(false);
}
// R^t = R^-1
// det R = 1
{ // (R-I)u = 0 where u is the null space of R-I
MatrixByArr RI = rot - LinAlg.Eye(3);
Vector[] eigvec;
double[] eigval;
NumericSolver.Eig(RI, out eigvec, out eigval);
double min_eigval = eigval.HAbs().Min();
if (min_eigval > 0.0000001)
{
return(false);
}
}
HDebug.ToDo("write selftest code");
return(true);
}
public static Vector GetRotAxis(MatrixByArr rot)
{
HDebug.ToDo("write selftest code");
// http://en.wikipedia.org/wiki/Rotation_matrix#Determining_the_axis
//
// Determining the axis
// Given a rotation matrix R, a vector u parallel to the rotation axis must satisfy
// R u = u
// since the rotation of u around the rotation axis must result in u. The equation
// above may be solved for u which is unique up to a scalar factor.
// Further, the equation may be rewritten
// R u = I u => (R-I) u = 0
// which shows that u is the null space of R-I. Viewed another way, u is an eigenvector
// of R corresponding to the eigenvalue λ=1(every rotation matrix must have this eigenvalue).
HDebug.Assert(IsRotMatrix(rot));
MatrixByArr RI = rot - LinAlg.Eye(3);
Vector[] eigvec;
double[] eigval;
NumericSolver.Eig(RI, out eigvec, out eigval);
int idx = eigval.HAbs().HIdxMin();
Vector axis = eigvec[idx];
return(axis);
}
public static Quaternion OptimalRotationWeighted(Vector[] p, Vector[] x, Vector up, Vector ux, double[] w)
{
MatrixByArr cov = CovarianceWeighted(p, x, up, ux, w);
double tr = cov[0, 0] + cov[1, 1] + cov[2, 2];
double A23 = cov[1, 2] - cov[2, 1];
double A31 = cov[2, 0] - cov[0, 2];
double A12 = cov[0, 1] - cov[1, 0];
MatrixByArr mat = new double[4, 4];
mat[0, 0] = tr;
mat[1, 0] = mat[0, 1] = A23;
mat[2, 0] = mat[0, 2] = A31;
mat[3, 0] = mat[0, 3] = A12;
cov = cov + cov.Tr();
cov = cov - LinAlg.Eye(3, tr);
mat[1, 1] = cov[0, 0];
mat[1, 2] = cov[1, 0];
mat[1, 3] = cov[2, 0];
mat[2, 1] = cov[0, 1];
mat[2, 2] = cov[1, 1];
mat[2, 3] = cov[2, 1];
mat[3, 1] = cov[0, 2];
mat[3, 2] = cov[1, 2];
mat[3, 3] = cov[2, 2];
LinAlg.Eigen eigen = new LinAlg.Eigen(mat);
double[,] eigenD = eigen.getD();
double[,] eigenV = eigen.getV();
int index_max = 0;
if (eigenD[1, 1] > eigenD[index_max, index_max])
{
index_max = 1;
}
if (eigenD[2, 2] > eigenD[index_max, index_max])
{
index_max = 2;
}
if (eigenD[3, 3] > eigenD[index_max, index_max])
{
index_max = 3;
}
Quaternion quater = new Quaternion(
eigenV[0, index_max],
eigenV[1, index_max],
eigenV[2, index_max],
eigenV[3, index_max]
);
return(quater);
}
public static void SelfTest()
{
if (_SelfTest == false)
{
return;
}
_SelfTest = false;
MatrixBySparseMatrix mat = new MatrixBySparseMatrix(4, 4, 2);
HDebug.AssertToleranceMatrix(0, mat - MatrixByArr.Zeros(4, 4));
mat[0, 0] = mat[1, 1] = mat[2, 2] = mat[3, 3] = 1;
HDebug.AssertToleranceMatrix(0, mat - LinAlg.Eye(4, 1));
mat[0, 0] = mat[1, 1] = mat[2, 2] = mat[3, 3] = 0;
HDebug.AssertToleranceMatrix(0, mat - MatrixByArr.Zeros(4, 4));
}
public static MatrixByArr InvSymm(MatrixByArr A)
{
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 = InvSymm(tA);
HDebug.AssertTolerance(0.0001, tB0 - tB1);
HDebug.AssertTolerance(0.0001, tI - tA * tB1);
HDebug.AssertTolerance(0.0001, tI - tB1 * tA);
}
HDebug.Assert(A.ColSize == A.RowSize);
//double[] eigval;
//double[,] eigvec;
bool success = false;//alglib.smatrixevd(A, A.ColSize, 1, false, out eigval, out eigvec);
if (success == false)
{
HDebug.Assert(false);
return(null);
}
HDebug.Assert(false);
return(null);
}
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);
}
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);
}
