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 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);
}
static void GetTriGeom(Vector pt1, Vector pt2, Vector pt3
, out double a, out double b, out double c
, out double A, out double B, out double C
, out double r
, out Vector pto
)
{
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 = pt2 - pt3; double dist23 = pt23.Dist;
Vector pt31 = pt3 - pt1; double dist31 = pt31.Dist;
Vector pt12 = pt1 - pt2; 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(pt3, pt1, pt2); // angle A = ∠pt3-pt1(a)-pt2
B = angle2(pt1, pt2, pt3); // angle B = ∠pt1-pt2(b)-pt3
C = angle2(pt2, pt3, pt1); // 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(pt1 - pt2, pt2 - pt3).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;
pto = v1 * pt1 + v2 * pt2 + v3 * pt3;
}
}
public static Trans3 GetTransformNoScale(Vector from1, Vector from2, Vector to1, Vector to2)
{
//HTLib.DoubleVector3 lfrom1 = new HTLib.DoubleVector3(from1);
//HTLib.DoubleVector3 lfrom2 = new HTLib.DoubleVector3(from2);
//HTLib.DoubleVector3 lto1 = new HTLib.DoubleVector3(to1 );
//HTLib.DoubleVector3 lto2 = new HTLib.DoubleVector3(to2 );
//HTLib.Trans3 ltrans = HTLib.Trans3.GetTransformNoScale(lfrom1, lfrom2, lto1, lto2);
//return new Trans3(ltrans);
{
double ds = 1;
//double dx = to2.x - from2.x; // dx + from2.x = to2.x
//double dy = to2.y - from2.y; // dx + from2.y = to2.y
//double dz = to2.z - from2.z; // dx + from2.z = to2.z
Vector from = (from2 - from1).UnitVector();
Vector to = (to2 - to1).UnitVector();
Quaternion dr;
double innerprod = LinAlg.DotProd(from, to); // InnerProduct(from,to);
if (innerprod >= 1)
{
HDebug.Assert(innerprod == 1);
dr = Quaternion.UnitRotation;
}
else
{
Vector axis = LinAlg.CrossProd(from, to); // CrossProduct(from, to);
if (axis.Dist2 == 0)
{
// to avoid degenerate cases
double mag = to.Dist / 100000000;
axis = LinAlg.CrossProd(from, to + (new double[] { mag, mag * 2, mag * 3 })); //Vector.CrossProduct(from, to+new DoubleVector3(mag, mag*2, mag*3));
}
double angle = Math.Acos(innerprod);
dr = new Quaternion(axis, angle);
HDebug.Assert(LinAlg.DotProd((dr.RotationMatrix * from), to) > 0.99999);
}
Trans3 trans = Trans3.GetTransform(-from1, 1, Quaternion.UnitRotation);
trans = Trans3.AppendTrans(trans, Trans3.GetTransform(new double[3], ds, dr));
trans = Trans3.AppendTrans(trans, Trans3.GetTransform(to1, 1, Quaternion.UnitRotation));
//new Trans3(dx, dy, dz, ds, dr);
if (HDebug.IsDebuggerAttached)
{
Vector fromto1 = trans.DoTransform(from1);
HDebug.Assert((fromto1 - to1).Dist < 0.000001);
Vector fromto2 = trans.DoTransform(from2);
HDebug.Assert(((fromto2 - fromto1).UnitVector() - (to2 - to1).UnitVector()).Dist < 0.000001);
}
return(trans);
}
}
// public static double DistanceLineLine(Line line1, Line line2)
// {
// // http://mathworld.wolfram.com/Line-LineDistance.html
// DoubleVector3 x1 = line1.Base;
// DoubleVector3 x2 = line1.Normal;
// DoubleVector3 x3 = line2.Base;
// DoubleVector3 x4 = line2.Normal;
// DoubleVector3 a = x2 - x1;
// DoubleVector3 b = x4 - x3;
// DoubleVector3 c = x3 - x1;
// DoubleVector3 axb = DoubleVector3.CrossProduct(a, b);
// double dist = Math.Abs(DoubleVector3.InnerProduct(c, axb)) / axb.Length;
// return dist;
// }
public static double AngleBetween(Vector left, Vector right)
{
double cos_angle = LinAlg.DotProd(left, right) / (left.Dist * right.Dist);
cos_angle = HMath.Between(-1, cos_angle, 1);
double angle = Math.Acos(cos_angle);
while (angle > Math.PI)
{
angle -= Math.PI;
}
while (angle < -Math.PI)
{
angle += Math.PI;
}
HDebug.Assert(angle >= 0);
HDebug.Assert(angle <= Math.PI);
return(angle);
}
// public static double DistancePointLineSegment(PointF point, PointF line1, PointF line2)
// {
// double distPointLine = DistancePointLine(point, line1, line2);
// double distPointLine1 = (line2 - point).Length;
// double distPointLine2 = (line1 - point).Length;
// return HMath.Mid(distPointLine, distPointLine1, distPointLine2);
// }
public static double DistancePointPlane(Vector pt, Plane plane)
{
// Return the signed distance.
// If the point is in the normal direction, its sign will be plus
// , otherwise (pt is in opposite of normal direction), sign will be negative.
//
// http://mathworld.wolfram.com/Point-PlaneDistance.html
// point: (X,Y,Z)
// plane: 0 = ax + by + cz + d
// = a(x-x0) + b(y-y0) + c(z-z0)
// = (a,b,c) . ((x,y,z) - (x0,y0,z0))
// dist = (ax0 + by0 + cz0 + d) / Sqrt(a*a + b*b + c*c)
// = (normal . (pt - base)) / abs(normal)
// = (normal . (pt - base))
// because abs(normal) = 1
double dist = (LinAlg.DotProd(plane.Normal, pt - plane.Base));
//dist = Math.Abs(dist);
return(dist);
}
// 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);
}
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 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);
}
}