Dev

src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs

@@ -160,7 +160,7 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB,
         }
 
         EdgeContactEvaluation evaluation = EvaluateTriangleContact(triangleShape, c2,
-            c1 ? pointA : pointB, normal, out JVector tnormal, out JVector nnormal);
+            c1 ? pointA : pointB, normal, out JVector triangleNormal, out JVector neighborNormal);
 
         if (evaluation == EdgeContactEvaluation.Discard) return false;
         if (evaluation == EdgeContactEvaluation.Keep) return true;
@@ -184,37 +184,37 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB,
 
             if (!result)
             {
-                // this should not happen
+                // MPR refinement failed; reject the contact.
                 return false;
             }
 
             evaluation = EvaluateTriangleContact(triangleShape, c2,
-                c1 ? pointA : pointB, normal, out tnormal, out nnormal);
+                c1 ? pointA : pointB, normal, out triangleNormal, out neighborNormal);
 
             if (evaluation == EdgeContactEvaluation.Discard) return false;
             if (evaluation == EdgeContactEvaluation.Keep) return true;
         }
 
         JVector midPoint = (Real)0.5 * (pointA + pointB);
 
-        // now the fun part
+        // Now the fun part.
         //
         // we have a collision close to an edge, with
         //
-        // tnormal -> the triangle normal where collision occurred
-        // nnormal -> the normal of neighboring triangle
-        // normal  -> the collision normal
-        if (JVector.Dot(tnormal, nnormal) > cosAngle)
+        // triangleNormal -> the triangle normal where collision occurred
+        // neighborNormal -> the normal of the neighboring triangle
+        // normal         -> the collision normal
+        if (JVector.Dot(triangleNormal, neighborNormal) > cosAngle)
         {
-            // tnormal and nnormal are the same
-            // --------------------------------
-            Real f5 = JVector.Dot(normal, nnormal);
-            Real f6 = JVector.Dot(normal, tnormal);
+            // triangleNormal and neighborNormal are the same
+            // ----------------------------------------------
+            Real f5 = JVector.Dot(normal, neighborNormal);
+            Real f6 = JVector.Dot(normal, triangleNormal);
 
             if (f5 > f6)
             {
 #if DEBUG_EDGEFILTER
-                Console.WriteLine($"case #1: adjusting; normal {normal} -> {nnormal}");
+                Console.WriteLine($"case #1: adjusting; normal {normal} -> {neighborNormal}");
 #endif
 
                 if (!isSpeculative)
@@ -223,12 +223,12 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB,
                     pointA = pointB = midPoint;
                 }
 
-                normal = nnormal;
+                normal = neighborNormal;
             }
             else
             {
 #if DEBUG_EDGEFILTER
-                Console.WriteLine($"case #1: adjusting; normal {normal} -> {tnormal}");
+                Console.WriteLine($"case #1: adjusting; normal {normal} -> {triangleNormal}");
 #endif
 
                 if (!isSpeculative)
@@ -237,17 +237,17 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB,
                     pointA = pointB = midPoint;
                 }
 
-                normal = tnormal;
+                normal = triangleNormal;
             }
 
             return true;
         }
-        // nnormal and tnormal are different
-        // ----------------------------------
+        // neighborNormal and triangleNormal are different
+        // -----------------------------------------------
 
-        // 1st step, project the normal onto the plane given by tnormal and nnormal
+        // 1st step, project the normal onto the plane given by triangleNormal and neighborNormal
         // by removing the component along the cross product axis
-        JVector cross = nnormal % tnormal;
+        JVector cross = neighborNormal % triangleNormal;
         Real crossLenSq = cross.LengthSquared();
         JVector proj = normal - (cross * normal / crossLenSq) * cross;
 
@@ -257,34 +257,34 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB,
             Console.WriteLine($"case #3: discarding");
 
 #endif
-            // can not project onto the plane, discard
+            // Cannot project onto the plane, discard.
             return false;
         }
 
-        // 2nd step, determine if "proj" is between nnormal and tnormal
+        // 2nd step, determine if "proj" is between neighborNormal and triangleNormal
         //
-        //    /    nnormal
+        //    /    neighborNormal
         //   /
         //  /
         //  -----  proj
         // \
         //  \
