tested aabb ray-triangle

geometry3Sharp.csproj

@@ -66,6 +66,8 @@
     <Compile Include="implicit\ImplicitField.cs" />
     <Compile Include="implicit\ImplicitOperators.cs" />
     <Compile Include="implicit\MarchingQuads.cs" />
+    <Compile Include="intersection\IntrLine3AxisAlignedBox3.cs" />
+    <Compile Include="intersection\IntrRay3AxisAlignedBox3.cs" />
     <Compile Include="io\MaterialTypes.cs" />
     <Compile Include="io\STLReader.cs" />
     <Compile Include="io\STLWriter.cs" />

math/MathUtil.cs

@@ -24,10 +24,10 @@ public class MathUtil
         public const float Epsilonf = 1.192092896e-07F;
 
 
-        public static bool EpsilonEqual(double a, double b, double epsilon) {
+        public static bool EpsilonEqual(double a, double b, double epsilon = MathUtil.Epsilon) {
             return Math.Abs(a - b) < epsilon;
         }
-        public static bool EpsilonEqual(float a, float b, float epsilon) {
+        public static bool EpsilonEqual(float a, float b, float epsilon = MathUtil.Epsilonf) {
             return (float)Math.Abs(a - b) < epsilon;
         }
 

queries/MeshQueries.cs

@@ -22,6 +22,18 @@ public static DistPoint3Triangle3 TriangleDistance(DMesh3 mesh, int ti, Vector3d
 
 
 
+        // 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))
+                return null;
+            Triangle3d tri = new Triangle3d();
+            mesh.GetTriVertices(ti, ref tri.V0, ref tri.V1, ref tri.V2);
+            IntrRay3Triangle3 q = new IntrRay3Triangle3(ray, tri);
+            q.Find();
+            return q;
+        }
+
 
         // 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?
@@ -195,7 +207,7 @@ public static double TriDistanceSqr(DMesh3 mesh, int ti, Vector3d point)
         // brute force search for nearest triangle to point
         public static int FindNearestTriangle_LinearSearch(DMesh3 mesh, Vector3d p)
         {
-            int tNearest = -1;
+            int tNearest = DMesh3.InvalidID;
             double fNearestSqr = double.MaxValue;
             foreach ( int ti in mesh.TriangleIndices() ) {
                 double distSqr = TriDistanceSqr(mesh, ti, p);
@@ -208,5 +220,27 @@ public static int FindNearestTriangle_LinearSearch(DMesh3 mesh, Vector3d p)
         }
 
 
+        public static int FindHitTriangle_LinearSearch(DMesh3 mesh, Ray3d ray)
+        {
+            int tNearestID = DMesh3.InvalidID;
+            double fNearestT = double.MaxValue;
+            Triangle3d tri = new Triangle3d();
+            foreach ( int ti in mesh.TriangleIndices() ) {
+
+                // [TODO] optimize this
+                mesh.GetTriVertices(ti, ref tri.V0, ref tri.V1, ref tri.V2);
+                IntrRay3Triangle3 ray_tri_hit = new IntrRay3Triangle3(ray, tri);
+                if ( ray_tri_hit.Find() ) {
+                    if ( ray_tri_hit.RayParameter < fNearestT ) {
+                        fNearestT = ray_tri_hit.RayParameter;
+                        tNearestID = ti;
+                    }
+                }
+            }
+
+            return tNearestID;
+        }
+
+
     }
 }

spatial/DMeshAABBTree.cs

@@ -138,7 +138,7 @@ void find_nearest_tri(int iBox, Vector3d p, ref double fNearestSqr, ref int tID)
 
 
         public bool SupportsTriangleRayIntersection { get { return true; } }
-        public int FindNearestHitTriangle(Ray3d ray, double fMaxDist)
+        public int FindNearestHitTriangle(Ray3d ray, double fMaxDist = double.MaxValue)
         {
             if (mesh_timestamp != mesh.Timestamp)
                 throw new Exception("DMeshAABBTree3.FindNearestTriangle: mesh has been modified since tree construction");
@@ -169,12 +169,15 @@ void find_hit_triangle(int iBox, ref Ray3d ray, ref double fNearestT, ref int tI
                 }
 
             } else {                                // internal node, either 1 or 2 child boxes
+                double e = MathUtil.ZeroTolerancef;
+
                 int iChild1 = index_list[idx];
                 if ( iChild1 < 0 ) {                 // 1 child, descend if nearer than cur min-dist
                     iChild1 = (-iChild1) - 1;
                     double fChild1T = box_ray_intersect_t(iChild1, ray);
-                    if ( fChild1T <= fNearestT )
+                    if (fChild1T <= fNearestT + e) {
                         find_hit_triangle(iChild1, ref ray, ref fNearestT, ref tID);
+                    }
 
                 } else {                            // 2 children, descend closest first
                     iChild1 = iChild1 - 1;
@@ -183,16 +186,18 @@ void find_hit_triangle(int iBox, ref Ray3d ray, ref double fNearestT, ref int tI
                     double fChild1T = box_ray_intersect_t(iChild1, ray);
                     double fChild2T = box_ray_intersect_t(iChild2, ray);
                     if (fChild1T < fChild2T) {
-                        if (fChild1T < fNearestT) {
+                        if (fChild1T <= fNearestT + e) {
                             find_hit_triangle(iChild1, ref ray, ref fNearestT, ref tID);
-                            if (fChild2T < fNearestT)
+                            if (fChild2T <= fNearestT + e) {
                                 find_hit_triangle(iChild2, ref ray, ref fNearestT, ref tID);
+                            }
                         }
                     } else {
-                        if (fChild2T < fNearestT) {
+                        if (fChild2T <= fNearestT + e) {
                             find_hit_triangle(iChild2, ref ray, ref fNearestT, ref tID);
-                            if (fChild1T < fNearestT)
+                            if (fChild1T <= fNearestT + e) {
                                 find_hit_triangle(iChild1, ref ray, ref fNearestT, ref tID);
+                            }
                         }
                     }