a bunch of useful functions, Box3 containment, some useful iterator c…

…lasses. no functionality changes.

approximation/GaussPointsFit3.cs

@@ -40,12 +40,61 @@ public GaussPointsFit3(IEnumerable<Vector3d> points)
                 sumZZ += diff[2] * diff[2];
             }
 
-            sumXX *= invNumPoints;
-            sumXY *= invNumPoints;
-            sumXZ *= invNumPoints;
-            sumYY *= invNumPoints;
-            sumYZ *= invNumPoints;
-            sumZZ *= invNumPoints;
+            do_solve(sumXX, sumXY, sumXZ, sumYY, sumYZ, sumZZ, invNumPoints);
+        }
+
+
+
+
+
+        public GaussPointsFit3(IEnumerable<Vector3d> points, IEnumerable<double> weights)
+        {
+            Box = new Box3d(Vector3d.Zero, Vector3d.One);
+
+            // Compute the mean of the points.
+            int numPoints = 0;
+            double weightSum = 0;
+            IEnumerator<double> weights_itr = weights.GetEnumerator();
+            foreach (Vector3d v in points) {
+                weights_itr.MoveNext();
+                double w = weights_itr.Current;
+                Box.Center += w * v;
+                weightSum += w;
+                numPoints++;
+            }
+            double invWeightDivide = (1.0) / weightSum;
+            Box.Center *= invWeightDivide;
+
+            // Compute the covariance matrix of the points.
+            double sumXX = (double)0, sumXY = (double)0, sumXZ = (double)0;
+            double sumYY = (double)0, sumYZ = (double)0, sumZZ = (double)0;
+            weights_itr = weights.GetEnumerator();
+            foreach (Vector3d p in points) {
+                weights_itr.MoveNext();
+                double w = weights_itr.Current;
+                w *= w;
+                Vector3d diff = p - Box.Center;
+                sumXX += w * diff[0] * diff[0];
+                sumXY += w * diff[0] * diff[1];
+                sumXZ += w * diff[0] * diff[2];
+                sumYY += w * diff[1] * diff[1];
+                sumYZ += w * diff[1] * diff[2];
+                sumZZ += w * diff[2] * diff[2];
+            }
+
+            do_solve(sumXX, sumXY, sumXZ, sumYY, sumYZ, sumZZ, invWeightDivide * invWeightDivide);
+        }
+
+
+
+        void do_solve(double sumXX, double sumXY, double sumXZ, double sumYY, double sumYZ, double sumZZ, double invSumMultiplier)
+        {
+            sumXX *= invSumMultiplier;
+            sumXY *= invSumMultiplier;
+            sumXZ *= invSumMultiplier;
+            sumYY *= invSumMultiplier;
+            sumYZ *= invSumMultiplier;
+            sumZZ *= invSumMultiplier;
 
             double[] matrix = new double[] {
                 sumXX, sumXY, sumXZ,
@@ -57,12 +106,13 @@ public GaussPointsFit3(IEnumerable<Vector3d> points)
             SymmetricEigenSolver solver = new SymmetricEigenSolver(3, 4096);
             int iters = solver.Solve(matrix, SymmetricEigenSolver.SortType.Increasing);
             ResultValid = (iters > 0 && iters < SymmetricEigenSolver.NO_CONVERGENCE);
-
-            Box.Extent = new Vector3d(solver.GetEigenvalues());
-            double[] evectors = solver.GetEigenvectors();
-            Box.AxisX = new Vector3d(evectors[0], evectors[1], evectors[2]);
-            Box.AxisY = new Vector3d(evectors[3], evectors[4], evectors[5]);
-            Box.AxisZ = new Vector3d(evectors[6], evectors[7], evectors[8]);
+            if (ResultValid) {
+                Box.Extent = new Vector3d(solver.GetEigenvalues());
+                double[] evectors = solver.GetEigenvectors();
+                Box.AxisX = new Vector3d(evectors[0], evectors[1], evectors[2]);
+                Box.AxisY = new Vector3d(evectors[3], evectors[4], evectors[5]);
+                Box.AxisZ = new Vector3d(evectors[6], evectors[7], evectors[8]);
+            }
         }
 
 

containment/ContBox3.cs

@@ -0,0 +1,38 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace g3
+{
+
+    // ported from GTEngine GteContOrientedBox3.h
+    // (2017) url: https://www.geometrictools.com/GTEngine/Include/Mathematics/GteContOrientedBox3.h
+    public class ContOrientedBox3
+    {
+        public Box3d Box;
+        public bool ResultValid = false;
+
+        public ContOrientedBox3(IEnumerable<Vector3d> points)
+        {
+            // Fit the points with a Gaussian distribution.
+            GaussPointsFit3 fitter = new GaussPointsFit3(points);
+            if (fitter.ResultValid == false)
+                return;
+            this.Box = fitter.Box;
+            this.Box.Contain(points);
+        }
+
+        public ContOrientedBox3(IEnumerable<Vector3d> points, IEnumerable<double> pointWeights)
+        {
+            // Fit the points with a Gaussian distribution.
+            GaussPointsFit3 fitter = new GaussPointsFit3(points, pointWeights);
+            if (fitter.ResultValid == false)
+                return;
+            this.Box = fitter.Box;
+            this.Box.Contain(points);
+        }
+    }
+}

core/g3Iterators.cs

@@ -0,0 +1,51 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+
+
+namespace g3
+{
+    /*
+     * Utility generic iterators
+     */
+
+    /// <summary>
+    /// Iterator that just returns a constant value N times
+    /// </summary>
+    public class ConstantItr<T> : IEnumerable<T>
+    {
+        public T ConstantValue = default(T);
+        public int N;
+
+        public ConstantItr(int count, T constant) {
+            N = count; ConstantValue = constant;
+        }
+        public IEnumerator<T> GetEnumerator() {
+            for (int i = 0; i < N; ++i)
+                yield return ConstantValue;
+        }
+        IEnumerator IEnumerable.GetEnumerator() { return GetEnumerator(); }
+    }
+
+
+    /// <summary>
+    /// Iterator that re-maps iterated values via a Func
+    /// </summary>
+    public class RemapItr<T,T2> : IEnumerable<T>
+    {
+        public IEnumerable<T2> OtherItr;
+        public Func<T2, T> ValueF;
+
+        public RemapItr(IEnumerable<T2> otherIterator, Func<T2,T> valueFunction)
+        {
+            OtherItr = otherIterator; ValueF = valueFunction;
+        }
+        public IEnumerator<T> GetEnumerator() {
+            foreach (T2 idx in OtherItr) 
+                yield return ValueF(idx);
+        }
+        IEnumerator IEnumerable.GetEnumerator() { return GetEnumerator(); }
+    }
+
+
+}

math/Box3.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Generic;
 
 namespace g3 {
 
@@ -157,6 +158,47 @@ public void Contain( Vector3d v) {
 			}			
 		}
 
+
+        /// <summary>
+        /// update the box to contain set of input points. More efficient tha ncalling Contain() many times
+        /// code ported from GTEngine GteContOrientedBox3.h 
+        /// </summary>
+        public void Contain(IEnumerable<Vector3d> points)
+        {
+            // Let C be the box center and let U0, U1, and U2 be the box axes.
+            // Each input point is of the form X = C + y0*U0 + y1*U1 + y2*U2.
+            // The following code computes min(y0), max(y0), min(y1), max(y1),
+            // min(y2), and max(y2).  The box center is then adjusted to be
+            //   C' = C + 0.5*(min(y0)+max(y0))*U0 + 0.5*(min(y1)+max(y1))*U1 + 0.5*(min(y2)+max(y2))*U2
+            IEnumerator<Vector3d> points_itr = points.GetEnumerator();
+            points_itr.MoveNext();
+
+            Vector3d diff = points_itr.Current - Center;
+            Vector3d pmin = new Vector3d( diff.Dot(AxisX), diff.Dot(AxisY), diff.Dot(AxisZ));
+            Vector3d pmax = pmin;
+            while (points_itr.MoveNext()) {
+                diff = points_itr.Current - Center;
+
+                double dotx = diff.Dot(AxisX);
+                if (dotx < pmin[0]) pmin[0] = dotx;
+                else if (dotx > pmax[0]) pmax[0] = dotx;
+
+                double doty = diff.Dot(AxisY);
+                if (doty < pmin[1]) pmin[1] = doty;
+                else if (doty > pmax[1]) pmax[1] = doty;
+
+                double dotz = diff.Dot(AxisZ);
+                if (dotz < pmin[2]) pmin[2] = dotz;
+                else if (dotz > pmax[2]) pmax[2] = dotz;
+            }
+            for (int j = 0; j < 3; ++j) {
+                Center += (((double)0.5) * (pmin[j] + pmax[j])) * Axis(j);
+                Extent[j] = ((double)0.5) * (pmax[j] - pmin[j]);
+            }
+        }
+
+
+
 		// I think this can be more efficient...no? At least could combine
 		// all the axis-interval updates before updating Center...
 		public void Contain( Box3d o ) {

math/Triangle3.cs

@@ -29,6 +29,11 @@ public Vector3d PointAt(Vector3d bary)
             return bary.x* V0 + bary.y* V1 + bary.z* V2;
         }
 
+        public Vector3d BarycentricCoords(Vector3d point)
+        {
+            return MathUtil.BarycentricCoords(point, V0, V1, V2);
+        }
+
         // conversion operators
         public static implicit operator Triangle3d(Triangle3f v)
         {
@@ -66,6 +71,11 @@ public Vector3f PointAt(Vector3f bary)
         {
             return bary.x * V0 + bary.y * V1 + bary.z * V2;
         }
+
+        public Vector3f BarycentricCoords(Vector3f point)
+        {
+            return (Vector3f)MathUtil.BarycentricCoords(point, V0, V1, V2);
+        }
     }
 
 }

