public static double TorsionalAngle(Vector p1, Vector p2, Vector p3, Vector p4)
{
//function angle = torangle(atom1, atom2, atom3, atom4)
// plane_123 = cross(atom2-atom1, atom3-atom2); n1 = plane_123; n1 = n1 / sqrt(dot(n1,n1));
// plane_234 = cross(atom3-atom2, atom4-atom3); n2 = plane_234; n2 = n2 / sqrt(dot(n2,n2));
// vec_23 = atom3 - atom2; b = vec_23; b = b / sqrt(dot(b,b));
// angle = atan2(dot(cross(n1,n2),b), dot(n1,n2));
//end
Vector plane_123 = LinAlg.CrossProd(p2 - p1, p3 - p2); Vector n1 = plane_123.UnitVector();
Vector plane_234 = LinAlg.CrossProd(p3 - p2, p4 - p3); Vector n2 = plane_234.UnitVector();
Vector vec_23 = p3 - p2; Vector b = vec_23.UnitVector();
double angle = Math.Atan2(LinAlg.VtV(LinAlg.CrossProd(n1, n2), b), LinAlg.VtV(n1, n2));
{
Vector p12 = p2 - p1;
Vector p23 = p3 - p2;
Vector p34 = p4 - p3;
Vector nn1 = LinAlg.CrossProd(p12, p23).UnitVector();
Vector nn2 = LinAlg.CrossProd(p23, p34).UnitVector();
double a = AngleBetween(nn1, nn2);
HDebug.Assert(Math.Abs(Math.Abs(a) - Math.Abs(angle)) < 0.00000001);
}
return(angle);
}
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 bool CheckPointOnSegment(Vector point, Vector segment1, Vector segment2, double toleranceAlign = 0.001, double toleranceWithin = 0.000001)
{
if (CheckPointOnSegment_SelfTest)
{
CheckPointOnSegment_SelfTest = false;
HDebug.Assert(CheckPointOnSegment(new double[] { 2, 2, 2 }, new double[] { 1, 1, 1 }, new double[] { 3, 3, 3 }) == true);
HDebug.Assert(CheckPointOnSegment(new double[] { 0, 0, 0 }, new double[] { 1, 1, 1 }, new double[] { 3, 3, 3 }) == false);
HDebug.Assert(CheckPointOnSegment(new double[] { 4, 4, 4 }, new double[] { 1, 1, 1 }, new double[] { 3, 3, 3 }) == false);
HDebug.Assert(CheckPointOnSegment(new double[] { 1, 2, 2 }, new double[] { 1, 1, 1 }, new double[] { 3, 3, 3 }) == false);
}
Vector vec1p = (point - segment1); Vector uvec1p = vec1p.UnitVector();
Vector vec12 = (segment2 - segment1); Vector uvec12 = vec12.UnitVector();
double dot = Math.Abs(LinAlg.VtV(uvec1p, uvec12));
if (dot < 1.0 - toleranceAlign)
{
return(false); // not aligned
}
double leng12 = vec12.Dist;
double t = LinAlg.VtV(vec1p, uvec12);
if (t < -toleranceWithin)
{
return(false); // smaller than 0
}
if (t > leng12 + toleranceWithin)
{
return(false); // larger than leng12
}
return(true);
}
public static double VtMV(Vector lvec, Matrix mat, Vector rvec, string options = "")
{
Vector MV = LinAlg.MV(mat, rvec, options);
double VMV = LinAlg.VtV(lvec, MV);
//Debug.AssertToleranceIf(lvec.Size<100, 0.00000001, Vector.VtV(lvec, Vector.MV(mat, rvec)) - VMV);
return(VMV);
}
////////////////////////////////////////////////////////////////////////////////////
// Functions
public static Vector VtV(Vectors l, Vectors r)
{
HDebug.Assert(l.Length == r.Length);
int length = l.Length;
Vector result = new double[length];
for (int i = 0; i < length; i++)
{
result[i] = LinAlg.VtV(l[i], r[i]);
}
return(result);
}
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 double[,] DistPtPt(IList <Vector> points1, IList <Vector> points2)
{
HDebug.Assert(points1.Count == points2.Count);
double[,] dists = new double[points1.Count, points2.Count];
for (int c1 = 0; c1 < points1.Count; c1++)
{
for (int c2 = 0; c2 < points2.Count; c2++)
{
Vector pt1 = points1[c1];
Vector pt2 = points2[c2];
HDebug.Assert(pt1.Size == pt2.Size);
double dist2 = LinAlg.VtV(pt1, pt2);
dists[c1, c2] = Math.Sqrt(dist2);
}
}
return(dists);
}
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 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 bool CheckPointOnLine(Vector point, Vector line1, Vector line2, double toleranceAlign = 0.001)
{
if (CheckPointOnLine_SelfTest)
{
CheckPointOnLine_SelfTest = false;
HDebug.Assert(CheckPointOnLine(new double[] { 2, 2, 2 }, new double[] { 0, 0, 0 }, new double[] { 1, 1, 1 }) == true);
HDebug.Assert(CheckPointOnLine(new double[] { 1, 2, 2 }, new double[] { 0, 0, 0 }, new double[] { 1, 1, 1 }) == false);
}
Vector uvec1p = (point - line1).UnitVector();
Vector uvec12 = (line2 - line1).UnitVector();
double dot = Math.Abs(LinAlg.VtV(uvec1p, uvec12));
if (dot < 1.0 - toleranceAlign)
{
return(false);
}
return(true);
}
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));
}
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));
}