Return to previous CalcBarycentric implementation in ConvexPolytope.c…

…s which also handles nearly degenerate cases.

src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs

@@ -116,72 +116,96 @@ public void CalculatePoints(in Triangle ctri, out JVector pA, out JVector pB)
 
     private bool CalcBarycentric(in Triangle tri, out JVector result)
     {
-        // The code in this function is largely based on the code
-        // "from (the book) Real-Time Collision Detection by Christer Ericson,
-        // published by Morgan Kaufmann Publishers, (c) 2005 Elsevier Inc".
+        bool clamped = false;
 
         JVector a = vertices[tri.A].V;
         JVector b = vertices[tri.B].V;
         JVector c = vertices[tri.C].V;
 
-        JVector ab = b - a;
-        JVector ac = c - a;
-
-        Real d1 = JVector.Dot(ab, a);
-        Real d2 = JVector.Dot(ac, a);
-
-        if (d1 > (Real)0.0 && d2 > (Real)0.0)
+        // Calculate the barycentric coordinates of the origin (0,0,0) projected
+        // onto the plane of the triangle.
+        //
+        // [W. Heidrich, Journal of Graphics, GPU, and Game Tools,Volume 10, Issue 3, 2005.]
+#pragma warning disable IDE0018
+        JVector u, v, w, tmp;
+#pragma warning restore IDE0018
+
+        JVector.Subtract(a, b, out u);
+        JVector.Subtract(a, c, out v);
+
+        float t = 1.0f / tri.NormalSq;
+
+        JVector.Cross(u, a, out tmp);
+        float gamma = JVector.Dot(tmp, tri.Normal) * t;
+        JVector.Cross(a, v, out tmp);
+        float beta = JVector.Dot(tmp, tri.Normal) * t;
+        float alpha = 1.0f - gamma - beta;
+
+        // Clamp the projected barycentric coordinates to lie within the triangle,
+        // such that the clamped coordinates are closest (euclidean) to the original point.
+        //
+        // [https://math.stackexchange.com/questions/1092912/find-closest-point-in-triangle-given-barycentric-coordinates-outside]
+        if (alpha >= 0.0f && beta < 0.0f)
         {
-            result = new JVector((Real)1.0, (Real)0.0, (Real)0.0);
-            return true;
-        }
+            t = JVector.Dot(a, u);
+            if (gamma < 0.0f && t > 0.0f)
+            {
+                beta = MathF.Min(1.0f, t / u.LengthSquared());
+                alpha = 1.0f - beta;
+                gamma = 0.0f;
+            }
+            else
+            {
+                gamma = MathF.Min(1.0f, MathF.Max(0.0f, JVector.Dot(a, v) / v.LengthSquared()));
+                alpha = 1.0f - gamma;
+                beta = 0.0f;
+            }
 
-        Real d3 = JVector.Dot(ab, b);
-        Real d4 = JVector.Dot(ac, b);
-        if (d3 < (Real)0.0 && d4 > d3)
-        {
-            result = new JVector(0, 1, 0);
-            return true;
+            clamped = true;
         }
-
-        Real vc = d1 * d4 - d3 * d2;
-        if (vc <= (Real)0.0 && d1 < (Real)0.0 && d3 > (Real)0.0)
+        else if (beta >= 0.0f && gamma < 0.0f)
         {
-            Real v = d1 / (d1 - d3);
-            result = new JVector((Real)1.0 - v, v, 0);
-            return true;
-        }
+            JVector.Subtract(b, c, out w);
+            t = JVector.Dot(b, w);
+            if (alpha < 0.0f && t > 0.0f)
+            {
+                gamma = MathF.Min(1.0f, t / w.LengthSquared());
+                beta = 1.0f - gamma;
+                alpha = 0.0f;
+            }
+            else
+            {
+                alpha = MathF.Min(1.0f, MathF.Max(0.0f, -JVector.Dot(b, u) / u.LengthSquared()));
+                beta = 1.0f - alpha;
+                gamma = 0.0f;
+            }
 
-        Real d5 = JVector.Dot(ab, c);
-        Real d6 = JVector.Dot(ac, c);
-        if (d6 < (Real)0.0 && d5 > d6)
-        {
-            result = new JVector(0, 0, 1);
-            return true;
+            clamped = true;
         }
-
-        Real vb = d5 * d2 - d1 * d6;
-        if (vb <= (Real)0.0 && d2 < (Real)0.0 && d6 > (Real)0.0)
+        else if (gamma >= 0.0f && alpha < 0.0f)
         {
-            Real w = d2 / (d2 - d6);
-            result = new JVector((Real)1.0 - w, 0, w);
-            return true;
-        }
+            JVector.Subtract(b, c, out w);
+            t = -JVector.Dot(c, v);
+            if (beta < 0.0f && t > 0.0f)
+            {
+                alpha = MathF.Min(1.0f, t / v.LengthSquared());
+                gamma = 1.0f - alpha;
+                beta = 0.0f;
+            }
+            else
+            {
+                beta = MathF.Min(1.0f, MathF.Max(0.0f, -JVector.Dot(c, w) / w.LengthSquared()));
+                gamma = 1.0f - beta;
+                alpha = 0.0f;
+            }
 
-        Real va = d3 * d6 - d5 * d4;
-        if (va <= (Real)0.0 && (d4 - d3) < (Real)0.0 && (d5 - d6) < (Real)0.0)
-        {
-            Real w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
-            result = new JVector(0, (Real)1.0 - w, w);
-            return true;
+            clamped = true;
         }
 
-        Real d = (Real)1.0 / (va + vb + vc);
-        Real vf = vb * d;
-        Real wf = vc * d;
-
-        result = new JVector((Real)1.0 - vf - wf, vf, wf);
-        return false;
+        result.X = alpha;
+        result.Y = beta;
+        result.Z = gamma;
+        return clamped;
     }
 
     [MethodImpl(MethodImplOptions.AggressiveInlining)]