mesh/DMesh3.cs

@@ -1467,6 +1467,14 @@ public bool IsBoundaryVertex(int vid) {
         }
 
 
+        public bool IsBoundaryTriangle(int tID)
+        {
+            debug_check_is_triangle(tID);
+            int i = 3 * tID;
+            return IsBoundaryEdge(triangle_edges[i]) || IsBoundaryEdge(triangle_edges[i + 1]) || IsBoundaryEdge(triangle_edges[i + 2]);
+        }
+
+
 
         int find_edge(int vA, int vB)
         {

mesh/MeshMeasurements.cs

@@ -161,6 +161,17 @@ public static Vector3d Centroid(IEnumerable<Vector3d> vertices)
             return centroid / (double)N;
         }
 
+        public static Vector3d Centroid<T>(IEnumerable<T> values, Func<T, Vector3d> PositionF)
+        {
+            Vector3d centroid = Vector3d.Zero;
+            int N = 0;
+            foreach (T t in values) { 
+                centroid += PositionF(t);
+                N++;
+            }
+            return centroid / (double)N;
+        }
+
 
         public static Vector3d Centroid(DMesh3 mesh, bool bOnlyTriVertices = true)
         {

queries/MeshQueries.cs

@@ -8,7 +8,9 @@ namespace g3
     public static class MeshQueries
     {
 
-        // convenience function to construct a DistPoint3Triangle3 object for a mesh triangle
+        /// <summary>
+        /// construct a DistPoint3Triangle3 object for a mesh triangle
+        /// </summary>
         public static DistPoint3Triangle3 TriangleDistance(DMesh3 mesh, int ti, Vector3d point)
         {
             if (!mesh.IsTriangle(ti))
@@ -20,9 +22,24 @@ public static DistPoint3Triangle3 TriangleDistance(DMesh3 mesh, int ti, Vector3d
             return q;
         }
 
+        /// <summary>
+        /// Find point-normal frame at closest point to queryPoint on mesh.
+        /// Returns interpolated vertex-normal frame if available, otherwise tri-normal frame.
+        /// </summary>
+        public static Frame3f NearestPointFrame(DMesh3 mesh, ISpatial spatial, Vector3d queryPoint)
+        {
+            int tid = spatial.FindNearestTriangle(queryPoint);
+            Vector3d surfPt = TriangleDistance(mesh, tid, queryPoint).TriangleClosest;
+            if (mesh.HasVertexNormals)
+                return SurfaceFrame(mesh, tid, surfPt);
+            else
+                return new Frame3f(surfPt, mesh.GetTriNormal(tid));
+        }
 
 
-        // convenience function to construct a IntrRay3Triangle3 object for a mesh triangle
+        /// <summary>
+        /// convenience function to construct a IntrRay3Triangle3 object for a mesh triangle
+        /// </summary>
         public static IntrRay3Triangle3 TriangleIntersection(DMesh3 mesh, int ti, Ray3d ray)
         {
             if (!mesh.IsTriangle(ti))
@@ -35,8 +52,54 @@ public static IntrRay3Triangle3 TriangleIntersection(DMesh3 mesh, int ti, Ray3d
         }
 
 
-        // compute distance from point to triangle ti in mesh, with minimal extra objects/etc
-        // TODO: take in current-max-distance so we can early-out?
+        /// <summary>
+        /// Find point-normal frame at ray-intersection point on mesh, or return false if no hit.
+        /// Returns interpolated vertex-normal frame if available, otherwise tri-normal frame.
+        /// </summary>
+        public static bool RayHitPointFrame(DMesh3 mesh, ISpatial spatial, Ray3d ray, out Frame3f hitPosFrame)
+        {
+            hitPosFrame = new Frame3f();
+            int tid = spatial.FindNearestHitTriangle(ray);
+            if (tid == DMesh3.InvalidID)
+                return false;
+            var isect = TriangleIntersection(mesh, tid, ray);
+            if (isect.Result != IntersectionResult.Intersects)
+                return false;
+            Vector3d surfPt = ray.PointAt(isect.RayParameter);
+            if (mesh.HasVertexNormals)
+                hitPosFrame = SurfaceFrame(mesh, tid, surfPt);
+            else
+                hitPosFrame = new Frame3f(surfPt, mesh.GetTriNormal(tid));
+            return true;
+        }
+
+
+        /// <summary>
+        /// Get point-normal frame on surface of mesh. Assumption is that point lies in tID.
+        /// returns interpolated vertex-normal frame if available, otherwise tri-normal frame.
+        /// </summary>
+        public static Frame3f SurfaceFrame(DMesh3 mesh, int tID, Vector3d point)
+        {
+            if (!mesh.IsTriangle(tID))
+                throw new Exception("MeshQueries.SurfaceFrame: triangle " + tID + " does not exist!");
+            Triangle3d tri = new Triangle3d();
+            mesh.GetTriVertices(tID, ref tri.V0, ref tri.V1, ref tri.V2);
+            Vector3d bary = tri.BarycentricCoords(point);
+            point = tri.PointAt(bary);
+            if (mesh.HasVertexNormals) {
+                Vector3d normal = mesh.GetTriBaryNormal(tID, bary.x, bary.y, bary.z);
+                return new Frame3f(point, normal);
+            } else
+                return new Frame3f(point, mesh.GetTriNormal(tID));
+        }
+
+
+
+
+
+        /// <summary>
+        /// Compute distance from point to triangle in mesh, with minimal extra objects/etc
+        /// </summary>
         public static double TriDistanceSqr(DMesh3 mesh, int ti, Vector3d point)
         {
             Vector3d V0 = Vector3d.Zero, V1 = Vector3d.Zero, V2 = Vector3d.Zero;

spatial/SpatialFunctions.cs

@@ -5,12 +5,15 @@
 namespace g3
 {
 
+    // [TODO] delete this file if nobody is using NormalOffset
+
     // collection of utility classes
     public static class SpatialFunctions
     {
 
         // various offset-surface functions, in class so the compute functions 
         // can be passed to other functions
+        [System.Obsolete("NormalOffset is deprecated - is anybody using it? please lmk.")]
         public class NormalOffset
         {
             public DMesh3 Mesh;