public Tuple <ILinAlgMat, double[]> EigSymm(ILinAlgMat A)
{
Func <ILinAlgMat, Tuple <ILinAlgMat, double[]> > func = _EigSymm;
if (HDebug.Selftest(func))
{
ILinAlgMat tA = ToILMat(new double[, ] {
{ 1, 2, 3 }, { 2, 4, 5 }, { 3, 5, 6 }
});
var tVD = EigSymm(tA);
ILinAlgMat tV = tVD.Item1;
double[] tD = tVD.Item2;
ILinAlgMat tDD = Diag(ToILMat(tD));
ILinAlgMat tAA = Mul(tV, tDD, tV.Tr);
double err = (tAA - tA).ToArray().HAbs().HMax();
if (err > 0.00000001)
{
Exit("EigSymm, A != V D V'");
}
ILinAlgMat tV_Vt = Mul(tV, tV.Tr);
ILinAlgMat tI = Eye(3);
err = (tV_Vt - tI).ToArray().HAbs().HMax();
if (err > 0.00000001)
{
Exit("EigSymm, I != V V'");
}
}
return(func(A));
}
public ILinAlgMat Diag(ILinAlgMat mat)
{
Func <ILinAlgMat, ILinAlgMat> func = _Diag;
if (HDebug.Selftest(func))
{
ILinAlgMat tA = ToILMat(new double[, ] {
{ 1, 2, 3 }, { 2, 4, 5 }, { 3, 5, 6 }
});
ILinAlgMat tADiag0 = Diag(tA);
ILinAlgMat tADiag1 = ToILMat(new double[] { 1, 4, 6 });
double tAerr = (tADiag1 - tADiag0).ToArray().HAbs().HMax();
if (tAerr > 0.00000001)
{
Exit("Diag(A) is wrong");
}
ILinAlgMat tB = ToILMat(new double[] { 1, 2, 3 });
ILinAlgMat tBDiag0 = Diag(tB);
ILinAlgMat tBDiag1 = ToILMat(new double[, ] {
{ 1, 0, 0 }, { 0, 2, 0 }, { 0, 0, 3 }
});
double tBerr = (tADiag1 - tADiag0).ToArray().HAbs().HMax();
if (tBerr > 0.00000001)
{
Exit("Diag(B) is wrong");
}
}
return(func(mat));
}
public ILinAlgMat PInv(ILinAlgMat A)
{
Func <ILinAlgMat, ILinAlgMat> func = _PInv;
if (HDebug.Selftest(func))
{
ILinAlgMat tA = ToILMat(new double[, ] {
{ 1, 2, 3 }, { 2, 3, 5 }, { 3, 4, 5 }
});
ILinAlgMat tiA = PInv(tA);
ILinAlgMat tiAA = ToILMat(new double[, ] {
{ -2.5, 1.0, 0.5 }, { 2.5, -2.0, 0.5 }, { -0.5, 1.0, -0.5 }
});
double err0 = (tiA - tiAA).ToArray().HAbs().HMax();
if (err0 > 0.00000001)
{
Exit("Pinv, is incorrect");
}
ILinAlgMat tI = Eye(3);
double err1 = (tA * tiA - tI).ToArray().HAbs().HMax();
if (err1 > 0.00000001)
{
Exit("Pinv, A * invA != I");
}
double err2 = (tiA * tA - tI).ToArray().HAbs().HMax();
if (err2 > 0.00000001)
{
Exit("Pinv, invA * A != I");
}
}
return(func(A));
}
public ILinAlgMat Mul(params ILinAlgMat[] mats)
{
Func <ILinAlgMat[], ILinAlgMat> func = _Mul;
if (HDebug.Selftest(func))
{
ILinAlgMat tA = ToILMat(new double[, ] {
{ 1, 2 }, { 3, 4 }, { 5, 6 }
});
ILinAlgMat tB = ToILMat(new double[, ] {
{ 7, 8 }, { 9, 1 }
});
ILinAlgMat tC = ToILMat(new double[, ] {
{ 2, 3, 4 }, { 5, 6, 7 }
});
ILinAlgMat tR = Mul(tA, tB, tC);
ILinAlgMat tRR = ToILMat(new double[, ] {
{ 100, 135, 170 }, { 254, 339, 424 }, { 408, 543, 678 }
});
double terr = (tR - tRR).ToArray().HAbs().HMax();
if (terr > 0.00000001)
{
Exit("Mul, (A * B * C) is wrong");
}
}
return(func(mats));
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
//public static Complex[] Roots2c(double p2, double p1, double p0)
//{
// Complex d = Complex.Sqrt(new Complex(p1 * p1 - 4 * p2 * p0));
// Complex[] roots = new Complex[2] {
// (-1*p1 + d)/(2*p2),
// (-1*p1 - d)/(2*p2)
// };
// return roots;
//}
public static double[] GetRootsClosedFormDegree2(double p2, double p1, double p0)
{
if (HDebug.Selftest())
{
double[] tsol;
tsol = GetRootsClosedFormDegree2(1, 2, -3);
HDebug.Assert(tsol[0] == 1, tsol[1] == -3);
tsol = GetRootsClosedFormDegree2(4, 3, 10);
HDebug.Assert(tsol == null);
}
double d = p1 * p1 - 4 * p2 * p0;
if (d >= 0)
{
d = Math.Sqrt(d);
double[] roots = new double[2] {
(-1 * p1 + d) / (2 * p2),
(-1 * p1 - d) / (2 * p2)
};
return(roots);
}
return(null);
}
//public static double Cov(Vector vec1, Vector vec2)
//{
// return Cov(vec1.ToArray().ToList(), vec2.ToArray().ToList());
//}
public static double HCov(double[] vec1, double[] vec2)
{
if (HDebug.Selftest())
{
// check with mathematica
double tcov = HCov(new double[] { 1, 2, 3 }, new double[] { 3, 7, 4 });
double terr = 0.5 - tcov;
HDebug.AssertTolerance(0.00000001, terr);
}
if (vec1.Length != vec2.Length)
{
throw new Exception();
}
double avg1 = vec1.HAvg();
double avg2 = vec2.HAvg();
double cov = 0;
int size = vec1.Length;
for (int i = 0; i < size; i++)
{
cov += (vec1[i] - avg1) * (vec2[i] - avg2);
}
cov = cov / (size - 1); /// the unbiased estimate of the covariance, which divide by (n-1)
return(cov);
}
public Matrix FnMul(Matrix A, Matrix B)
{
if (HDebug.Selftest())
{
Matrix tA = new double[, ] {
{ 1, 2 }
};
Matrix tB = new double[, ] {
{ 2, 3 }, { 4, 5 }
};
Matrix tAB0 = new double[, ] {
{ 10, 13 }
};
Matrix tAB1 = FnMul(tA, tB);
HDebug.AssertToleranceMatrix(0, tAB0 - tAB1);
}
var AA = ToILMat(A);
var BB = ToILMat(B);
var AABB = AA * BB;
AA.Dispose();
BB.Dispose();
Matrix AB = AABB.ToArray();
AABB.Dispose();
return(AB);
}
public HDictionary()
{
//HDebug.Depreciated();
if (HDebug.Selftest())
{
HDebug.Assert(false); // depreciated
}
}
public ILinAlgMat ToILMat(Matrix mat)
{
Func <Matrix, ILinAlgMat> func = _ToILMat;
if (HDebug.Selftest(func))
{
}
return(_ToILMat(mat));
}
public ILinAlgMat Eye(int size)
{
Func <int, ILinAlgMat> func = _Eye;
if (HDebug.Selftest(func))
{
}
return(func(size));
}
public ILinAlgMat Ones(int colsize, int rowsize)
{
Func <int, int, ILinAlgMat> func = _Ones;
if (HDebug.Selftest(func))
{
}
return(func(colsize, rowsize));
}
public ILinAlgMat ToILMat(double[] mat)
{
Func <double[], ILinAlgMat> func = _ToILMat;
if (HDebug.Selftest(func))
{
}
return(_ToILMat(mat));
}
public static int[] HCount <T>(this IList <T[]> valuess)
{
if (HDebug.Selftest())
{
double[][] tvaluess = new double[][] { new double[] { 1, 2, 3 }, new double[0], new double[] { 1, 2, 3, 4 } };
int[] tcnt = HCount(tvaluess);
HDebug.Assert(new TVector <int>(tcnt) == new int[] { 3, 0, 4 });
}
int[] counts = new int[valuess.Count];
for (int i = 0; i < counts.Length; i++)
{
counts[i] = valuess[i].Length;
}
return(counts);
}
//public static double Corr(Vector vec1, Vector vec2)
//{
// return Corr(vec1.ToArray().ToList(), vec2.ToArray().ToList());
//}
public static double HCorr(double[] vec1, double[] vec2)
{
if (HDebug.Selftest())
{
// check with mathematica
double tcorr = HCorr(new double[] { 1, 2, 3 }, new double[] { 3, 7, 4 });
double terr = 0.2401922307076307 - tcorr;
HDebug.AssertTolerance(0.00000001, terr);
}
if (vec1.Length != vec2.Length)
{
throw new Exception();
}
double corr = HCov(vec1, vec2) / Math.Sqrt(vec1.HVar() * vec2.HVar());
return(corr);
}
public double Det(ILinAlgMat mat)
{
Func <ILinAlgMat, double> func = _Det;
if (HDebug.Selftest(func))
{
ILinAlgMat tA = ToILMat(new double[, ] {
{ 1, 2, 3 }, { 2, 4, 5 }, { 3, 5, 6 }
});
double tdet = Det(tA);
if (Math.Abs(tdet - (-1.0)) > 0.00000001)
{
Exit("Det(A) is wrong");
}
}
return(func(mat));
}
static public MatrixByArr Inv2x2(MatrixByArr _this)
{
if (HDebug.Selftest())
{
// >>A = [ 1, 2 ; 3, 4 ];
// >>invA = inv(A)
// invA =
// -2.0000 1.0000
// 1.5000 -0.5000
// >> invA* A
// ans =
// 1.0000 0
// 0.0000 1.0000
// >> A*invA
// ans =
// 1.0000 0
// 0.0000 1.0000
MatrixByArr _A = new double[2, 2] {
{ 1, 2 }, { 3, 4 }
};
MatrixByArr _invA = Inv2x2(_A);
HDebug.Assert(_invA[0, 0] == -2); HDebug.Assert(_invA[0, 1] == 1);
HDebug.Assert(_invA[1, 0] == 1.5); HDebug.Assert(_invA[1, 1] == -0.5);
}
// http://www.cvl.iis.u-tokyo.ac.jp/~miyazaki/tech/teche23.html
if (_this.RowSize != 2 || _this.ColSize != 2)
{
return(null);
}
double a = _this[0, 0];
double b = _this[0, 1];
double c = _this[1, 0];
double d = _this[1, 1];
double detA = a * d - b * c;
MatrixByArr inv = new MatrixByArr(2, 2);
inv[0, 0] = d;
inv[0, 1] = -b;
inv[1, 0] = -c;
inv[1, 1] = a;
inv /= detA;
return(inv);
}
public static double HVar(this IList <double> values)
{
if (HDebug.Selftest())
{
double tvar = (new double[] { 1, 2, 3, 4, 5 }).HVar();
double terr = 2.5 - tvar;
HDebug.AssertTolerance(0.0000000001, terr);
}
double avg = values.HAvg();
double var = 0;
foreach (double value in values)
{
var += (avg - value) * (avg - value);
}
var /= (values.Count - 1); /// the unbiased estimate of variance, which divide by (n-1)
return(var);
}
public Tuple <Matrix, Vector> FnEigSymm(Matrix A)
{
if (HDebug.Selftest())
{
Matrix tA = new double[, ] {
{ 1, 2 }, { 2, 1 }
};
var tVD = FnEigSymm(tA);
Matrix tV = new double[, ] {
{ -0.7071, 0.7071 }, { 0.7071, 0.7071 }
};
Vector tD = new double[] { -1, 3 };
HDebug.AssertToleranceMatrix(0.0001, tV - tVD.Item1);
HDebug.AssertToleranceVector(0, tD - tVD.Item2);
}
var AA = ToILMat(A);
var VVDD = EigSymm(AA);
return(new Tuple <Matrix, Vector>(VVDD.Item1.ToArray(), VVDD.Item2));
}
public static MatrixByArr AlterDotProd(Vector v1, Vector v2)
{
HDebug.ToDo("depreciated. Call VVt(v1,v2)");
if (HDebug.IsDebuggerAttached && HDebug.Selftest())
{
MatrixByArr M0 = AlterDotProd(v1, v2);
MatrixByArr M1 = VVt(v1, v2);
HDebug.AssertTolerance(0, M0 - M1);
}
MatrixByArr mat = new MatrixByArr(v1.Size, v2.Size);
for (int c = 0; c < mat.ColSize; c++)
{
for (int r = 0; r < mat.RowSize; r++)
{
mat[c, r] = v1[c] * v2[r];
}
}
return(mat);
}
public ILinAlgMat InvSymm(ILinAlgMat A)
{
Func <ILinAlgMat, ILinAlgMat> func = _InvSymm;
if (HDebug.Selftest(func))
{
ILinAlgMat tA = ToILMat(new double[, ] {
{ 1, 2, 3 }, { 2, 4, 5 }, { 3, 5, 6 }
});
ILinAlgMat tI = ToILMat(new double[, ] {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
});
ILinAlgMat tInvA = func(tA);
double err = (tInvA * tA - tI).ToArray().HAbs().HMax();
if (err > 0.00000001)
{
Exit("LinSolve, is incorrect");
}
}
return(func(A));
}
public ILinAlgMat LinSolve(ILinAlgMat A, ILinAlgMat B)
{
Func <ILinAlgMat, ILinAlgMat, ILinAlgMat> func = _LinSolve;
if (HDebug.Selftest(func))
{
ILinAlgMat tA = ToILMat(new double[, ] {
{ 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, 9 }
});
ILinAlgMat tB = ToILMat(new double[, ] {
{ 3, 4, 5 }, { 6, 7, 8 }, { 9, 10, 11 }
});
ILinAlgMat tX = _LinSolve(tA, tB);
double err = (tA * tX - tB).ToArray().HAbs().HMax();
if (err > 0.00000001)
{
Exit("LinSolve, is incorrect");
}
}
return(func(A, B));
}
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);
}
public Matrix FnMul(Matrix A, Matrix B, Matrix C)
{
if (HDebug.Selftest())
{
Matrix tA = new double[, ] {
{ 1, 2 }
};
Matrix tB = new double[, ] {
{ 2, 3 }, { 4, 5 }
};
Matrix tC = new double[, ] {
{ 6 }, { 7 }
};
Matrix tABC0 = new double[, ] {
{ 151 }
};
Matrix tABC1 = FnMul(tA, tB, tC);
HDebug.AssertToleranceMatrix(0, tABC0 - tABC1);
}
if (C == null)
{
return(FnMul(A, B));
}
var AA = ToILMat(A);
var BB = ToILMat(B);
var CC = ToILMat(C);
var AABBCC = AA * BB * CC;
AA.Dispose();
BB.Dispose();
CC.Dispose();
Matrix ABC = AABBCC.ToArray();
AABBCC.Dispose();
return(ABC);
}
//public static double Corr(Vector vec1, Vector vec2, bool ignore_nan /*=false*/)
//{
// return Corr(vec1.ToArray().ToList(), vec2.ToArray().ToList(), ignore_nan);
//}
public static double HCorr(double[] vec1, double[] vec2, bool ignore_nan /*=false*/)
{
if (HDebug.Selftest())
{
double tcorr = HCorr(new double[] { 1, 4, double.NaN, double.NaN, 5, 2, 8 }
, new double[] { 2, 1, 3, double.NaN, double.NaN, 7, 9 }
, true);
double terr = 0.5784990588061418 - tcorr;
HDebug.AssertTolerance(0.00000001, terr);
}
if (vec1.Length != vec2.Length)
{
throw new Exception();
}
if (ignore_nan == true)
{
int size = vec1.Length;
bool[] isnan = new bool[size]; // initially false
foreach (int idxnan in vec1.HListIndexEqualTo(double.NaN))
{
isnan[idxnan] = true;
}
foreach (int idxnan in vec2.HListIndexEqualTo(double.NaN))
{
isnan[idxnan] = true;
}
int[] idxnnon = isnan.HListIndexEqualTo(false).ToArray();
if (idxnnon.Length == 0)
{
return(double.NaN);
}
vec1 = vec1.HSelectByIndex(idxnnon).ToArray();
vec2 = vec2.HSelectByIndex(idxnnon).ToArray();
}
return(HCorr(vec1, vec2));
}
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);
}
public static object LeastSquare
(double[,] As, double[] bs
, bool opt_get_stat = false
)
{
if (HDebug.Selftest())
{
/// >> A = [ 1,3,2, 1 ; 4,5,6, 1 ; 7,9,9, 1 ; 11,11,12, 1 ; 13,16,15, 1 ]
/// >> b = [1, 4, 6, 9, 12]'
/// >> x = inv(A' * A) * (A' * b)
/// 0.2171
/// 0.2125
/// 0.4205
/// -0.7339
/// >> esti = A * x
/// 0.9619
/// 3.7203
/// 6.4832
/// 9.0381
/// 11.7965
/// >> corr(esti,b)
/// 0.9976
/// >> mean( (b-esti).^2 )
/// 0.0712
double[,] _A = new double[5, 3] {
{ 1, 3, 2 }, { 4, 5, 6 }, { 7, 9, 9 }, { 11, 11, 12 }, { 13, 16, 15 }
};
double[] _b = new double[5] {
1, 4, 6, 9, 12
};
dynamic _out = LeastSquare(_A, _b, true);
double _matlab_corr = 0.9976;
double _matlab_mse = 0.0712;
double[] _matlab_x = new double[] { 0.2171, 0.2125, 0.4205, -0.7339 };
double[] _matlab_esti = new double[] { 0.9619, 3.7203, 6.4832, 9.0381, 11.7965 };
double err1 = Math.Abs(_matlab_corr - _out.opt_estimation_corr);
double err2 = Math.Abs(_matlab_mse - _out.opt_mean_square_err);
double err3 = (_matlab_x - (Vector)_out.x).ToArray().MaxAbs();
double err4 = (_matlab_esti - (Vector)_out.opt_estimation).ToArray().MaxAbs();
HDebug.Assert(err1 < 0.0001);
HDebug.Assert(err2 < 0.0001);
HDebug.Assert(err3 < 0.0001);
HDebug.Assert(err4 < 0.0001);
}
/// => A x = b
///
/// => At A x = At b
///
/// => AA * x = Ab
/// => x = inv(AA) * Ab
HDebug.Assert(As.GetLength(0) == bs.Length);
int n = As.GetLength(0);
int k = As.GetLength(1);
Matrix A = Matrix.Zeros(n, k + 1);
for (int c = 0; c < n; c++)
{
for (int r = 0; r < k; r++)
{
A[c, r] = As[c, r];
}
A[c, k] = 1;
}
Matrix AA = LinAlg.MtM(A, A);
Vector Ab = LinAlg.MtV(A, bs);
Vector x;
switch (k + 1)
{
case 2: { Matrix invAA = LinAlg.Inv2x2(AA.ToArray()); x = LinAlg.MV(invAA, Ab); } break;
case 3: { Matrix invAA = LinAlg.Inv3x3(AA.ToArray()); x = LinAlg.MV(invAA, Ab); } break;
case 4: { Matrix invAA = LinAlg.Inv4x4(AA.ToArray()); x = LinAlg.MV(invAA, Ab); } break;
default:
Matlab.PutMatrix("LinAlg_LeastSquare.AA", AA);
Matlab.PutVector("LinAlg_LeastSquare.Ab", Ab);
Matlab.Execute("LinAlg_LeastSquare.AA = inv(LinAlg_LeastSquare.AA);");
Matlab.Execute("LinAlg_LeastSquare.x = LinAlg_LeastSquare.AA * LinAlg_LeastSquare.Ab;");
x = Matlab.GetVector("LinAlg_LeastSquare.x");
Matlab.Execute("clear LinAlg_LeastSquare;");
break;
}
double?opt_mean_square_err = null;
double?opt_estimation_corr = null;
Vector opt_estimation = null;
if (opt_get_stat)
{
opt_estimation = new double[n];
double avg_err2 = 0;
for (int i = 0; i < n; i++)
{
double esti = 0;
for (int j = 0; j < k; j++)
{
esti += As[i, j] * x[j];
}
esti += x[k];
opt_estimation[i] = esti;
avg_err2 += (esti - bs[i]) * (esti - bs[i]);
}
avg_err2 /= n;
opt_mean_square_err = avg_err2;
opt_estimation_corr = HMath.HCorr(opt_estimation, bs);
}
return(new
{
x = x,
/// optional outputs
opt_mean_square_err = opt_mean_square_err,
opt_estimation_corr = opt_estimation_corr,
opt_estimation = opt_estimation,
});
}
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));
}
public static double DerivativeOfTriangleRadius(Vector p1, Vector p2, Vector p3, Vector dp1, Vector dp2, Vector dp3)
{
/// r = |p1-p2|*|p2-p3|*|p3-p1| / (2 area(p1,p2,p3))
/// = |p1-p2|*|p2-p3|*|p3-p1| / (2 |(p1-p2)x(p2-p3)|)
/// r2 = ( |p1-p2|*|p2-p3|*|p3-p1| / (2 area(p1,p2,p3)) )^2
/// = |p1-p2|^2 * |p2-p3|^2 * |p3-p1|^2 / (4 |(p1-p2)x(p2-p3)|^2)
/// (r+dr*t) = R = |(p1+dp1*t)-(p2+dp2*t)|*|(p2+dp2*t)-(p3+dp3*t)|*|(p3+dp3*t)-(p1+dp1*t)| / (2 area(p1+dp1*t,p2+dp2*t,p3+dp3*t))
/// (r+dr*t)2 = R2 = { |(p1+dp1*t)-(p2+dp2*t)|*|(p2+dp2*t)-(p3+dp3*t)|*|(p3+dp3*t)-(p1+dp1*t)| / (2 area(p1+dp1*t,p2+dp2*t,p3+dp3*t)) }^2
///
/// d(r+dr*t)/dt = dR_dt
/// = d(r+dr*t)/d((r+dr*t)^2) * d((r+dr*t)^2)/dt
/// = dR_dR2 * dR2_dt
/// d(r+dr*t)/d((r+dr*t)^2) = dR_dR2
/// = 1/(2*r)
/// d((r+dr*t)^2)/dt = dR2_dt
/// = + ( Dotp31dp31 * P12dist2 * P23dist2
/// + Dotp23dp31 * P12dist2 * P31dist2
/// + Dotp12dp12 * P23dist2 * P31dist2
/// )/(2 * Area2)
/// - ( DotCroP12P23CroP12Dp23CroDp12P23 * P12dist2 * P23dist2 * P31dist2)/(2 * Area4)
///
/// P12dist2 := ((p1x-p2x)^2+(p1y-p2y)^2+(p1z-p2z)^2) = (p1-p2).dist2
/// P23dist2 := ((p2x-p3x)^2+(p2y-p3y)^2+(p2z-p3z)^2) = (p2-p3).dist2
/// P31dist2 := ((p1x-p3x)^2+(p1y-p3y)^2+(p1z-p3z)^2) = (p1-p3).dist2
/// Dotp12dp12 := ((dp1x-dp2x)*(p1x-p2x) + (dp1y-dp2y)*(p1y-p2y) + (dp1z-dp2z)*(p1z-p2z))
/// = [dp1x-dp2x, dp1y-dp2y, dp1z-dp2z] . [p1x-p2x, p1y-p2y, p1z-p2z]
/// = (dp1-dp2).(p1-p2)
/// Dotp23dp31 := ((dp2x-dp3x)*(p2x-p3x) + (dp2y-dp3y)*(p2y-p3y) + (dp2z-dp3z)*(p2z-p3z))
/// = (dp2-dp3).(p2-p3)
/// Dotp31dp31 := ((dp1x-dp3x)*(p1x-p3x) + (dp1y-dp3y)*(p1y-p3y) + (dp1z-dp3z)*(p1z-p3z))
/// = (dp1-dp3).(p1-p3)
/// DotCroP12P23CroP12Dp23CroDp12P23 := Dot[Cross[p1-p2,p2-p3],(Cross[p1-p2,dp2-dp3]+Cross[dp1-dp2,p2-p3])]
/// = [(p1-p2)x(p2-p3)] . [(p1-p2)x(dp2-dp3) + (dp1-dp2)x(p2-p3)]
if (HDebug.Selftest())
#region selftest
{
Vector lp1 = new double[] { 1, 0, 0 };
Vector lp2 = new double[] { 0, 1, 0 };
Vector lp3 = new double[] { 0, 0, 1 };
Vector ldp1 = new double[] { 0.1, 0.1, 0.1 };
Vector ldp2 = new double[] { 1, 2, 3 };
Vector ldp3 = new double[] { -1, 0, -0.1 };
double r0 = Geometry.RadiusOfTriangle(lp1, lp2, lp3);
double dr0 = DerivativeOfTriangleRadius(lp1, lp2, lp3, ldp1, ldp2, ldp3);
List <Tuple <double, double, double, double> > drx = new List <Tuple <double, double, double, double> >();
foreach (double dt in new double[] { 0.1, 0.01, 0.001, 0.0001, 0.00001, 0.000001, 0.0000001 })
{
double t00 = -dt; double r00 = Geometry.RadiusOfTriangle(lp1 + t00 * ldp1, lp2 + t00 * ldp2, lp3 + t00 * ldp3);
double t01 = +dt; double r01 = Geometry.RadiusOfTriangle(lp1 + t01 * ldp1, lp2 + t01 * ldp2, lp3 + t01 * ldp3);
double dr1 = (r01 - r00) / (t01 - t00);
double dr2 = (r01 * r01 - r00 * r00) / (t01 - t00);
drx.Add(new Tuple <double, double, double, double>(dt, dr1, dr2, (0.5 / r0) * dr2));
}
double diff = dr0 - drx.Last().Item2;
HDebug.AssertTolerance(0.00000001, diff);
}
#endregion
double P12dist2 = (p1 - p2).Dist2;
double P23dist2 = (p2 - p3).Dist2;
double P31dist2 = (p1 - p3).Dist2;
double Dotp12dp12 = LinAlg.VtV(dp1 - dp2, p1 - p2);
double Dotp23dp31 = LinAlg.VtV(dp2 - dp3, p2 - p3);
double Dotp31dp31 = LinAlg.VtV(dp1 - dp3, p1 - p3);
double DotCroP12P23CroP12Dp23CroDp12P23 = LinAlg.VtV(LinAlg.CrossProd(p1 - p2, p2 - p3)
, LinAlg.CrossProd(p1 - p2, dp2 - dp3) + LinAlg.CrossProd(dp1 - dp2, p2 - p3)
);
double Area2 = LinAlg.CrossProd(p1 - p2, p2 - p3).Dist2;
double Area4 = Area2 * Area2;
double Rad2 = P12dist2 * P23dist2 * P31dist2 / (4 * Area2);
double Rad = Math.Sqrt(Rad2);
HDebug.AssertTolerance(0.00000001, Rad - Geometry.RadiusOfTriangle(p1, p2, p3));
double t = 1;
double dR_dR2 = 0.5 / Rad;
double dR2_dt = ((t * Dotp31dp31) * P12dist2 * P23dist2 + (t * Dotp23dp31) * P12dist2 * P31dist2 + (t * Dotp12dp12) * P23dist2 * P31dist2) / (2 * Area2)
- ((t * DotCroP12P23CroP12Dp23CroDp12P23) * P12dist2 * P23dist2 * P31dist2) / (2 * Area4);
double dR_dt = dR_dR2 * dR2_dt;
return(dR_dt);
}
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);
}
