// public static double ClosestIndexFromLine(DoubleVector3 point, DoubleVector3 LinePt0, DoubleVector3 LinePt1)
// {
// // Q (query)
// // /|\
// // / | \
// // / | \
// // /---+---\
// // A T B
// //
// // AQ . AB = |AQ| * |AB| * cos(QAB)
// // cos(QAB) = |AT| / |AQ|
// // |AT| / |AB| = AQ . AB / |AB|^2
// DoubleVector3 P = point;
// DoubleVector3 A = LinePt0;
// DoubleVector3 B = LinePt1;
//
// DoubleVector3 AP = P - A;
// DoubleVector3 AB = B - A;
// double ab2 = AB.Length2;
// if(ab2 == 0)
// return 0;
// double ap_ab = DoubleVector3.InnerProduct(AP, AB);
// double t = ap_ab / ab2;
// return t;
// }
// public static double IndexSegmentClosestToLine(Segment segment, Line line)
// {
// // 1. find the plane containing line and point which is the closest on the segment
// DoubleVector3 orthogonal_line_segment = DoubleVector3.CrossProduct(segment.Direct, line.Normal);
// DoubleVector3 normal_plane = DoubleVector3.CrossProduct(orthogonal_line_segment, line.Normal).UnitVector;
// Plane plane = new Plane(line.Base, normal_plane);
// // 2. fine index of segment intersecting the plane
// double index = IndexSegmentIntersectingPlane(segment, plane);
// //Debug.Assert(DoubleVector3.InnerProduct(segment[index]-))
// if(Debug.IsDebuggerAttached)
// {
// double dist_pt2line = DistancePointLine(segment[index],line);
// double dist_line2line = DistanceLineLine(line, segment.ToLine());
// Debug.AssertSimilar(dist_pt2line, dist_line2line, 0.000000001);
// }
// return index;
// }
// public static double[] IndexSegmentToLineByDistance(Segment segment, Line line, double distance)
// {
// double dist_lineline = DistanceLineLine(line, segment.ToLine());
// if(dist_lineline > distance)
// return null;
// double index = IndexSegmentClosestToLine(segment, line);
// if(dist_lineline == distance)
// return new double[1] { index };
// double dist_more = Math.Sqrt(distance*distance + dist_lineline*dist_lineline);
// double angle_segment_line = DoubleVector3.AngleBetween(segment.Direct, line.Normal).Radian;
// double index_dist = (dist_more / Math.Atan(angle_segment_line)) / segment.Direct.Length;
// Debug.Assert(DistancePointLine(segment[index+index_dist], line) > distance);
// Func<double,double> funcdist = delegate(double t) { return DistancePointLine(segment[t], line); };
// Pair<bool,double> index_dist2 = RootsBisection.Root(funcdist, index, index+index_dist);
// Debug.Assert(index_dist2.first == true);
// double[] indexes = new double[2] { index-index_dist2.second, index+index_dist2.second };
// if(Debug.IsDebuggerAttached)
// {
// DoubleVector3 pt1 = segment[indexes[0]];
// DoubleVector3 pt2 = segment[indexes[1]];
// Debug.AssertSimilar(distance, DistancePointLine(pt1, line), 0.00000001);
// Debug.AssertSimilar(distance, DistancePointLine(pt2, line), 0.00000001);
// }
// return indexes;
// }
// public static double IndexSegmentIntersectingPlane(Segment segment, Plane plane)
// {
// // http://softsurfer.com/Archive/algorithm_0104/algorithm_0104B.htm
// // s = -n.w / n.u
// double dist = -1 * DoubleVector3.InnerProduct(plane.Normal, segment.Base-plane.Base)
// / DoubleVector3.InnerProduct(plane.Normal, segment.Direct);
// Debug.Assert(plane.DistanceFrom(segment[dist]) < 0.00000001);
// return dist;
// //// index of segment which intersecting plane
// //// if index is in [0,1], the segment intersects plane
// //// otherwise, it is outside of plane
// //if(DoubleVector3.InnerProduct(segment.Direct, plane.Normal) == 0)
// // return double.NaN;
// //double dist = DistancePointPlane(segment.Base, plane);
// //DoubleVector3 point2plane = -1*Math.Sign(dist)*plane.Normal;
// //double sin = DoubleVector3.CrossProduct(point2plane, segment.Direct.UnitVector).Length;
// //double cos = DoubleVector3.InnerProduct(point2plane, segment.Direct.UnitVector);
// //double sign = (cos >=0) ? 1 : -1;
// //double unitlength = sign * sin * segment.Direct.Length;
// //return dist/unitlength;
// }
// public static double IndexSegmentIntersectingTriangle(Segment segment, DoubleVector3 tri0, DoubleVector3 tri1, DoubleVector3 tri2)
// {
// // return [0,1] if the segement intersects triangle
// // return (-inf,0) or (1,inf) if the line of segment intersects the triangle
// // return double.NegativeInfinity or double.PositiveInfinity if line does not intersects the triangle
// double index = IndexSegmentIntersectingPlane(segment, ToPlane(tri0, tri1, tri2));
// bool ptInTriangle = CheckPointInTriangle(segment[index], tri0, tri1, tri2);
// if(ptInTriangle == true)
// return index;
// if(index >= 0)
// return double.PositiveInfinity;
// if(index < 0)
// return double.NegativeInfinity;
// return double.NaN;
// }
public static double IndexSegmentClosestPoint(Segment segment, Vector point)
{
// http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
// t = - (x1 - x0) . (x2 - x1 ) / |x2-x1|^2
Vector x1 = segment.PtFrom;
Vector x2 = segment.PtTo;
Vector x0 = point;
double index = -1 * LinAlg.DotProd(x1 - x0, x2 - x1) / (x2 - x1).Dist2;
if (HDebug.IsDebuggerAttached)
{
// check orthogonal
Vector ortho = (point - segment[index]);
HDebug.Assert(LinAlg.DotProd(ortho, segment.Direct) < 0.00000001);
// check closestness
double dist = (point - segment[index]).Dist;
double dist1 = (point - segment[index - 0.001]).Dist;
double dist2 = (point - segment[index + 0.001]).Dist;
HDebug.Assert(dist <= dist1);
HDebug.Assert(dist <= dist2);
}
return(index);
}
public static double DistancePointLine(Vector point, Vector line1, Vector line2)
{
// http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
if (DistancePointLine_selftest)
{
DistancePointLine_selftest = false;
Vector tp = new Vector(2, 2, 0);
Vector tl1 = new Vector(0, 0, -20);
Vector tl2 = new Vector(0, 0, -10);
double tdist0 = tp.Dist;
double tdist1 = DistancePointLine(tp, tl1, tl2);
double tdist2 = (ClosestPointOnLine(tl1, tl2, tp, false) - tp).Dist;
HDebug.AssertTolerance(0.00000001, tdist0 - tdist1);
HDebug.AssertTolerance(0.00000001, tdist0 - tdist2);
}
Vector x21 = line2 - line1;
Vector x10 = line1 - point;
double dist
= LinAlg.CrossProd(x21, x10).Dist
/ x21.Dist;
return(dist);
}
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 double Dist2PointLine(Vector point, Vector line0, Vector line1)
{
/// Squared distance from a point
/// http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
///
/// (line0) x1-----+--------x2 (line1)
/// |
/// | d
/// x0 (point)
Vector x1 = line0;
Vector x2 = line1;
Vector x0 = point;
double d21 = (x2 - x1).Dist; HDebug.Assert(d21 != 0);
double d10 = (x1 - x0).Dist;
double d1021 = LinAlg.DotProd(x1 - x0, x2 - x1);
double dist2 = ((d10 * d10 * d21 * d21) - d1021 * d1021) / (d21 * d21);
if ((0 > dist2) && (dist2 > -0.0000000001))
{
// consider this as the precision problem by numerical error
dist2 = 0;
}
return(dist2);
}
public static Vector Point4IntersectSegmentPlane(Vector plane1, Vector plane2, Vector plane3, Vector segment1, Vector segment2, double tolerance = 0.00001)
{
/// return intersection point between plane (by p1, p2, p3) and line semgnet (seg1, seg2).
/// If the line segment does not pass through the plane, return null
Vector point = Point4LinePlaneIntersect(plane1, plane2, plane3, segment1, segment2);
Vector vec12 = (segment2 - segment1);
Vector uvec12 = vec12.UnitVector();
double leng12 = vec12.Dist;
Vector vec1p = (point - segment1);
double t = LinAlg.VtV(uvec12, vec1p);
tolerance = Math.Abs(tolerance);
if (t < -tolerance)
{
point = null; // smaller than 0
}
if (t > leng12 + tolerance)
{
point = null; // larger than leng12
}
return(point);
}
public static Tuple <double, Vector> MaxSphereBetweenSpheres(Vector pt1, double rad1,
Vector pt2, double rad2,
Vector pt3, double rad3,
Vector pt4, double rad4)
{
if (MaxSphereBetweenSpheres_SelfTest)
{
MaxSphereBetweenSpheres_SelfTest = false;
Vector tpo = new double[] { 0 + 1, 0 + 2, 0 + 3 }; double tro = 0.10;
Vector tpa = new double[] { 1 + 1, 2 + 2, 3 + 3 }; double tra = 0.20;
Vector tpb = new double[] { 5 + 1, 3 + 2, 2 + 3 }; double trb = 0.30;
Vector tpc = new double[] { 3 + 1, 3 + 2, 3 + 3 }; double trc = 0.15;
var test_rad_cent = MaxSphereBetweenSpheres(tpo, tro,
tpa, tra,
tpb, trb,
tpc, trc
);
double radius = test_rad_cent.Item1;
Vector center = test_rad_cent.Item2;
HDebug.AssertTolerance(0.00000001, radius - ((center - tpo).Dist - tro));
HDebug.AssertTolerance(0.00000001, radius - ((center - tpa).Dist - tra));
HDebug.AssertTolerance(0.00000001, radius - ((center - tpb).Dist - trb));
HDebug.AssertTolerance(0.00000001, radius - ((center - tpc).Dist - trc));
}
Vector po = pt1 - pt1; double po2 = po.Dist2; double ro = rad1; double ro2 = ro * ro;
Vector pa = pt2 - pt1; double pa2 = pa.Dist2; double ra = rad2; double ra2 = ra * ra;
Vector pb = pt3 - pt1; double pb2 = pb.Dist2; double rb = rad3; double rb2 = rb * rb;
Vector pc = pt4 - pt1; double pc2 = pc.Dist2; double rc = rad4; double rc2 = rc * rc;
double alpah_ab = (ro2 - ra2 + pa2) * (rb - ro) - (ro2 - rb2 + pb2) * (ra - ro);
double alpah_bc = (ro2 - rb2 + pb2) * (rc - ro) - (ro2 - rc2 + pc2) * (rb - ro);
Vector beta_ab = (pa2 - (ro - ra) * (ro - ra)) * pb - (pb2 - (ro - rb) * (ro - rb)) * pa;
Vector beta_bc = (pb2 - (ro - rb) * (ro - rb)) * pc - (pc2 - (ro - rc) * (ro - rc)) * pb;
double n_alpah_ab = alpah_ab / beta_ab.Dist; Vector n_beta_ab = beta_ab.UnitVector();
double n_alpah_bc = alpah_bc / beta_bc.Dist; Vector n_beta_bc = beta_bc.UnitVector();
var line_pt_vec = Geometry.LineOnTwoPlanes(n_beta_ab, n_alpah_ab,
n_beta_bc, n_alpah_bc);
Vector gamma = line_pt_vec.Item1;
Vector delta = line_pt_vec.Item2;
double a = delta.Dist2;
double b = 2 * LinAlg.VtV(gamma, delta);
double c = gamma.Dist2 - 1;
double[] roots = HRoots.GetRootsClosedFormDegree2(a, b, c);
foreach (double root in roots)
{
Vector q = gamma + root * delta;
double pa_q = LinAlg.VtV(pa, q);
double radius = (ro2 - ra2 + pa2 - 2 * ro * pa_q) / (2 * (ra - ro + pa_q));
if (radius < 0)
{
continue;
}
Vector center = (ro + radius) * q;
HDebug.AssertTolerance(0.00000001, radius - ((center - po).Dist - ro));
HDebug.AssertTolerance(0.00000001, radius - ((center - pa).Dist - ra));
HDebug.AssertTolerance(0.00000001, radius - ((center - pb).Dist - rb));
HDebug.AssertTolerance(0.00000001, radius - ((center - pc).Dist - rc));
HDebug.AssertTolerance(0.00000001, radius - ((center + pt1 - pt1).Dist - rad1));
HDebug.AssertTolerance(0.00000001, radius - ((center + pt1 - pt2).Dist - rad2));
HDebug.AssertTolerance(0.00000001, radius - ((center + pt1 - pt3).Dist - rad3));
HDebug.AssertTolerance(0.00000001, radius - ((center + pt1 - pt4).Dist - rad4));
return(new Tuple <double, Vector>(radius, center + pt1));
}
HDebug.Assert(roots == null);
return(null);
}
public static Tuple <double, Vector, bool> MaxCircleBetweenCircles(Vector pt1, double rad1,
Vector pt2, double rad2,
Vector pt3, double rad3)
{
/// b:pt3
/// / \
/// / \
/// / \
/// o:pt1 ------ a:pt2
///
/// o : (0 , 0), radii ro
/// a : (pax, 0), radii ra
/// b : (pbx, pby), radii rb
Vector uvec12 = (pt2 - pt1).UnitVector();
Vector uvec23 = (pt3 - pt2).UnitVector();
Vector uvec31 = (pt1 - pt3).UnitVector();
double cos123 = LinAlg.VtV(-uvec12, uvec23);
double cos231 = LinAlg.VtV(-uvec23, uvec31);
double cos312 = LinAlg.VtV(-uvec31, uvec12);
double cos_aob;
double ro, ra, rb;
Vector po, pa, pb;
if (cos123 > 0)
{
pa = pt1; ra = rad1; po = pt2; ro = rad2; pb = pt3; rb = rad3; cos_aob = cos123;
}
else if (cos231 > 0)
{
pa = pt2; ra = rad2; po = pt3; ro = rad3; pb = pt1; rb = rad1; cos_aob = cos231;
}
else if (cos312 > 0)
{
pa = pt2; ra = rad2; po = pt1; ro = rad1; pb = pt3; rb = rad3; cos_aob = cos312;
}
else
{
throw new Exception("cos123>0 and cos231>0");
}
Vector uvec_oa = (pa - po).UnitVector();
Vector uvec_ob = (pb - po).UnitVector();
Vector normal = LinAlg.CrossProd(uvec_oa, uvec_ob);
double sin_aob = normal.Dist;
double pax = (pa - po).Dist;
double pbx = (pb - po).Dist * cos_aob;
double pby = (pb - po).Dist * sin_aob;
Trans3 trans;
{
Vector npo = new double[] { 0, 0, 0 };
Vector npa = new double[] { 0, pax, 0 };
Vector npb = new double[] { 0, pbx, pby };
trans = Trans3.GetTransformNoScale(npo, npa, npb, po, pa, pb);
if (HDebug.IsDebuggerAttached)
{
Vector pox = trans.DoTransform(npo);
HDebug.AssertTolerance(0.0000001, po - trans.DoTransform(npo));
HDebug.AssertTolerance(0.0000001, pa - trans.DoTransform(npa));
HDebug.AssertTolerance(0.0000001, pb - trans.DoTransform(npb));
}
}
Tuple <double, Vector, bool> rad_cent_in = MaxCircleBetweenCircles(ro, ra, rb, pax, pbx, pby);
if (rad_cent_in == null)
{
return(null);
}
double radius = rad_cent_in.Item1;
bool cenerInTriangle = rad_cent_in.Item3;
Vector center;
{
HDebug.Assert(rad_cent_in.Item2.Size == 2);
center = new double[] { 0, rad_cent_in.Item2[0], rad_cent_in.Item2[1] };
center = trans.DoTransform(center);
}
if (HDebug.IsDebuggerAttached)
{
double radx_1 = (center - pt1).Dist - rad1;
double radx_2 = (center - pt2).Dist - rad2;
double radx_3 = (center - pt3).Dist - rad3;
HDebug.AssertTolerance(0.000001, radx_1 - radx_2, radx_2 - radx_3, radx_3 - radx_1);
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt1 - pt2));
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt2 - pt3));
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt3 - pt1));
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt1 - center));
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt2 - center));
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt3 - center));
}
return(new Tuple <double, Vector, bool>(radius, center, cenerInTriangle));
}
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 void GetTriGeom(Vector pa, Vector pb, Vector pc
, out double a, out double b, out double c
, out double A, out double B, out double C
, out double r
, out Vector po
)
{
Func <Vector, Vector, Vector, double> angle2 = delegate(Vector p1, Vector p2, Vector p3)
{
Vector v21 = p1 - p2;
Vector v23 = p3 - p2;
double d21 = v21.Dist;
double d23 = v23.Dist;
double cos2 = LinAlg.DotProd(v21, v23) / (d21 * d23);
double ang2 = Math.Acos(cos2);
return(ang2);
};
////////////////////////////////////////////////////////////////
// 1 (1,2,3)
// /A\ |
// / | \ r
// c | b |
// / o \ origin
// /B C\
// 2-----a-----3
//
/////////////////////////////////////////////////////////////////
// http://schools-wikipedia.org/wp/t/Trigonometric_functions.htm
// a/sinA = b/sinB = c/sinC = 2R
Vector pt23 = pb - pc; double dist23 = pt23.Dist;
Vector pt31 = pc - pa; double dist31 = pt31.Dist;
Vector pt12 = pa - pb; double dist12 = pt12.Dist;
a = dist23; // =(pt2-pt3).Dist; // distance between pt2 and pt3
b = dist31; // =(pt3-pt1).Dist; // distance between pt2 and pt3
c = dist12; // =(pt1-pt2).Dist; // distance between pt2 and pt3
A = angle2(pc, pa, pb); // angle A = ∠pt3-pt1(a)-pt2
B = angle2(pa, pb, pc); // angle B = ∠pt1-pt2(b)-pt3
C = angle2(pb, pc, pa); // angle C = ∠pt2-pt3(c)-pt1
r = 0.5 * a / Math.Sin(A);
// Barycentric coordinates from cross- and dot-products
// http://en.wikipedia.org/wiki/Circumscribed_circle#Barycentric_coordinates_as_a_function_of_the_side_lengths
/// po = v1.p1 + v2.p2 + v3.p3
/// v1 = (|p2-p3|^2 * (p1-p2).(p1-p3)) / (2*|(p1-p2)x(p2-p3)|^2) = a*a*Dot(
/// v2 = (|p1-p3|^2 * (p2-p1).(p2-p3)) / (2*|(p1-p2)x(p2-p3)|^2) = b*b*Dot(
/// v3 = (|p1-p2|^2 * (p3-p1).(p3-p2)) / (2*|(p1-p2)x(p2-p3)|^2) = c*c*Dot(
{
double div = 2 * LinAlg.CrossProd(pa - pb, pb - pc).Dist2;
double v1 = dist23 * dist23 * LinAlg.DotProd(pt12, -pt31) / div;
double v2 = dist31 * dist31 * LinAlg.DotProd(-pt12, pt23) / div;
double v3 = dist12 * dist12 * LinAlg.DotProd(pt31, -pt23) / div;
po = v1 * pa + v2 * pb + v3 * pc;
}
if (HDebug.IsDebuggerAttached)
{
double la, lb, lc;
double lA, lB, lC;
double lr;
Vector lpo;
Geometry.GetTriGeom(pa, pb, pc, out la, out lb, out lc, out lA, out lB, out lC, out lr, out lpo);
HDebug.Assert(la == a, lb == b, lc == c);
HDebug.Assert(lA == A, lB == B, lC == C);
HDebug.Assert(lr == r);
HDebug.Assert(lpo == po);
}
}
public static Vector operator*(MatrixByArr lmat, Vector rvec)
{
return(LinAlg.MV(lmat, rvec));
}
public static Vector operator*(Vector lvec, MatrixByArr rmat)
{
return(LinAlg.VtM(lvec, rmat));
}
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 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 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 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);
}