added a bunch of distance functions, ray/triangle intersection

math/MathUtil.cs

@@ -67,6 +67,13 @@ public static double SignedClamp(double f, double fMin, double fMax) {
         }
 
 
+        // there are fast approximations to this...
+        public static double InvSqrt(double f)
+        {
+            return f / Math.Sqrt(f);
+        }
+
+
         public static float PlaneAngleD(Vector3f a, Vector3f b, int nPlaneNormalIdx = 1)
         {
             a[nPlaneNormalIdx] = b[nPlaneNormalIdx] = 0.0f;

math/Triangle3.cs

@@ -0,0 +1,71 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+namespace g3
+{
+    public struct Triangle3d
+    {
+        public Vector3d V0, V1, V2;
+
+        public Triangle3d(Vector3d v0, Vector3d v1, Vector3d v2)
+        {
+            V0 = v0; V1 = v1; V2 = v2;
+        }
+
+        public Vector3d this[int key]
+        {
+            get { return (key == 0) ? V0 : (key == 1) ? V1 : V2; }
+            set { if (key == 0) V0 = value; else if (key == 1) V1 = value; else V2 = value; }
+        }
+
+        public Vector3d PointAt(double bary0, double bary1, double bary2)
+        {
+            return bary0 * V0 + bary1 * V1 + bary2 * V2;
+        }
+        public Vector3d PointAt(Vector3d bary)
+        {
+            return bary.x* V0 + bary.y* V1 + bary.z* V2;
+        }
+
+        // conversion operators
+        public static implicit operator Triangle3d(Triangle3f v)
+        {
+            return new Triangle3d(v.V0, v.V1, v.V2);
+        }
+        public static explicit operator Triangle3f(Triangle3d v)
+        {
+            return new Triangle3f((Vector3f)v.V0, (Vector3f)v.V1, (Vector3f)v.V2);
+        }
+    }
+
+
+
+    public struct Triangle3f
+    {
+        public Vector3f V0, V1, V2;
+
+        public Triangle3f(Vector3f v0, Vector3f v1, Vector3f v2)
+        {
+            V0 = v0; V1 = v1; V2 = v2;
+        }
+
+        public Vector3f this[int key]
+        {
+            get { return (key == 0) ? V0 : (key == 1) ? V1 : V2; }
+            set { if (key == 0) V0 = value; else if (key == 1) V1 = value; else V2 = value; }
+        }
+
+
+        public Vector3f PointAt(float bary0, float bary1, float bary2)
+        {
+            return bary0 * V0 + bary1 * V1 + bary2 * V2;
+        }
+        public Vector3f PointAt(Vector3f bary)
+        {
+            return bary.x * V0 + bary.y * V1 + bary.z * V2;
+        }
+    }
+
+}

math/Vector3d.cs

@@ -250,5 +250,68 @@ public static explicit operator Vector3(Vector3d v)
 #endif
 
 
+
+
+        // complicated functions go down here...
+
+
+        public static void Orthonormalize(ref Vector3d u, ref Vector3d v, ref Vector3d w)
+        {
+            // If the input vectors are v0, v1, and v2, then the Gram-Schmidt
+            // orthonormalization produces vectors u0, u1, and u2 as follows,
+            //
+            //   u0 = v0/|v0|
+            //   u1 = (v1-(u0*v1)u0)/|v1-(u0*v1)u0|
+            //   u2 = (v2-(u0*v2)u0-(u1*v2)u1)/|v2-(u0*v2)u0-(u1*v2)u1|
+            //
+            // where |A| indicates length of vector A and A*B indicates dot
+            // product of vectors A and B.
+
+            // compute u0
+            u.Normalize();
+
+            // compute u1
+            double dot0 = u.Dot(v);
+            v -= dot0 * u;
+            v.Normalize();
+
+            // compute u2
+            double dot1 = v.Dot(w);
+            dot0 = u.Dot(w);
+            w -= dot0 * u + dot1 * v;
+            w.Normalize();
+        }
+
+
+        // Input W must be a unit-length vector.  The output vectors {U,V} are
+        // unit length and mutually perpendicular, and {U,V,W} is an orthonormal basis.
+        // ported from WildMagic5
+        public static void GenerateComplementBasis(ref Vector3d u, ref Vector3d v, Vector3d w)
+        {
+            double invLength;
+
+            if (Math.Abs(w.x) >= Math.Abs(w.y)) {
+                // W.x or W.z is the largest magnitude component, swap them
+                invLength = MathUtil.InvSqrt(w.x * w.x + w.z * w.z);
+                u.x = -w.z * invLength;
+                u.y = 0;
+                u.z = +w.x * invLength;
+                v.x = w.y * u.z;
+                v.y = w.z * u.x - w.x * u.z;
+                v.z = -w.y * u.x;
+            } else {
+                // W.y or W.z is the largest magnitude component, swap them
+                invLength = MathUtil.InvSqrt(w.y * w.y + w.z * w.z);
+                u.x = 0;
+                u.y = +w.z * invLength;
+                u.z = -w.y * invLength;
+                v.x = w.y * u.z - w.z * u.y;
+                v.y = -w.x * u.z;
+                v.z = w.x * u.y;
+            }
+        }
+
+
+
     }
 }

