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.