diff --git a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs
index a1bb5aa7..a4430493 100644
--- a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs
+++ b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs
@@ -455,7 +455,8 @@ 3. This notice may not be removed or altered from any source distribution.
float delta = JVector.Dot(temp3, normal);
penetration = JVector.Dot(v4.V, normal);
- // If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
+ // If the boundary is thin enough or the origin is outside the support plane for the newly discovered
+ // vertex, then we can terminate
if (delta * delta <= CollideEpsilon * CollideEpsilon * normalSq || penetration <= 0.0f ||
phase2 > MaxIter)
{
@@ -526,6 +527,75 @@ 3. This notice may not be removed or altered from any source distribution.
}
}
+ public bool Distance(in MinkowskiDifference mkd,
+ out JVector point1, out JVector point2, out float distance)
+ {
+ const float CollideEpsilon = 1e-4f;
+ const int MaxIter = 34;
+
+ Unsafe.SkipInit(out SimplexSolverAB simplexSolver);
+ simplexSolver.Reset();
+
+ int maxIter = MaxIter;
+
+ mkd.GetCenter(out var center);
+ JVector v = center.V;
+ float distSq = v.LengthSquared();
+
+ while (maxIter-- != 0)
+ {
+ mkd.Support(-v, out var w);
+
+ distSq = v.LengthSquared();
+
+ float deltaDist = JVector.Dot(v - w.V, v);
+ if (deltaDist * deltaDist < CollideEpsilon * CollideEpsilon * distSq)
+ {
+ break;
+ }
+
+ if (distSq < CollideEpsilon * CollideEpsilon ||
+ !simplexSolver.AddVertex(w, out v))
+ {
+ distance = 0.0f;
+ point1 = point2 = JVector.Zero;
+ return false;
+ }
+ }
+
+ distance = MathF.Sqrt(distSq);
+ simplexSolver.GetClosest(out point1, out point2);
+
+ return true;
+ }
+
+ public bool Overlap(in MinkowskiDifference mkd)
+ {
+ const float CollideEpsilon = 1e-4f;
+ const int MaxIter = 34;
+
+ Unsafe.SkipInit(out SimplexSolverAB simplexSolver);
+ simplexSolver.Reset();
+
+ int maxIter = MaxIter;
+
+ mkd.GetCenter(out var center);
+ JVector v = center.V;
+ float distSq = v.LengthSquared();
+
+ while (distSq > CollideEpsilon * CollideEpsilon && maxIter-- != 0)
+ {
+ mkd.Support(-v, out var w);
+ float vw = JVector.Dot(v, w.V);
+ if (vw >= 0.0f)
+ return false;
+ if (!simplexSolver.AddVertex(w, out v)) return true;
+ distSq = v.LengthSquared();
+ }
+
+ return true;
+ }
+
public bool SolveGJKEPA(in MinkowskiDifference mkd,
out JVector point1, out JVector point2, out JVector normal, out float penetration)
{
@@ -782,6 +852,128 @@ public static bool GJKEPA(in ISupportMappable supportA, in ISupportMappable supp
return success;
}
+ ///
+ /// Provides the distance and closest points for non overlapping shapes. It
+ /// assumes that support shape A is located at position zero and not rotated.
+ ///
+ /// The support function of shape A.
+ /// The support function of shape B.
+ /// The orientation of shape B in world space.
+ /// The position of shape B in world space.
+ /// Closest point on shape A. Zero if shapes overlap.
+ /// Closest point on shape B. Zero if shapes overlap.
+ /// The distance between the separating shapes. Zero if shapes overlap.
+ /// Returns true if the shapes do not overlap and distance information
+ /// can be provided.
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static bool Distance(in ISupportMappable supportA, in ISupportMappable supportB,
+ in JQuaternion orientationB, in JVector positionB,
+ out JVector pointA, out JVector pointB, out float distance)
+ {
+ Unsafe.SkipInit(out MinkowskiDifference mkd);
+ mkd.SupportA = supportA;
+ mkd.SupportB = supportB;
+ mkd.PositionB = positionB;
+ mkd.OrientationB = orientationB;
+
+ // ..perform overlap test..
+ return solver.Distance(mkd, out pointA, out pointB, out distance);
+ }
+
+ ///
+ /// Provides the distance and closest points for non overlapping shapes.
+ ///
+ /// The support function of shape A.
+ /// The support function of shape B.
+ /// The orientation of shape A in world space.
+ /// The orientation of shape B in world space.
+ /// The position of shape A in world space.
+ /// The position of shape B in world space.
+ /// Closest point on shape A. Zero if shapes overlap.
+ /// Closest point on shape B. Zero if shapes overlap.
+ /// The distance between the separating shapes. Zero if shapes overlap.
+ /// Returns true if the shapes do not overlap and distance information
+ /// can be provided.
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static bool Distance(in ISupportMappable supportA, in ISupportMappable supportB,
+ in JQuaternion orientationA, in JQuaternion orientationB,
+ in JVector positionA, in JVector positionB,
+ out JVector pointA, out JVector pointB, out float distance)
+ {
+ Unsafe.SkipInit(out MinkowskiDifference mkd);
+ mkd.SupportA = supportA;
+ mkd.SupportB = supportB;
+
+ // rotate into the reference frame of bodyA..
+ JQuaternion.ConjugateMultiply(orientationA, orientationB, out mkd.OrientationB);
+ JVector.Subtract(positionB, positionA, out mkd.PositionB);
+ JVector.ConjugatedTransform(mkd.PositionB, orientationA, out mkd.PositionB);
+
+ // ..perform overlap test..
+ bool result = solver.Distance(mkd, out pointA, out pointB, out distance);
+ if (!result) return false;
+
+ // ..rotate back. This approach potentially saves some matrix-vector multiplication when
+ // the support function is called multiple times.
+ JVector.Transform(pointA, orientationA, out pointA);
+ JVector.Add(pointA, positionA, out pointA);
+ JVector.Transform(pointB, orientationA, out pointB);
+ JVector.Add(pointB, positionA, out pointB);
+
+ return true;
+ }
+
+ ///
+ /// Performs an overlap test. It assumes that support shape A is located
+ /// at position zero and not rotated.
+ ///
+ /// The support function of shape A.
+ /// The support function of shape B.
+ /// The orientation of shape B in world space.
+ /// The position of shape B in world space.
+ /// Returns true of the shapes overlap, and false otherwise.
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static bool Overlap(in ISupportMappable supportA, in ISupportMappable supportB,
+ in JQuaternion orientationB, in JVector positionB)
+ {
+ Unsafe.SkipInit(out MinkowskiDifference mkd);
+ mkd.SupportA = supportA;
+ mkd.SupportB = supportB;
+ mkd.PositionB = positionB;
+ mkd.OrientationB = orientationB;
+
+ // ..perform overlap test..
+ return solver.Overlap(mkd);
+ }
+
+ ///
+ /// Performs an overlap test.
+ ///
+ /// The support function of shape A.
+ /// The support function of shape B.
+ /// The orientation of shape A in world space.
+ /// The orientation of shape B in world space.
+ /// The position of shape A in world space.
+ /// The position of shape B in world space.
+ /// Returns true of the shapes overlap, and false otherwise.
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static bool Overlap(in ISupportMappable supportA, in ISupportMappable supportB,
+ in JQuaternion orientationA, in JQuaternion orientationB,
+ in JVector positionA, in JVector positionB)
+ {
+ Unsafe.SkipInit(out MinkowskiDifference mkd);
+ mkd.SupportA = supportA;
+ mkd.SupportB = supportB;
+
+ // rotate into the reference frame of bodyA..
+ JQuaternion.ConjugateMultiply(orientationA, orientationB, out mkd.OrientationB);
+ JVector.Subtract(positionB, positionA, out mkd.PositionB);
+ JVector.ConjugatedTransform(mkd.PositionB, orientationA, out mkd.PositionB);
+
+ // ..perform overlap test..
+ return solver.Overlap(mkd);
+ }
+
///
/// Detects whether two convex shapes overlap and provides detailed collision information for overlapping shapes.
/// Internally, this method utilizes the Minkowski Portal Refinement (MPR) to obtain the collision information.