queries/DistLine3Ray3.cs

@@ -5,6 +5,9 @@
 
 namespace g3
 {
+    // ported from WildMagic 5 
+    // https://www.geometrictools.com/Downloads/Downloads.html
+
     public class DistLine3Ray3
     {
         Line3d line;
@@ -29,9 +32,9 @@ public Ray3d Ray
         public double RayParameter;
 
 
-        public DistLine3Ray3(Ray3d ray, Line3d Line)
+        public DistLine3Ray3(Ray3d rayIn, Line3d LineIn)
         {
-            this.ray = ray; this.line = Line;
+            this.ray = rayIn; this.line = LineIn;
         }
 
         static public double MinDistance(Ray3d r, Line3d s)
@@ -58,6 +61,9 @@ public double Get()
 
         public double GetSquared ()
         {
+            if (DistanceSquared >= 0)
+                return DistanceSquared;
+
             Vector3d kDiff = line.Origin - ray.Origin;
             double a01 = -line.Direction.Dot(ray.Direction);
             double b0 = kDiff.Dot(line.Direction);
@@ -98,6 +104,8 @@ public double GetSquared ()
             if (sqrDist < (double)0) {
                 sqrDist = (double)0;
             }
+            DistanceSquared = sqrDist;
+
             return sqrDist;
         }
 

queries/DistLine3Segment3.cs

@@ -0,0 +1,126 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+namespace g3
+{
+	// ported from WildMagic 5 
+	// https://www.geometrictools.com/Downloads/Downloads.html
+
+	public class DistLine3Segment3
+	{
+		Line3d line;
+		public Line3d Line
+		{
+			get { return line; }
+			set { line = value; DistanceSquared = -1.0; }
+		}
+
+		Segment3d segment;
+		public Segment3d Segment
+		{
+			get { return segment; }
+			set { segment = value; DistanceSquared = -1.0; }
+		}
+
+		public double DistanceSquared = -1.0;
+
+		public Vector3d LineClosest;
+		public double LineParameter;
+		public Vector3d SegmentClosest;
+		public double SegmentParameter;
+
+
+		public DistLine3Segment3( Line3d LineIn, Segment3d SegmentIn)
+		{
+			this.segment = SegmentIn; this.line = LineIn;
+		}
+
+		static public double MinDistance(Line3d line, Segment3d segment)
+		{
+			return new DistLine3Segment3( line, segment ).Get();
+		}
+		static public double MinDistanceLineParam( Line3d line, Segment3d segment )
+		{
+			return new DistLine3Segment3( line, segment ).Compute().LineParameter;
+		}
+
+
+		public DistLine3Segment3 Compute()
+		{
+			GetSquared();
+			return this;
+		}
+
+		public double Get()
+		{
+			return Math.Sqrt( GetSquared() );
+		}
+
+
+		public double GetSquared()
+		{
+			if (DistanceSquared >= 0)
+				return DistanceSquared;
+
+            Vector3d diff = line.Origin - segment.Center;
+            double a01 = -line.Direction.Dot(segment.Direction);
+            double b0 = diff.Dot(line.Direction);
+            double c = diff.LengthSquared;
+            double det = Math.Abs(1 - a01 * a01);
+            double b1, s0, s1, sqrDist, extDet;
+
+            if (det >= MathUtil.ZeroTolerance) {
+                // The line and segment are not parallel.
+                b1 = -diff.Dot(segment.Direction);
+                s1 = a01 * b0 - b1;
+                extDet = segment.Extent * det;
+
+                if (s1 >= -extDet) {
+                    if (s1 <= extDet) {
+                        // Two interior points are closest, one on the line and one
+                        // on the segment.
+                        double invDet = (1) / det;
+                        s0 = (a01 * b1 - b0) * invDet;
+                        s1 *= invDet;
+                        sqrDist = s0 * (s0 + a01 * s1 + (2) * b0) +
+                            s1 * (a01 * s0 + s1 + (2) * b1) + c;
+                    } else {
+                        // The endpoint e1 of the segment and an interior point of
+                        // the line are closest.
+                        s1 = segment.Extent;
+                        s0 = -(a01 * s1 + b0);
+                        sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
+                    }
+                } else {
+                    // The end point e0 of the segment and an interior point of the
+                    // line are closest.
+                    s1 = -segment.Extent;
+                    s0 = -(a01 * s1 + b0);
+                    sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
+                }
+            } else {
+                // The line and segment are parallel.  Choose the closest pair so that
+                // one point is at segment center.
+                s1 = 0;
+                s0 = -b0;
+                sqrDist = b0 * s0 + c;
+            }
+
+            LineClosest = line.Origin + s0 * line.Direction;
+            SegmentClosest = segment.Center + s1 * segment.Direction;
+            LineParameter = s0;
+            SegmentParameter = s1;
+
+            // Account for numerical round-off errors.
+            if (sqrDist < 0) {
+                sqrDist = 0;
+            }
+
+            DistanceSquared = sqrDist;
+			return sqrDist;
+		}
+	}
+
+}