diff --git a/src/NwcIO/Model/Clash/NwcTriangleIntersection.cs b/src/NwcIO/Model/Clash/NwcTriangleIntersection.cs index ff8c396..1ba4bfb 100644 --- a/src/NwcIO/Model/Clash/NwcTriangleIntersection.cs +++ b/src/NwcIO/Model/Clash/NwcTriangleIntersection.cs @@ -18,76 +18,67 @@ public static bool TrianglesIntersect( { intersectionPoint = Vector3d.Zero; - // Plane 1: N1 . X + d1 = 0 + // Unit normals make plane distances independent of triangle area. var e1 = v1 - v0; var e2 = v2 - v0; var n1 = Vector3d.Cross(e1, e2); - double d1 = -Vector3d.Dot(n1, v0); + var f1 = u1 - u0; + var f2 = u2 - u0; + var n2 = Vector3d.Cross(f1, f2); + var length1 = Math.Sqrt(Vector3d.Dot(n1, n1)); + var length2 = Math.Sqrt(Vector3d.Dot(n2, n2)); + if (length1 == 0.0 || length2 == 0.0) + { + return false; + } + + n1 /= length1; + n2 /= length2; + var size = Math.Sqrt(Math.Max( + Math.Max(Vector3d.Dot(e1, e1), Math.Max(Vector3d.Dot(e2, e2), Vector3d.Dot(v2 - v1, v2 - v1))), + Math.Max(Vector3d.Dot(f1, f1), Math.Max(Vector3d.Dot(f2, f2), Vector3d.Dot(u2 - u1, u2 - u1))))); + var distanceTolerance = Epsilon * size; // Distances of triangle U to plane 1 - double du0 = Vector3d.Dot(n1, u0) + d1; - double du1 = Vector3d.Dot(n1, u1) + d1; - double du2 = Vector3d.Dot(n1, u2) + d1; + var du0 = Vector3d.Dot(n1, u0 - v0); + var du1 = Vector3d.Dot(n1, u1 - v0); + var du2 = Vector3d.Dot(n1, u2 - v0); - if (Math.Abs(du0) < Epsilon) du0 = 0.0; - if (Math.Abs(du1) < Epsilon) du1 = 0.0; - if (Math.Abs(du2) < Epsilon) du2 = 0.0; + if (Math.Abs(du0) < distanceTolerance) du0 = 0.0; + if (Math.Abs(du1) < distanceTolerance) du1 = 0.0; + if (Math.Abs(du2) < distanceTolerance) du2 = 0.0; double du0du1 = du0 * du1; double du0du2 = du0 * du2; if (du0du1 > 0.0 && du0du2 > 0.0) return false; // Same side of plane 1 - // Plane 2: N2 . X + d2 = 0 - var f1 = u1 - u0; - var f2 = u2 - u0; - var n2 = Vector3d.Cross(f1, f2); - double d2 = -Vector3d.Dot(n2, u0); - // Distances of triangle V to plane 2 - double dv0 = Vector3d.Dot(n2, v0) + d2; - double dv1 = Vector3d.Dot(n2, v1) + d2; - double dv2 = Vector3d.Dot(n2, v2) + d2; + var dv0 = Vector3d.Dot(n2, v0 - u0); + var dv1 = Vector3d.Dot(n2, v1 - u0); + var dv2 = Vector3d.Dot(n2, v2 - u0); - if (Math.Abs(dv0) < Epsilon) dv0 = 0.0; - if (Math.Abs(dv1) < Epsilon) dv1 = 0.0; - if (Math.Abs(dv2) < Epsilon) dv2 = 0.0; + if (Math.Abs(dv0) < distanceTolerance) dv0 = 0.0; + if (Math.Abs(dv1) < distanceTolerance) dv1 = 0.0; + if (Math.Abs(dv2) < distanceTolerance) dv2 = 0.0; double dv0dv1 = dv0 * dv1; double dv0dv2 = dv0 * dv2; if (dv0dv1 > 0.0 && dv0dv2 > 0.0) return false; // Same side of plane 2 - // Direction of intersection line + // Unit normals make this parallelism test dimensionless. var d = Vector3d.Cross(n1, n2); double maxD = Math.Max(Math.Abs(d.X), Math.Max(Math.Abs(d.Y), Math.Abs(d.Z))); if (maxD < Epsilon) { - // Triangles are nearly coplanar; check AABB overlap as conservative test - var minV = new Vector3d( - Math.Min(v0.X, Math.Min(v1.X, v2.X)), - Math.Min(v0.Y, Math.Min(v1.Y, v2.Y)), - Math.Min(v0.Z, Math.Min(v1.Z, v2.Z))); - var maxV = new Vector3d( - Math.Max(v0.X, Math.Max(v1.X, v2.X)), - Math.Max(v0.Y, Math.Max(v1.Y, v2.Y)), - Math.Max(v0.Z, Math.Max(v1.Z, v2.Z))); - - var minU = new Vector3d( - Math.Min(u0.X, Math.Min(u1.X, u2.X)), - Math.Min(u0.Y, Math.Min(u1.Y, u2.Y)), - Math.Min(u0.Z, Math.Min(u1.Z, u2.Z))); - var maxU = new Vector3d( - Math.Max(u0.X, Math.Max(u1.X, u2.X)), - Math.Max(u0.Y, Math.Max(u1.Y, u2.Y)), - Math.Max(u0.Z, Math.Max(u1.Z, u2.Z))); - - bool aabbIntersect = - minV.X <= maxU.X && maxV.X >= minU.X && - minV.Y <= maxU.Y && maxV.Y >= minU.Y && - minV.Z <= maxU.Z && maxV.Z >= minU.Z; - - if (aabbIntersect) + if (du0 != 0.0 || du1 != 0.0 || du2 != 0.0 || + dv0 != 0.0 || dv1 != 0.0 || dv2 != 0.0) + { + return false; + } + + if (CoplanarTrianglesIntersect(v0, v1, v2, u0, u1, u2, n1, distanceTolerance)) { intersectionPoint = (v0 + v1 + v2 + u0 + u1 + u2) / 6.0; return true; @@ -111,6 +102,56 @@ public static bool TrianglesIntersect( return true; } + private static bool CoplanarTrianglesIntersect( + in Vector3d v0, in Vector3d v1, in Vector3d v2, + in Vector3d u0, in Vector3d u1, in Vector3d u2, + in Vector3d normal, double tolerance) + { + // Drop the dominant normal axis and test the six edge normals in 2D. + var axis = Math.Abs(normal.X) > Math.Abs(normal.Y) ? 0 : 1; + if (Math.Abs(normal.Z) > (axis == 0 ? Math.Abs(normal.X) : Math.Abs(normal.Y))) + { + axis = 2; + } + + var a = Project(Vector3d.Zero, axis); + var b = Project(v1 - v0, axis); + var c = Project(v2 - v0, axis); + var d = Project(u0 - v0, axis); + var e = Project(u1 - v0, axis); + var f = Project(u2 - v0, axis); + return !IsSeparatingEdge(a, b, a, b, c, d, e, f, tolerance) && + !IsSeparatingEdge(b, c, a, b, c, d, e, f, tolerance) && + !IsSeparatingEdge(c, a, a, b, c, d, e, f, tolerance) && + !IsSeparatingEdge(d, e, a, b, c, d, e, f, tolerance) && + !IsSeparatingEdge(e, f, a, b, c, d, e, f, tolerance) && + !IsSeparatingEdge(f, d, a, b, c, d, e, f, tolerance); + } + + private static (double X, double Y) Project(in Vector3d point, int axis) + { + return axis == 0 ? (point.Y, point.Z) : axis == 1 ? (point.X, point.Z) : (point.X, point.Y); + } + + private static bool IsSeparatingEdge( + (double X, double Y) start, (double X, double Y) end, + (double X, double Y) a, (double X, double Y) b, (double X, double Y) c, + (double X, double Y) d, (double X, double Y) e, (double X, double Y) f, + double tolerance) + { + var x = start.Y - end.Y; + var y = end.X - start.X; + var pa = x * a.X + y * a.Y; + var pb = x * b.X + y * b.Y; + var pc = x * c.X + y * c.Y; + var pd = x * d.X + y * d.Y; + var pe = x * e.X + y * e.Y; + var pf = x * f.X + y * f.Y; + var margin = tolerance * Math.Sqrt(x * x + y * y); + return Math.Max(pa, Math.Max(pb, pc)) < Math.Min(pd, Math.Min(pe, pf)) - margin || + Math.Max(pd, Math.Max(pe, pf)) < Math.Min(pa, Math.Min(pb, pc)) - margin; + } + private static void ComputeInterval( in Vector3d a, in Vector3d b, in Vector3d c, double da, double db, double dc, @@ -119,6 +160,7 @@ private static void ComputeInterval( out double t0, out double t1, out Vector3d p0, out Vector3d p1) { + // Distances have already been snapped using the relative plane tolerance. if (dadb > 0.0) { // c is on the other side @@ -131,13 +173,13 @@ private static void ComputeInterval( p0 = Interpolate(a, b, da, db); p1 = Interpolate(c, b, dc, db); } - else if (db * dc > 0.0 || Math.Abs(da) > Epsilon) + else if (db * dc > 0.0 || da != 0.0) { // a is on the other side p0 = Interpolate(b, a, db, da); p1 = Interpolate(c, a, dc, da); } - else if (Math.Abs(db) > Epsilon) + else if (db != 0.0) { p0 = Interpolate(a, b, da, db); p1 = Interpolate(c, b, dc, db); @@ -155,7 +197,7 @@ private static void ComputeInterval( private static Vector3d Interpolate(in Vector3d p1, in Vector3d p2, double d1, double d2) { double diff = d1 - d2; - if (Math.Abs(diff) < Epsilon) return (p1 + p2) * 0.5; + if (diff == 0.0) return (p1 + p2) * 0.5; double alpha = d1 / diff; return p1 + (p2 - p1) * alpha; } diff --git a/tests/NwcIO.Tests/ClashDetectiveTests.cs b/tests/NwcIO.Tests/ClashDetectiveTests.cs index 762407d..a0d3915 100644 --- a/tests/NwcIO.Tests/ClashDetectiveTests.cs +++ b/tests/NwcIO.Tests/ClashDetectiveTests.cs @@ -8,6 +8,82 @@ namespace NwcIO.Tests; public sealed class ClashDetectiveTests { + [Theory] + [InlineData(0.001, 0.008, false)] + [InlineData(1, 0.008, false)] + [InlineData(1000, 0.008, false)] + [InlineData(0.001, 0.002, true)] + [InlineData(1, 0.002, true)] + [InlineData(1000, 0.002, true)] + [InlineData(0.001, 0.005, true)] + [InlineData(1, 0.005, true)] + [InlineData(1000, 0.005, true)] + public void TriangleIntersectionPerpendicularPairsAreScaleIndependent(double scale, double y, bool expected) + { + var v0 = Vector3d.Zero; + var v1 = new Vector3d(0.01, 0, 0) * scale; + var v2 = new Vector3d(0, 0.01, 0) * scale; + var u0 = new Vector3d(0.005, y, -0.005) * scale; + var u1 = new Vector3d(0.005, y, 0.005) * scale; + var u2 = new Vector3d(0.005, 0.02, 0) * scale; + + Assert.Equal(expected, NwcTriangleIntersection.TrianglesIntersect(v0, v1, v2, u0, u1, u2, out _)); + Assert.Equal(expected, NwcTriangleIntersection.TrianglesIntersect(u0, u1, u2, v0, v2, v1, out _)); + } + + [Theory] + [InlineData(0.001, 0)] + [InlineData(1, 0)] + [InlineData(1000, 0)] + [InlineData(0.001, 1)] + [InlineData(1, 1)] + [InlineData(1000, 1)] + [InlineData(0.001, 2)] + [InlineData(1, 2)] + [InlineData(1000, 2)] + public void TriangleIntersectionCoplanarPairsUseProjectedOverlap(double scale, int axis) + { + Vector3d Point(double x, double y) + { + return (axis == 0 ? new Vector3d(0, x, y) : + axis == 1 ? new Vector3d(x, 0, y) : new Vector3d(x, y, 0)) * scale; + } + + var v0 = Point(0, 0); + var v1 = Point(1, 0); + var v2 = Point(0, 1); + var pairs = new[] + { + (Point(1, 1), Point(0.6, 1), Point(1, 0.6), false), // Disjoint despite AABB overlap. + (Point(0.2, 0.2), Point(1, 0.2), Point(0.2, 1), true), // Crossing edges. + (Point(0.1, 0.1), Point(0.2, 0.1), Point(0.1, 0.2), true), // Containment. + (Point(1, 0), Point(0, 1), Point(1, 1), true), // Shared edge. + (Point(1, 0), Point(2, 0), Point(1, 1), true), // Shared vertex. + }; + + foreach (var (u0, u1, u2, expected) in pairs) + { + Assert.Equal(expected, NwcTriangleIntersection.TrianglesIntersect(v0, v1, v2, u0, u1, u2, out _)); + Assert.Equal(expected, NwcTriangleIntersection.TrianglesIntersect(u0, u2, u1, v0, v1, v2, out _)); + } + } + + [Theory] + [InlineData(0.001)] + [InlineData(1)] + [InlineData(1000)] + public void TriangleIntersectionRejectsZeroAreaTriangles(double scale) + { + var a = Vector3d.Zero; + var b = new Vector3d(1, 0, 0) * scale; + var c = new Vector3d(0, 1, 0) * scale; + foreach (var end in new[] { a, b * 0.5 }) + { + Assert.False(NwcTriangleIntersection.TrianglesIntersect(a, b, c, a, b, end, out _)); + Assert.False(NwcTriangleIntersection.TrianglesIntersect(a, b, end, a, b, c, out _)); + } + } + [Fact] public void TriangleIntersectionDetectsCrossingTriangles() {