-        //   \     tnormal
-        Real f1 = proj % nnormal * cross;
-        Real f2 = proj % tnormal * cross;
+        //   \     triangleNormal
+        Real f1 = proj % neighborNormal * cross;
+        Real f2 = proj % triangleNormal * cross;
 
         bool between = f1 <= (Real)0.0 && f2 >= (Real)0.0;
 
         if (!between)
         {
             // not in-between, snap normal
-            Real f3 = JVector.Dot(normal, nnormal);
-            Real f4 = JVector.Dot(normal, tnormal);
+            Real f3 = JVector.Dot(normal, neighborNormal);
+            Real f4 = JVector.Dot(normal, triangleNormal);
 
             if (f3 > f4)
             {
 #if DEBUG_EDGEFILTER
-                Console.WriteLine($"case #2: adjusting; normal {normal} -> {nnormal}");
+                Console.WriteLine($"case #2: adjusting; normal {normal} -> {neighborNormal}");
 
 #endif
                 if (!isSpeculative)
@@ -293,20 +293,20 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB,
                     pointA = pointB = midPoint;
                 }
 
-                normal = nnormal;
+                normal = neighborNormal;
             }
             else
             {
 #if DEBUG_EDGEFILTER
-                Console.WriteLine($"case #2: adjusting; normal {normal} -> {tnormal}");
+                Console.WriteLine($"case #2: adjusting; normal {normal} -> {triangleNormal}");
 #endif
                 if (!isSpeculative)
                 {
                     penetration = 0;
                     pointA = pointB = midPoint;
                 }
 
-                normal = tnormal;
+                normal = triangleNormal;
             }
         }
 

src/Jitter2/Collision/CollisionIsland.cs

@@ -45,4 +45,9 @@ internal void ClearLists()
     {
         InternalBodies.Clear();
     }
+
+    internal void TrimLists()
+    {
+        InternalBodies.TrimExcess();
+    }
 }

src/Jitter2/Collision/DynamicTree/DynamicTree.FindNearest.cs

