From 5c0b44097e5cb91a5d9168cedf963bcbac6333cf Mon Sep 17 00:00:00 2001 From: rms Date: Sun, 22 Oct 2017 12:05:30 -0400 Subject: [PATCH] a bunch of useful functions, Box3 containment, some useful iterator classes. no functionality changes. --- approximation/GaussPointsFit3.cs | 74 ++++++++++++++++++++++++++------ containment/ContBox3.cs | 38 ++++++++++++++++ core/g3Iterators.cs | 51 ++++++++++++++++++++++ math/Box3.cs | 42 ++++++++++++++++++ math/Triangle3.cs | 10 +++++ mesh/DMesh3.cs | 8 ++++ mesh/MeshMeasurements.cs | 11 +++++ queries/MeshQueries.cs | 71 ++++++++++++++++++++++++++++-- spatial/SpatialFunctions.cs | 3 ++ 9 files changed, 292 insertions(+), 16 deletions(-) create mode 100644 containment/ContBox3.cs create mode 100644 core/g3Iterators.cs diff --git a/approximation/GaussPointsFit3.cs b/approximation/GaussPointsFit3.cs index d5fbe509..0f66cbb4 100644 --- a/approximation/GaussPointsFit3.cs +++ b/approximation/GaussPointsFit3.cs @@ -40,12 +40,61 @@ public GaussPointsFit3(IEnumerable 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 points, IEnumerable weights) + { + Box = new Box3d(Vector3d.Zero, Vector3d.One); + + // Compute the mean of the points. + int numPoints = 0; + double weightSum = 0; + IEnumerator 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 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]); + } } diff --git a/containment/ContBox3.cs b/containment/ContBox3.cs new file mode 100644 index 00000000..f466a515 --- /dev/null +++ b/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 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 points, IEnumerable 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); + } + } +} diff --git a/core/g3Iterators.cs b/core/g3Iterators.cs new file mode 100644 index 00000000..a90b0f24 --- /dev/null +++ b/core/g3Iterators.cs @@ -0,0 +1,51 @@ +using System; +using System.Collections; +using System.Collections.Generic; + + +namespace g3 +{ + /* + * Utility generic iterators + */ + + /// + /// Iterator that just returns a constant value N times + /// + public class ConstantItr : IEnumerable + { + public T ConstantValue = default(T); + public int N; + + public ConstantItr(int count, T constant) { + N = count; ConstantValue = constant; + } + public IEnumerator GetEnumerator() { + for (int i = 0; i < N; ++i) + yield return ConstantValue; + } + IEnumerator IEnumerable.GetEnumerator() { return GetEnumerator(); } + } + + + /// + /// Iterator that re-maps iterated values via a Func + /// + public class RemapItr : IEnumerable + { + public IEnumerable OtherItr; + public Func ValueF; + + public RemapItr(IEnumerable otherIterator, Func valueFunction) + { + OtherItr = otherIterator; ValueF = valueFunction; + } + public IEnumerator GetEnumerator() { + foreach (T2 idx in OtherItr) + yield return ValueF(idx); + } + IEnumerator IEnumerable.GetEnumerator() { return GetEnumerator(); } + } + + +} diff --git a/math/Box3.cs b/math/Box3.cs index b5c6904e..80184d42 100644 --- a/math/Box3.cs +++ b/math/Box3.cs @@ -1,4 +1,5 @@ using System; +using System.Collections.Generic; namespace g3 { @@ -157,6 +158,47 @@ public void Contain( Vector3d v) { } } + + /// + /// update the box to contain set of input points. More efficient tha ncalling Contain() many times + /// code ported from GTEngine GteContOrientedBox3.h + /// + public void Contain(IEnumerable 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 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 ) { diff --git a/math/Triangle3.cs b/math/Triangle3.cs index f6c10c61..c918870e 100644 --- a/math/Triangle3.cs +++ b/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); + } } } diff --git a/mesh/DMesh3.cs b/mesh/DMesh3.cs index 12d160f3..7449640d 100644 --- a/mesh/DMesh3.cs +++ b/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) { diff --git a/mesh/MeshMeasurements.cs b/mesh/MeshMeasurements.cs index 329c9ad2..0badd239 100644 --- a/mesh/MeshMeasurements.cs +++ b/mesh/MeshMeasurements.cs @@ -161,6 +161,17 @@ public static Vector3d Centroid(IEnumerable vertices) return centroid / (double)N; } + public static Vector3d Centroid(IEnumerable values, Func 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) { diff --git a/queries/MeshQueries.cs b/queries/MeshQueries.cs index b18e310f..01494f51 100644 --- a/queries/MeshQueries.cs +++ b/queries/MeshQueries.cs @@ -8,7 +8,9 @@ namespace g3 public static class MeshQueries { - // convenience function to construct a DistPoint3Triangle3 object for a mesh triangle + /// + /// construct a DistPoint3Triangle3 object for a mesh triangle + /// 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; } + /// + /// Find point-normal frame at closest point to queryPoint on mesh. + /// Returns interpolated vertex-normal frame if available, otherwise tri-normal frame. + /// + 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 + /// + /// convenience function to construct a IntrRay3Triangle3 object for a mesh triangle + /// 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? + /// + /// 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. + /// + 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; + } + + + /// + /// 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. + /// + 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)); + } + + + + + + /// + /// Compute distance from point to triangle in mesh, with minimal extra objects/etc + /// public static double TriDistanceSqr(DMesh3 mesh, int ti, Vector3d point) { Vector3d V0 = Vector3d.Zero, V1 = Vector3d.Zero, V2 = Vector3d.Zero; diff --git a/spatial/SpatialFunctions.cs b/spatial/SpatialFunctions.cs index 6d48358a..f99b1a4d 100644 --- a/spatial/SpatialFunctions.cs +++ b/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;