public static Trans3 GetTransformNoScale(Vector from1, Vector from2, Vector from3, Vector to1, Vector to2, Vector to3)
{
//HTLib.DoubleVector3 lfrom1 = new HTLib.DoubleVector3(from1);
//HTLib.DoubleVector3 lfrom2 = new HTLib.DoubleVector3(from2);
//HTLib.DoubleVector3 lfrom3 = new HTLib.DoubleVector3(from3);
//HTLib.DoubleVector3 lto1 = new HTLib.DoubleVector3(to1);
//HTLib.DoubleVector3 lto2 = new HTLib.DoubleVector3(to2);
//HTLib.DoubleVector3 lto3 = new HTLib.DoubleVector3(to3);
//HTLib.Trans3 ltrans = HTLib.Trans3.GetTransformNoScale(lto1, lto2, lto3, lfrom1, lfrom2, lfrom3);
Trans3 ltrans;
{
Vector[] p = new Vector[] { to1, to2, to3 };
Vector[] x = new Vector[] { from1, from2, from3 };
Trans3 optTrans = ICP3.OptimalTransform(p, x);
ltrans = optTrans;
}
if (HDebug.IsDebuggerAttached)
{
var nto1 = ltrans.DoTransform(from1); var err1 = (nto1 - to1).Dist;
var nto2 = ltrans.DoTransform(from2); var err2 = (nto2 - to2).Dist;
var nto3 = ltrans.DoTransform(from3); var err3 = (nto3 - to3).Dist;
HDebug.AssertTolerance(0.00000001, err1, err2, err3);
}
return(ltrans);
}
public static Tuple <Vector, Vector, Vector, Vector> Points4TetrahedronEquilateral()
{
/// https://en.wikipedia.org/wiki/Tetrahedron
/// Expressed symmetrically as 4 points on the unit sphere,
/// centroid at the origin, with lower face level, the vertices are:
/// v1 = ( sqrt(8 / 9), 0, -1 / 3)
/// v2 = (-sqrt(2 / 9), sqrt(2 / 3), -1 / 3)
/// v3 = (-sqrt(2 / 9), -sqrt(2 / 3), -1 / 3)
/// v4 = ( 0, 0, 1)
// determine point locations of equilateral tetrahedron
Vector v1 = new double[3] {
0, Math.Sqrt(8.0 / 9), -1.0 / 3
};
Vector v2 = new double[3] {
Math.Sqrt(2.0 / 3), -Math.Sqrt(2.0 / 9), -1.0 / 3
};
Vector v3 = new double[3] {
-Math.Sqrt(2.0 / 3), -Math.Sqrt(2.0 / 9), -1.0 / 3
};
Vector v4 = new double[3] {
0, 0, 1
};
// scale to make each edge's length as 1
double d12 = (v1 - v2).Dist;
v1 = v1 / d12; v2 = v2 / d12; v3 = v3 / d12; v4 = v4 / d12;
// make v3 as origin
v1 -= v3;
v2 -= v3;
v4 -= v3;
v3 -= v3;
// check length
HDebug.AssertTolerance(0.00000001, 1 - (v1 - v2).Dist);
HDebug.AssertTolerance(0.00000001, 1 - (v1 - v3).Dist);
HDebug.AssertTolerance(0.00000001, 1 - (v1 - v4).Dist);
HDebug.AssertTolerance(0.00000001, 1 - (v2 - v3).Dist);
HDebug.AssertTolerance(0.00000001, 1 - (v2 - v4).Dist);
HDebug.AssertTolerance(0.00000001, 1 - (v3 - v4).Dist);
// check height (v1.z == v2.z == v3.z)
HDebug.AssertTolerance(0.00000001, v1[2] - v2[2]);
HDebug.AssertTolerance(0.00000001, v1[2] - v2[2]);
HDebug.AssertTolerance(0.00000001, v1[2] - v3[2]);
// check others
HDebug.AssertTolerance(0.00000001, v1[2] - 0); // v1,v2,v3 are on z-plane
HDebug.AssertTolerance(0.00000001, v2[2] - 0); // v1,v2,v3 are on z-plane
HDebug.AssertTolerance(0.00000001, v3[2] - 0); // v1,v2,v3 are on z-plane
HDebug.AssertTolerance(0.00000001, v2[1] - 0); // v2,v3 are on x-axis
HDebug.AssertTolerance(0.00000001, v3[1] - 0); // v2,v3 are on x-axis
HDebug.AssertTolerance(0.00000001, v2[0] - 1); // v3 is on (1,0,0)
/// v1
/// / \
/// / \
/// / v4 \
/// / \
/// v3----------v2
///
return(new Tuple <Vector, Vector, Vector, Vector>(v1, v2, v3, v4));
}
public static IList <Vector> GetOrthonormals(this IList <Vector> vectors)
{
Vector[] omvecs = new Vector[vectors.Count];
omvecs[0] = vectors[0].UnitVector();
for (int i = 1; i < vectors.Count; i++)
{
Vector nomvec = vectors[i].UnitVector();
foreach (Vector omvec in omvecs)
{
if (omvec == null)
{
continue;
}
double proj = LinAlg.DotProd(nomvec, omvec);
nomvec = (nomvec - proj * omvec).UnitVector();
}
omvecs[i] = nomvec.UnitVector();
}
if (HDebug.IsDebuggerAttached)
{
for (int i = 0; i < omvecs.Length - 1; i++)
{
for (int j = i + 1; j < omvecs.Length; j++)
{
double proj = LinAlg.DotProd(omvecs[i].UnitVector(), omvecs[j].UnitVector());
HDebug.AssertTolerance(0.00000001, proj);
}
}
}
return(omvecs);
}
public static double GetD2LengthByDAngle2(double a, double b, double c)
{
HDebug.ToDo("check !!!");
/// return d^2_a / d_A^2
///
/// A = acos(b^2 + c^2 - a^2 / 2 b c)
/// cosA = (b^2 + c^2 - a^2 / 2 b c)
/// a^2 = b^2 + c^2 - 2 b c cos(A)
/// la = sqrt(b^2 + c^2 - 2 b c cos(A))
/// == a
/// da_dA = (b c Sin[A])/Sqrt[b^2 + c^2 - 2 b c Cos[A]]
/// = (b c Sin[A])/( la)
/// d2a_dA2 = d_dA (da_dA)
/// = (b c Cos[A])/Sqrt[b^2 + c^2 - 2 b c Cos[A]] - (b^2 c^2 Sin[A]^2)/(b^2 + c^2 - 2 b c Cos[A])^(3/2)
/// = (b c Cos[A])/Sqrt[b^2 + c^2 - 2 b c Cos[A]] - (b^2 c^2 Sin[A]^2)/(Sqrt[b^2 + c^2 - 2 b c Cos[A]]^3)
/// = (b c Cos[A])/( la) - (b^2 c^2 Sin[A]^2)/( la^3)
/// = (b c cosA )/( la) - (b2 c2 sinA2 )/( la^3)
///
double a2 = a * a;
double b2 = b * b;
double c2 = c * c;
double cosA = (b2 + c2 - a2) / (2 * b * c);
double la = Math.Sqrt(b2 + c2 - 2 * b * c * cosA);
double la3 = la * la * la;
HDebug.AssertTolerance(0.00000001, la - a);
double A = Math.Acos(cosA);
double sinA = Math.Sin(A);
double sinA2 = sinA * sinA;
double da_dA = (b * c * sinA) / la;
double d2a_dA2 = (b * c * cosA) / (la) - (b2 * c2 * sinA2) / (la3);
return(d2a_dA2);
}
public static Vector Point4LinePlaneIntersect(Vector planeNormal, Vector planePoint, Vector lineDire, Vector linePoint)
{
if (Point4LinePlaneIntersect_SelfTest)
{
Point4LinePlaneIntersect_SelfTest = false;
Vector _plnm = new double[] { 1, 0, 0 };
Vector _plpt = new double[] { 2, 0, 0 };
Vector _lndr = new double[] { 1, 1, 1 };
Vector _lnpt = new double[] { 0, 0, 0 };
Vector _itpt0 = Point4LinePlaneIntersect(_plnm, _plpt, _lndr, _lnpt);
Vector _itpt1 = new double[] { 2, 2, 2 };
HDebug.AssertTolerance(0.000001, _itpt0 - _itpt1);
}
/// http://en.wikipedia.org/wiki/Line-plane_intersection
/// Algebraic form
/// plane : (pt - p0).n = 0
/// line : pt = d l + l0
///
/// point-line intersection: d = ((p0 - l0) . n) / (l . n)
/// pt = d * l + l0
Vector n = planeNormal;
Vector p0 = planePoint;
Vector l = lineDire;
Vector l0 = linePoint;
double d = LinAlg.DotProd(p0 - l0, n) / LinAlg.DotProd(l, n);
Vector pt = d * l + l0;
HDebug.AssertTolerance(0.00000001, LinAlg.DotProd(pt - p0, n));
HDebug.AssertTolerance(0.00000001, pt - (d * l + l0));
return(pt);
}
public static Vector DV(Matrix D, Vector V, bool assertDiag = true)
{
if (DV_SelfTest1)
{
DV_SelfTest1 = false;
MatrixByArr tD = new double[3, 3] {
{ 1, 0, 0 }, { 0, 2, 0 }, { 0, 0, 3 }
};
Vector tV = new double[3] {
1, 2, 3
};
Vector tDV = new double[3] {
1, 4, 9
};
// [1 0 0] [1] [1]
// [0 2 0] * [2] = [4]
// [0 0 3] [3] [9]
HDebug.AssertTolerance(double.Epsilon, DV(tD, tV) - tDV);
}
// D is the diagonal matrix
HDebug.Assert(D.ColSize == D.RowSize);
if (assertDiag) // check diagonal matrix
{
HDebug.AssertToleranceMatrix(double.Epsilon, D - Diag(Diag(D)));
}
HDebug.Assert(D.ColSize == V.Size);
Vector diagD = Diag(D);
Vector diagDV = DV(diagD, V);
return(diagDV);
}
public static double AngleBetween(Vector a, Vector b, Vector c)
{
if (AngleBetween_selftest2)
{
AngleBetween_selftest2 = false;
Vector ta, tb, tc;
double tang;
ta = new double[] { 1, 0, 0 };
tb = new double[] { 0, 0, 0 };
tc = new double[] { 0, 1, 0 };
tang = AngleBetween(ta, tb, tc);
HDebug.AssertTolerance(0.0001, Math.PI / 2 - tang);
ta = new double[] { 0, 1, 0 };
tb = new double[] { 0, 0, 0 };
tc = new double[] { 0, 1, 1 };
tang = AngleBetween(ta, tb, tc);
HDebug.AssertTolerance(0.0001, Math.PI / 4 - tang);
}
Vector left = a - b;
Vector right = c - b;
double ang = AngleBetween(left, right);
HDebug.Assert(ang >= 0);
HDebug.Assert(ang <= Math.PI);
return(ang);
}
//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 static Vector DV(Vector D, Vector V, bool assertDiag = true)
{
if (DV_SelfTest2)
{
DV_SelfTest2 = false;
Vector tD = new double[3] {
1, 2, 3
};
Vector tV = new double[3] {
1, 2, 3
};
Vector tDV = new double[3] {
1, 4, 9
};
// [1 0 0] [1] [1]
// [0 2 0] * [2] = [4]
// [0 0 3] [3] [9]
HDebug.AssertTolerance(double.Epsilon, DV(tD, tV) - tDV);
}
// D is the diagonal matrix
HDebug.Assert(D.Size == V.Size);
int size = V.Size;
Vector dv = new double[size];
for (int i = 0; i < size; i++)
{
dv[i] = D[i] * V[i];
}
return(dv);
}
public static double V1tD2V3(Vector V1, Matrix D2, Vector V3, bool assertDiag = true)
{
if (V1tD2V3_SelfTest)
{
V1tD2V3_SelfTest = false;
Vector tV1 = new double[3] {
1, 2, 3
};
MatrixByArr tD2 = new double[3, 3] {
{ 2, 0, 0 }, { 0, 3, 0 }, { 0, 0, 4 }
};
Vector tV3 = new double[3] {
3, 4, 5
};
// [2 ] [3] [ 6]
// [1 2 3] * [ 3 ] * [4] = [1 2 3] * [12] = 6+24+60 = 90
// [ 4] [5] [20]
double tV1tD2V3 = 90;
HDebug.AssertTolerance(double.Epsilon, tV1tD2V3 - V1tD2V3(tV1, tD2, tV3));
}
if (assertDiag) // check diagonal matrix
{
HDebug.AssertToleranceMatrix(double.Epsilon, D2 - Diag(Diag(D2)));
}
HDebug.Assert(V1.Size == D2.ColSize);
HDebug.Assert(D2.RowSize == V3.Size);
Vector lD2V3 = DV(D2, V3, assertDiag);
double lV1tD2V3 = VtV(V1, lD2V3);
return(lV1tD2V3);
}
public static Vector ClosestPointOnLine(Vector A, Vector B, Vector P, bool segmentClamp)
{
// http://www.gamedev.net/community/forums/topic.asp?topic_id=444154
HDebug.AssertAnd(A.Size == B.Size, B.Size == P.Size);
Vector AP = P - A;
Vector AB = B - A;
double ab2 = LinAlg.VtV(AB, AB);// AB.x*AB.x + AB.y*AB.y;
if (ab2 == 0)
{
return(A.Clone());
}
double ap_ab = LinAlg.VtV(AP, AB); //AP.x*AB.x + AP.y*AB.y;
double t0 = ap_ab / ab2;
double t = t0;
if (segmentClamp)
{
if (t < 0.0)
{
t = 0.0;
}
else if (t > 1.0)
{
t = 1.0;
}
}
Vector Closest = A + AB * t;
HDebug.AssertTolerance(0.000000001, LinAlg.VtV(B - A, P - (A + AB * t0)));
return(Closest);
}
public static PClosestPointOnSegment ClosestPointOnSegment(IList <Vector> ABs, Vector Pt)
{
HDebug.Assert(ABs.Count >= 2);
Vector closest = ABs[0];
//Tuple<Vector,Vector> closest_segment = new Tuple<Vector,Vector>(ABs[0], ABs[1]);
double closest_dist2 = (closest - Pt).Dist2;
double closest_dist = Math.Sqrt(closest_dist2);
int closest_iAB = 0;
for (int i = 1; i < ABs.Count; i++)
{
int iAB = i - 1;
Vector A = ABs[iAB];
Vector B = ABs[iAB + 1];
Vector C = ClosestPointOnLine(A, B, Pt, true);
double C_dist2 = (C - Pt).Dist2;
if (C_dist2 < closest_dist2)
{
closest = C;
closest_dist2 = C_dist2;
closest_dist = Math.Sqrt(C_dist2);
closest_iAB = iAB;
//closest_segment = new Tuple<Vector, Vector>(A, B);
}
}
HDebug.AssertTolerance(0.00000001, closest_dist - DistPointSegment(Pt, ABs));
return(new PClosestPointOnSegment
{
point = closest,
dist = closest_dist,
iAB = closest_iAB,
//segment = closest_segment,
});
}
public static Vector CrossProd3(Vector v1, Vector v2)
{
// http://en.wikipedia.org/wiki/Cross_product
HDebug.Assert(v1.Size == 3);
HDebug.Assert(v2.Size == 3);
if (CrossProd3_SelfTest)
{
CrossProd3_SelfTest = false;
Vector tv1 = new double[] { 1, 2, 3 };
Vector tv2 = new double[] { 4, 5, 6 };
Vector tv = CrossProd3(tv1, tv2);
HDebug.AssertTolerance(0, tv - new Vector(-3, 6, -3));
}
Vector cross = new double[3];
cross[0] = v1[1] * v2[2] - v1[2] * v2[1];
cross[1] = v1[2] * v2[0] - v1[0] * v2[2];
cross[2] = v1[0] * v2[1] - v1[1] * v2[0];
return(cross);
//HDebug.Assert(left.Size == 3, right.Size == 3);
//return new double[]{
// left[1]*right[2] - left[2]*right[1],
// left[2]*right[0] - left[0]*right[2],
// left[0]*right[1] - left[1]*right[0]
// };
}
public static Trans3 AppendTrans(Trans3 _trans, Trans3 dtrans)
{
//Trans3 newtrans = new Trans3(HTLib.Trans3.GetTransform(dtrans.trans, trans.trans));
Trans3 newtrans;
{ //public static Trans3 GetTransform(Trans3 trans2, Trans3 trans1)
Trans3 trans2 = dtrans;
Trans3 trans1 = _trans;
// trans2 * trans2
// DoubleMatrix4 mat2 = trans2.TransformMatrix;
// DoubleMatrix4 mat1 = trans1.TransformMatrix;
// DoubleMatrix4 trans_mat = mat2*mat1;
// DoubleVector3 dt = new DoubleVector3(trans_mat.m03, trans_mat.m13, trans_mat.m23);
Vector dt = trans2.DoTransform(trans1.dt);
double ds = trans2.ds * trans1.ds;
Quaternion dr = trans2.dr * trans1.dr; //Quaternion.GetQuaternion(trans_mat, ds);
Trans3 trans = new Trans3(dt, ds, dr);
if (HDebug.IsDebuggerAttached)
{
Vector pt1, pt2, pt12;
pt1 = new double[] { 10, 0, 0 };
pt2 = trans2.DoTransform(trans1.DoTransform(pt1));
pt12 = trans.DoTransform(pt1);
HDebug.Assert((pt12 - pt2).Dist < 0.001);
pt1 = new double[] { 0, 10, 0 };
pt2 = trans2.DoTransform(trans1.DoTransform(pt1));
pt12 = trans.DoTransform(pt1);
HDebug.Assert((pt12 - pt2).Dist < 0.001);
pt1 = new double[] { 0, 0, 10 };
pt2 = trans2.DoTransform(trans1.DoTransform(pt1));
pt12 = trans.DoTransform(pt1);
HDebug.Assert((pt12 - pt2).Dist < 0.001);
}
//return trans;
newtrans = trans;
}
if (HDebug.IsDebuggerAttached)
{
MatrixByArr mat = _trans.TransformMatrix;
MatrixByArr dmat = dtrans.TransformMatrix;
MatrixByArr newmat = newtrans.TransformMatrix;
MatrixByArr diff = newmat - dmat * mat;
HDebug.AssertTolerance(0.00001, diff);
}
return(newtrans);
}
public static Vector RotateTangentUnit(Vector pt, Vector axis1, Vector axis2)
{
Vector pt_on_axis = ClosestPointOnLine(axis1, axis2, pt, false);
Vector direct_pt = (pt - pt_on_axis).UnitVector();
Vector direct_axis = (axis2 - axis1).UnitVector();
Vector direct_tanpt = LinAlg.CrossProd(direct_axis, direct_pt).UnitVector();
if (HDebug.IsDebuggerAttached)
{
Vector dbg_axis = LinAlg.CrossProd(direct_pt, direct_tanpt).UnitVector();
HDebug.AssertTolerance(0.00000001, (direct_axis - dbg_axis).Dist);
}
return(direct_tanpt);
}
public static PlatonicSolid CloneByRadius(PlatonicSolid obj, double radius)
{
Vector[] verts = obj.verts.HCloneVectors().ToArray();
double rad = verts[0].Dist;
for (int i = 0; i < verts.Length; i++)
{
HDebug.AssertTolerance(0.00000001, rad - verts[i].Dist);
verts[i] = verts[i] * (radius / rad);
}
return(new PlatonicSolid
{
verts = verts
});
}
public static Vector[] ExtendSegmentBegin(IList <Vector> segment, double extlength)
{
List <Vector> extsegment = segment.HCloneVectors().ToList();
Vector vec10 = (segment[0] - segment[1]).UnitVector();
extsegment.Insert(0, extsegment[0] + vec10 * extlength);
if (HDebug.IsDebuggerAttached)
{
Vector vec01 = (extsegment[0] - extsegment[1]).UnitVector();
Vector vec12 = (extsegment[1] - extsegment[2]).UnitVector();
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(vec01, vec12) - 1);
}
return(extsegment.ToArray());
}
public static Tuple <Vector, Vector> LineOnTwoPlanes(Vector norm1, double val1, Vector norm2, double val2)
{
/// http://intumath.org/Math/Geometry/Analytic%20Geometry/plane-planeinter.html
///
if (LineOnTwoPlanes_SelfTest)
{
LineOnTwoPlanes_SelfTest = false;
Vector tn1 = new double[] { 1, 2, 3 };
Vector tn2 = new double[] { 2, 5, 3 };
double td1 = 10;
double td2 = 2;
LineOnTwoPlanes(tn1, td1, tn2, td2);
}
Vector norm = LinAlg.CrossProd(norm1, norm2);
/// norm1' * pt = -val1 [n1x n1y n1z] [ptx] [-val1] [n1x n1y 0] [ptx] [-val1]
/// norm2' * pt = -val2 => [n2x n2y n2z] * [pty] = [-val2] => [n2x n2y 0] * [pty] = [-val2]
/// [ptz] [0 ]
/// => [n1x n1y] * [ptx] = [-val1] => [n1x n1y] * [ptx] = [-val1]
/// [n2x n2y] [pty] [-val2] [n2x n2y] [pty] [-val2]
/// => [ptx] = [n1x n1y]-1 [-val1] = [ n2y -n1y] * [-val1] / (n1x*n2y - n1y*n2x)
/// [pty] [n2x n2y] * [-val2] [-n2x n1x] [-val2]
/// = [ n2y*-val1 + -n1y*-val2 ] / (n1x*n2y - n1y*n2x)
/// [-n2x*-val1 + n1x*-val2 ]
double n1x = norm1[0], n1y = norm1[1];
double n2x = norm2[0], n2y = norm2[1];
double div = n1x * n2y - n1y * n2x;
Vector pt = new double[] { (n2y * -val1 + -n1y * -val2) / div,
(-n2x * -val1 + n1x * -val2) / div,
0 };
if (HDebug.IsDebuggerAttached)
{
Vector dbg_pt;
dbg_pt = pt + 1 * norm;
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm1, dbg_pt) + val1);
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm2, dbg_pt) + val2);
dbg_pt = pt + 2 * norm;
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm1, dbg_pt) + val1);
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm2, dbg_pt) + val2);
dbg_pt = pt - 2 * norm;
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm1, dbg_pt) + val1);
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm2, dbg_pt) + val2);
}
return(new Tuple <Vector, Vector>(pt, norm));
}
//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 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 static Vector[] ExtendSegmentEnd(IList <Vector> segment, double extlength)
{
List <Vector> extsegment = segment.HCloneVectors().ToList();
int cnt = segment.Count;
Vector vec89 = (segment[cnt - 1] - segment[cnt - 2]).UnitVector();
extsegment.Add(extsegment[cnt - 1] + vec89 * extlength);
if (HDebug.IsDebuggerAttached)
{
int extcnt = extsegment.Count;
Vector extvec78 = (extsegment[extcnt - 3] - extsegment[extcnt - 2]).UnitVector();
Vector extvec89 = (extsegment[extcnt - 2] - extsegment[extcnt - 1]).UnitVector();
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(extvec78, extvec89) - 1);
}
return(extsegment.ToArray());
}
public static MatrixByArr Eye(int size, double diagval = 1)
{
if (Eye_SelfTest)
{
Eye_SelfTest = false;
MatrixByArr tT0 = new double[3, 3] {
{ 2, 0, 0 }, { 0, 2, 0 }, { 0, 0, 2 },
};
MatrixByArr tT1 = Eye(3, 2);
HDebug.AssertTolerance(double.Epsilon, tT0 - tT1);
}
MatrixByArr mat = new MatrixByArr(size, size);
for (int i = 0; i < size; i++)
{
mat[i, i] = diagval;
}
return(mat);
}
public static T[] Differences <T>(this IList <T> values)
{
if (Differences_SelfTest)
{
Differences_SelfTest = false;
double[] tvals = new double[] { 0, 1, 3, 6, 10, 15, 21 };
Vector tacs0 = new double[] { 1, 2, 3, 4, 5, 6 };
Vector tacs1 = Differences(tvals);
HDebug.AssertTolerance(double.Epsilon, tacs0 - tacs1);
}
T[] diffs = new T[values.Count - 1];
for (int i = 0; i < diffs.Length; i++)
{
dynamic val0 = values[i];
dynamic val1 = values[i + 1];
diffs[i] = (val1 - val0);
}
return(diffs);
}
public static T[] HAccumulate <T>(this IList <T> values)
{
if (HAccumulate_SelfTest)
{
HAccumulate_SelfTest = false;
double[] tvals = new double[] { 0, 1, 2, 4, 6, 9, 12 };
Vector tacs0 = new double[] { 0, 1, 3, 7, 13, 22, 34 };
Vector tacs1 = HAccumulate(tvals);
HDebug.AssertTolerance(double.Epsilon, tacs0 - tacs1);
}
T[] accs = new T[values.Count];
accs[0] = values[0];
for (int i = 1; i < accs.Length; i++)
{
dynamic val = values[i];
accs[i] = (val + accs[i - 1]);
}
return(accs);
}
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 override Tuple <ILinAlgMat, double[]> _EigSymm(ILinAlgMat A)
{
using (new Matlab.NamedLock("LA"))
{
Matlab.PutMatrix("LA.A", A.ToArray());
Matlab.Execute("LA.A = (LA.A + LA.A)/2;");
Matlab.Execute("[LA.V, LA.D] = eig(LA.A);");
Matlab.Execute("LA.D = diag(LA.D);");
CMatrix V = Matlab.GetMatrix("LA.V");
double[] D = Matlab.GetVector("LA.D");
if (HDebug.IsDebuggerAttached)
{
Matlab.Execute("LA.ERR = LA.A - (LA.V * diag(LA.D) * LA.V');");
Matlab.Execute("LA.ERR = max(max(abs(LA.ERR)));");
double err = Matlab.GetValue("LA.ERR");
HDebug.AssertTolerance(0.00000001, err);
}
Matlab.Clear();
return(new Tuple <ILinAlgMat, double[]>(V, D));
}
}
public static Vector MV <MATRIX>(MATRIX lmat, Vector rvec, string options = "")
where MATRIX : IMatrix <double>
{
Vector result = new Vector(lmat.ColSize);
MV(lmat, rvec, result, options);
if (MV_SelfTest_lmat_rvec)
{
MV_SelfTest_lmat_rvec = false;
HDebug.Assert(lmat.RowSize == rvec.Size);
Vector lresult = new Vector(lmat.ColSize);
for (int c = 0; c < lmat.ColSize; c++)
{
for (int r = 0; r < lmat.RowSize; r++)
{
lresult[c] += lmat[c, r] * rvec[r];
}
}
HDebug.AssertTolerance(double.Epsilon, lresult - result);
}
return(result);
}
public static MatrixByArr Diag(this Vector d)
{
if (Diagd_SelfTest)
{
Diagd_SelfTest = false;
MatrixByArr tD1 = new double[3, 3] {
{ 1, 0, 0 }, { 0, 2, 0 }, { 0, 0, 3 }
};
Vector td1 = new double[3] {
1, 2, 3
};
MatrixByArr tD = Diag(td1);
HDebug.AssertTolerance(double.Epsilon, tD - tD1);
}
int size = d.Size;
MatrixByArr D = new MatrixByArr(size, size);
for (int i = 0; i < size; i++)
{
D[i, i] = d[i];
}
return(D);
}
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);
}