@@ -95,7 +95,8 @@ public bool FindNearestSphere(Real radius, in JVector position, Real maxDistance
 
     private struct DistanceQuery(in JBoundingBox box, in JQuaternion orientation, in JVector position)
     {
-        public readonly JBoundingBox Box = box;
+        public readonly JVector BoxExtents = (box.Max - box.Min) * (Real)0.5;
+        public readonly JVector BoxCenter = (box.Max + box.Min) * (Real)0.5;
         public readonly JQuaternion Orientation = orientation;
         public readonly JVector Position = position;
 
@@ -181,14 +182,11 @@ public bool FindNearest<T>(in T support, in JQuaternion orientation, in JVector
     // Returns the minimum distance between the query AABB and a tree node's expanded AABB.
     // Uses the Minkowski sum: expand the target by the query half-extents, then measure
     // point-to-AABB distance from the query center. Returns 0 if the AABBs overlap.
-    private static Real MinDistBox(in JBoundingBox queryBox, in TreeBox targetBox)
+    private static Real MinDistBox(in JVector queryExtents, in JVector queryCenter, in TreeBox targetBox)
     {
-        JVector extents = (queryBox.Max - queryBox.Min) * (Real)0.5;
-        JVector center = (queryBox.Max + queryBox.Min) * (Real)0.5;
-
-        Real dx = MathR.Max(MathR.Max(targetBox.Min.X - extents.X - center.X, center.X - targetBox.Max.X - extents.X), (Real)0.0);
-        Real dy = MathR.Max(MathR.Max(targetBox.Min.Y - extents.Y - center.Y, center.Y - targetBox.Max.Y - extents.Y), (Real)0.0);
-        Real dz = MathR.Max(MathR.Max(targetBox.Min.Z - extents.Z - center.Z, center.Z - targetBox.Max.Z - extents.Z), (Real)0.0);
+        Real dx = MathR.Max(MathR.Max(targetBox.Min.X - queryExtents.X - queryCenter.X, queryCenter.X - targetBox.Max.X - queryExtents.X), (Real)0.0);
+        Real dy = MathR.Max(MathR.Max(targetBox.Min.Y - queryExtents.Y - queryCenter.Y, queryCenter.Y - targetBox.Max.Y - queryExtents.Y), (Real)0.0);
+        Real dz = MathR.Max(MathR.Max(targetBox.Min.Z - queryExtents.Z - queryCenter.Z, queryCenter.Z - targetBox.Max.Z - queryExtents.Z), (Real)0.0);
 
         return MathR.Sqrt(dx * dx + dy * dy + dz * dz);
     }
@@ -217,14 +215,15 @@ private bool QueryDistance<T>(in T support, in DistanceQuery query, out FindNear
 
             if (node.IsLeaf)
             {
-                if (node.Proxy is not IDistanceTestable distCastable) continue;
-                if (query.FilterPre != null && !query.FilterPre(node.Proxy!)) continue;
+                IDynamicTreeProxy proxy = node.Proxy!;
+                if (proxy is not IDistanceTestable distanceTestable) continue;
+                if (query.FilterPre != null && !query.FilterPre(proxy)) continue;
 
                 Unsafe.SkipInit(out FindNearestResult res);
-                bool separated = distCastable.Distance(support,
+                bool separated = distanceTestable.Distance(support,
                     query.Orientation, query.Position,
                     out res.PointA, out res.PointB, out res.Normal, out res.Distance);
-                res.Entity = node.Proxy;
+                res.Entity = proxy;
 
                 if (!separated)
                 {
@@ -246,8 +245,8 @@ private bool QueryDistance<T>(in T support, in DistanceQuery query, out FindNear
             ref Node leftNode = ref nodes[node.Left];
             ref Node rightNode = ref nodes[node.Right];
 
-            Real leftDist = MinDistBox(query.Box, leftNode.ExpandedBox);
-            Real rightDist = MinDistBox(query.Box, rightNode.ExpandedBox);
+            Real leftDist = MinDistBox(query.BoxExtents, query.BoxCenter, leftNode.ExpandedBox);
+            Real rightDist = MinDistBox(query.BoxExtents, query.BoxCenter, rightNode.ExpandedBox);
 
             bool leftHit = leftDist <= result.Distance;
             bool rightHit = rightDist <= result.Distance;

src/Jitter2/Collision/DynamicTree/DynamicTree.RayCast.cs

@@ -60,8 +60,8 @@ private struct Ray(in JVector origin, in JVector direction)
     /// </summary>
     /// <param name="origin">Origin of the ray.</param>
     /// <param name="direction">Direction of the ray. Does not have to be normalized.</param>
-    /// <param name="pre">Optional pre-filter which allows to skip shapes in the detection.</param>
-    /// <param name="post">Optional post-filter which allows to skip detections.</param>
+    /// <param name="pre">Optional pre-filter that can skip candidate shapes.</param>
+    /// <param name="post">Optional post-filter that can skip candidate results.</param>
     /// <param name="proxy">The shape which was hit.</param>
     /// <param name="normal">
     /// The surface normal at the hit point. <see cref="JVector.Zero"/> if the ray does not hit,
@@ -107,7 +107,7 @@ private bool QueryRay(in Ray ray, out RayCastResult result)
     {
         result = new RayCastResult();
 
-        if (root == -1)
+        if (root == NullNode)
         {
             return false;
         }
@@ -122,19 +122,20 @@ private bool QueryRay(in Ray ray, out RayCastResult result)
 
         while (stack.Count > baseCount)
         {
-            int pop = stack.Pop();
+            int index = stack.Pop();
 
-            ref Node node = ref nodes[pop];
+            ref Node node = ref nodes[index];
 
             if (node.IsLeaf)
             {
-                if (node.Proxy is not IRayCastable irc) continue;
+                IDynamicTreeProxy proxy = node.Proxy!;
+                if (proxy is not IRayCastable rayCastable) continue;
 
-                if (ray.FilterPre != null && !ray.FilterPre(node.Proxy)) continue;
+                if (ray.FilterPre != null && !ray.FilterPre(proxy)) continue;
 
                 Unsafe.SkipInit(out RayCastResult res);
-                bool hit = irc.RayCast(ray.Origin, ray.Direction, out res.Normal, out res.Lambda);
-                res.Entity = node.Proxy;
+                bool hit = rayCastable.RayCast(ray.Origin, ray.Direction, out res.Normal, out res.Lambda);
+                res.Entity = proxy;
 
                 if (hit && res.Lambda < result.Lambda)
                 {

src/Jitter2/Collision/DynamicTree/DynamicTree.cs

@@ -38,6 +38,14 @@ private struct OverlapEnumerationParam
     /// </summary>
     public ReadOnlyPartitionedSet<IDynamicTreeProxy> Proxies => new(proxies);
 
+    internal void Trim()
+    {
+        proxies.Trim();
+        movedProxies.Trim();
+        tempList.TrimExcess();
+        freeNodes.TrimExcess();
+    }
+
     private readonly PairHashSet potentialPairs = [];
 
     /// <summary>
@@ -236,8 +244,8 @@ private void EnumerateOverlapsCallback(OverlapEnumerationParam parameter)
             var proxyA = nodes[node.ID1].Proxy;
             var proxyB = nodes[node.ID2].Proxy;
 
-            if(proxyA == null || proxyB == null) continue;
-            if(!Filter(proxyA, proxyB)) continue;
+            if (proxyA == null || proxyB == null) continue;
+            if (!Filter(proxyA, proxyB)) continue;
 
             if (!JBoundingBox.Disjoint(proxyA.WorldBoundingBox, proxyB.WorldBoundingBox))
             {
@@ -272,7 +280,7 @@ public void EnumerateOverlaps(Action<IDynamicTreeProxy, IDynamicTreeProxy> actio
             {
                 Parallel.GetBounds(slotsLength, taskCount, i, out int start, out int end);
                 overlapEnumerationParam.Batch = new Parallel.Batch(start, end);
-                ThreadPool.Instance.AddTask(enumerateOverlaps, overlapEnumerationParam);
+                tpi.AddTask(enumerateOverlaps, overlapEnumerationParam);
             }
 
             tpi.Execute();
@@ -528,8 +536,8 @@ private void EnumerateTreeBoxes(ref Node node, Action<TreeBox, int> action, int
     private uint stepper;
 
     /// <summary>
-    /// Removes entries from the internal bookkeeping which are both marked as inactive or
-    /// whose expanded bounding box do not overlap any longer.
+    /// Removes entries from the internal bookkeeping when both proxies are inactive or
+    /// their expanded bounding boxes no longer overlap.
     /// Only searches a small subset of all elements per call to reduce overhead.
     /// </summary>
     private void PruneInvalidPairs()
@@ -717,7 +725,7 @@ public void Optimize(int sweeps = 100, Real chance = (Real)0.01, bool incrementa
     public void Optimize(Func<double> getNextRandom, int sweeps, Real chance, bool incremental)
     {
         if (sweeps <= 0) throw new ArgumentOutOfRangeException(nameof(sweeps), "Sweeps must be greater than zero.");
-        if (chance is < 0 or > 1) throw new ArgumentOutOfRangeException(nameof(chance), "Chance must be between 0 and 1.");
+        ArgumentCheck.InRange(chance, (Real)0.0, (Real)1.0, nameof(chance));
 
         for (int e = 0; e < sweeps; e++)
         {
@@ -908,7 +916,7 @@ private void InternalAddRemoveProxy(IDynamicTreeProxy proxy)
 
         // InsertLeaf takes 'where' as a hint, i.e. it still walks up the tree until
         // the new node is fully contained. Note: The insertion node could also be found when searching
-        // from the root, since the search always descents into child nodes fully containing the new node.
+        // from the root, since the search always descends into child nodes fully containing the new node.
         InsertLeaf(index, parent);
     }
 
@@ -1188,7 +1196,7 @@ private int FindBestHeuristic(int node, int where)
         while (!nodes[where].IsLeaf)
         {
             int left = nodes[where].Left;
-            int rght = nodes[where].Right;
+            int right = nodes[where].Right;
 
             double cost = 2.0d * nodes[where].ExpandedBox.GetSurfaceArea();
 
@@ -1207,20 +1215,20 @@ private int FindBestHeuristic(int node, int where)
                 leftCost = newArea - oldArea;
             }
 
-            if (nodes[rght].IsLeaf)
+            if (nodes[right].IsLeaf)
             {
-                rightCost = TreeBox.MergedSurface(nodes[rght].ExpandedBox, nodeTreeBox);
+                rightCost = TreeBox.MergedSurface(nodes[right].ExpandedBox, nodeTreeBox);
             }
             else
             {
-                double oldArea = nodes[rght].ExpandedBox.GetSurfaceArea();
-                double newArea = TreeBox.MergedSurface(nodes[rght].ExpandedBox, nodeTreeBox);
+                double oldArea = nodes[right].ExpandedBox.GetSurfaceArea();
+                double newArea = TreeBox.MergedSurface(nodes[right].ExpandedBox, nodeTreeBox);
                 rightCost = newArea - oldArea;
             }
 
             if (cost < leftCost && cost < rightCost) break;
 
-            where = leftCost < rightCost ? left : rght;
+            where = leftCost < rightCost ? left : right;
         }
 
         return where;