| | | 1 | | //======================================================================= |
| | | 2 | | // ContinuousCollisionBounds.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2026-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using FixedMathSharp; |
| | | 9 | | using FixedMathSharp.Geometry; |
| | | 10 | | using Gravitas.Colliders; |
| | | 11 | | using SwiftCollections.Query; |
| | | 12 | | |
| | | 13 | | namespace Gravitas.CollisionHandling; |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// Certifies empty rotational intervals from midpoint geometry and the outward-rounded |
| | | 17 | | /// bound on both participants' relative travel. Every leaf pair must be separated. |
| | | 18 | | /// </summary> |
| | | 19 | | internal static class ContinuousCollisionBounds |
| | | 20 | | { |
| | | 21 | | internal static bool AreSeparated(LSCollider source, LSCollider target, Fixed64 motionBound) |
| | | 22 | | { |
| | 257 | 23 | | if (ContinuousCollisionMath.AreBoundsSeparatedByMoreThan(source.Bounds, target.Bounds, motionBound)) |
| | 54 | 24 | | return true; |
| | | 25 | | |
| | 203 | 26 | | var sourceCompound = source as LSCompoundCollider; |
| | 203 | 27 | | var targetCompound = target as LSCompoundCollider; |
| | 510 | 28 | | for (int i = 0; i < (sourceCompound?.PartCount ?? 1); i++) |
| | 526 | 29 | | for (int j = 0; j < (targetCompound?.PartCount ?? 1); j++) |
| | | 30 | | { |
| | 211 | 31 | | LSCollider first = sourceCompound?.GetPartCollider(i) ?? source; |
| | 211 | 32 | | LSCollider second = targetCompound?.GetPartCollider(j) ?? target; |
| | 211 | 33 | | if (ContinuousCollisionMath.AreBoundsSeparatedByMoreThan(first.Bounds, second.Bounds, motionBound)) |
| | | 34 | | continue; |
| | 203 | 35 | | if (second is LSMeshCollider meshTarget && IsMeshSeparated(meshTarget, first, motionBound)) |
| | | 36 | | continue; |
| | 179 | 37 | | if (first is LSMeshCollider meshSource && IsMeshSeparated(meshSource, second, motionBound)) |
| | | 38 | | continue; |
| | 154 | 39 | | return false; |
| | | 40 | | } |
| | 49 | 41 | | return true; |
| | | 42 | | } |
| | | 43 | | |
| | | 44 | | internal static bool AreSeparated(LSCollider2D source, LSCollider2D target, Fixed64 motionBound) |
| | | 45 | | { |
| | 378 | 46 | | if (ContinuousCollisionMath.AreBoundsSeparatedByMoreThan(source.Bounds, target.Bounds, motionBound)) |
| | 160 | 47 | | return true; |
| | | 48 | | |
| | 218 | 49 | | var sourceCompound = source as LSCompoundCollider2D; |
| | 218 | 50 | | var targetCompound = target as LSCompoundCollider2D; |
| | 464 | 51 | | for (int i = 0; i < (sourceCompound?.PartCount ?? 1); i++) |
| | 492 | 52 | | for (int j = 0; j < (targetCompound?.PartCount ?? 1); j++) |
| | | 53 | | { |
| | 232 | 54 | | LSCollider2D first = sourceCompound?.GetPartCollider(i) ?? source; |
| | 232 | 55 | | LSCollider2D second = targetCompound?.GetPartCollider(j) ?? target; |
| | 232 | 56 | | if (!ContinuousCollisionMath.AreBoundsSeparatedByMoreThan(first.Bounds, second.Bounds, motionBound)) |
| | 204 | 57 | | return false; |
| | | 58 | | } |
| | 14 | 59 | | return true; |
| | | 60 | | } |
| | | 61 | | |
| | | 62 | | internal static bool AreSeparated(LSCollider source, LSCollider2D target, Fixed64 motionBound) |
| | | 63 | | { |
| | 7161 | 64 | | if (ContinuousCollisionMath.AreBoundsSeparatedByMoreThan(source.Bounds, target.MixedBounds3D, motionBound)) |
| | 2747 | 65 | | return true; |
| | | 66 | | |
| | 4414 | 67 | | var sourceCompound = source as LSCompoundCollider; |
| | 4414 | 68 | | var targetCompound = target as LSCompoundCollider2D; |
| | 9016 | 69 | | for (int i = 0; i < (sourceCompound?.PartCount ?? 1); i++) |
| | 9104 | 70 | | for (int j = 0; j < (targetCompound?.PartCount ?? 1); j++) |
| | | 71 | | { |
| | 4458 | 72 | | LSCollider first = sourceCompound?.GetPartCollider(i) ?? source; |
| | 4458 | 73 | | LSCollider2D second = targetCompound?.GetPartCollider(j) ?? target; |
| | 4458 | 74 | | if (ContinuousCollisionMath.AreBoundsSeparatedByMoreThan(first.Bounds, second.MixedBounds3D, motionBound |
| | | 75 | | continue; |
| | 4370 | 76 | | if (first is LSMeshCollider mesh) |
| | | 77 | | { |
| | 509 | 78 | | ColliderCanonicalBounds2D.GetMixedRelativeBounds(second, mesh.Mesh.Origin, mesh.Mesh.Rotation, |
| | 509 | 79 | | out FixedBoundBox localBounds, out FixedOrientedBox orientedBounds); |
| | 509 | 80 | | if (IsMeshSeparated(mesh, localBounds, motionBound, orientedBounds)) |
| | | 81 | | continue; |
| | | 82 | | } |
| | 4344 | 83 | | return false; |
| | | 84 | | } |
| | 70 | 85 | | return true; |
| | | 86 | | } |
| | | 87 | | |
| | | 88 | | private static bool IsMeshSeparated(LSMeshCollider mesh, LSCollider other, Fixed64 motionBound) |
| | | 89 | | { |
| | | 90 | | // Admitted local capsule extents fit; retain clipping only in the final mesh-relative frame. |
| | 210 | 91 | | return IsMeshSeparated(mesh, ColliderCanonicalBounds.GetRelativeBounds(other, |
| | 210 | 92 | | mesh.Mesh.Origin, mesh.Mesh.Rotation), motionBound, (other as LSCuboidCollider)?.OrientedBox); |
| | | 93 | | } |
| | | 94 | | |
| | | 95 | | private static bool IsMeshSeparated(LSMeshCollider mesh, FixedBoundBox localBounds, Fixed64 motionBound, |
| | | 96 | | FixedOrientedBox? orientedBox = null) |
| | | 97 | | { |
| | | 98 | | // An empty boundary query cannot certify the empty interior of a solid hull. |
| | 719 | 99 | | if (mesh.Mode == MeshColliderMode.Convex && mesh.IsClosedSurface) |
| | 43 | 100 | | return false; |
| | | 101 | | |
| | 676 | 102 | | if (!mesh.Mesh.GeometryFrame.IsRigid) |
| | 17 | 103 | | return IsScaledMeshSeparated(mesh, localBounds, motionBound, orientedBox); |
| | | 104 | | |
| | | 105 | | // The combined source/target motion ball fits in this midpoint-frame box. |
| | | 106 | | // Only mesh-local endpoints are clipped; all stored triangles lie in that domain. |
| | 659 | 107 | | Vector3d expansion = Vector3d.One * motionBound; |
| | 659 | 108 | | var candidates = mesh.Context.CollisionScratch.MeshTriangleCandidatesA; |
| | 659 | 109 | | FixedBoundBox expanded = FixedBoundBox.FromMinMax(localBounds.Min - expansion, localBounds.Max + expansion); |
| | 659 | 110 | | mesh.Mesh.GetTrianglesInGeometryBounds(new FixedBoundVolume(expanded.Min, expanded.Max), candidates); |
| | 659 | 111 | | if (candidates.Count == 0) |
| | 16 | 112 | | return true; |
| | | 113 | | |
| | | 114 | | // A triangle's box also contains empty corners. Use the existing exact |
| | | 115 | | // finite-triangle SAT to certify them, without constructing world witnesses. |
| | | 116 | | FixedOrientedBox box; |
| | | 117 | | Vector3d triangleOrigin; |
| | | 118 | | FixedQuaternion triangleRotation; |
| | 643 | 119 | | if (orientedBox is FixedOrientedBox rigidBox) |
| | | 120 | | { |
| | | 121 | | // Preserve the shape enclosure's orientation: its AABB can cover empty corners |
| | | 122 | | // throughout rotation. The same motion ball fits along each box axis. |
| | 568 | 123 | | if (!Vector3d.TryAdd(rigidBox.HalfExtents, expansion, out Vector3d scope)) |
| | 1 | 124 | | return false; |
| | 567 | 125 | | box = new FixedOrientedBox(rigidBox.Center, rigidBox.Orientation, scope); |
| | 567 | 126 | | triangleOrigin = mesh.Mesh.Origin; |
| | 567 | 127 | | triangleRotation = mesh.Mesh.Rotation; |
| | | 128 | | } |
| | | 129 | | else |
| | | 130 | | { |
| | 75 | 131 | | Vector3d center = expanded.Center; |
| | 75 | 132 | | bool scopeResolved = Vector3d.TrySubtract(center, expanded.Min, out Vector3d lowerScope) |
| | 75 | 133 | | & Vector3d.TrySubtract(expanded.Max, center, out Vector3d upperScope); |
| | 75 | 134 | | if (!scopeResolved) |
| | 1 | 135 | | return false; |
| | 74 | 136 | | Vector3d scope = new(FixedMath.Max(lowerScope.X, upperScope.X), |
| | 74 | 137 | | FixedMath.Max(lowerScope.Y, upperScope.Y), FixedMath.Max(lowerScope.Z, upperScope.Z)); |
| | 74 | 138 | | if (scope.X == Fixed64.Zero || scope.Y == Fixed64.Zero || scope.Z == Fixed64.Zero) |
| | 10 | 139 | | return false; |
| | 64 | 140 | | box = new FixedOrientedBox(center, FixedQuaternion.Identity, scope); |
| | 64 | 141 | | triangleOrigin = Vector3d.Zero; |
| | 64 | 142 | | triangleRotation = FixedQuaternion.Identity; |
| | | 143 | | } |
| | 1730 | 144 | | for (int i = 0; i < candidates.Count; i++) |
| | | 145 | | { |
| | 811 | 146 | | mesh.Mesh.GetGeometryTriangleVertices(candidates[i], out Vector3d first, out Vector3d second, out Vector3d t |
| | 811 | 147 | | var triangle = new FixedTriangle(first, second, third); |
| | | 148 | | // Degeneracy is a distinct false result, so it cannot prove separation. |
| | 811 | 149 | | if (triangle.IsDegenerate || box.TryGetTriangleContact(triangleOrigin, triangleRotation, triangle, out _)) |
| | 577 | 150 | | return false; |
| | | 151 | | } |
| | 54 | 152 | | return true; |
| | | 153 | | } |
| | | 154 | | private static bool IsScaledMeshSeparated(LSMeshCollider collider, FixedBoundBox physicalBounds, |
| | | 155 | | Fixed64 motionBound, FixedOrientedBox? orientedBox) |
| | | 156 | | { |
| | 17 | 157 | | PhysicsMesh mesh = collider.Mesh; |
| | 17 | 158 | | Vector3d expansion = Vector3d.One * motionBound; |
| | 17 | 159 | | FixedBoundBox expanded = FixedBoundBox.FromMinMax(physicalBounds.Min - expansion, physicalBounds.Max + expansion |
| | | 160 | | // Motion is a physical sphere. Expand before converting to the authored |
| | | 161 | | // chart, where nonuniform scale would otherwise distort that sphere. |
| | 17 | 162 | | var physicalFrame = ScaledRigidFrame3d.Rigid(mesh.Origin, mesh.Rotation); |
| | 17 | 163 | | bool transformed = physicalFrame.TryGetBoundsIn(expanded.Min, expanded.Max, mesh.GeometryFrame, |
| | 17 | 164 | | out FixedBoundBox candidatesBounds); |
| | | 165 | | // Collider scale admission guarantees an invertible target mapping. |
| | | 166 | | System.Diagnostics.Debug.Assert(transformed); |
| | 17 | 167 | | var candidates = collider.Context.CollisionScratch.MeshTriangleCandidatesA; |
| | 17 | 168 | | mesh.GetTrianglesInGeometryBounds(new FixedBoundVolume(candidatesBounds.Min, candidatesBounds.Max), candidates); |
| | 18 | 169 | | if (candidates.Count == 0) return true; |
| | | 170 | | ScaledRigidFrame3d boxFrame; |
| | | 171 | | Vector3d scope; |
| | 16 | 172 | | if (orientedBox is FixedOrientedBox rigid) |
| | | 173 | | { |
| | 10 | 174 | | if (!Vector3d.TryAdd(rigid.HalfExtents, expansion, out scope)) return false; |
| | 6 | 175 | | boxFrame = ScaledRigidFrame3d.Rigid(rigid.Center, rigid.Orientation); |
| | | 176 | | } |
| | | 177 | | else |
| | | 178 | | { |
| | 8 | 179 | | Vector3d center = expanded.Center; |
| | 8 | 180 | | if (!Vector3d.TrySubtract(center, expanded.Min, out Vector3d lower) |
| | 10 | 181 | | || !Vector3d.TrySubtract(expanded.Max, center, out Vector3d upper)) return false; |
| | 6 | 182 | | scope = new Vector3d(FixedMath.Max(lower.X, upper.X), FixedMath.Max(lower.Y, upper.Y), |
| | 6 | 183 | | FixedMath.Max(lower.Z, upper.Z)); |
| | 6 | 184 | | boxFrame = new ScaledRigidFrame3d(mesh.Origin, mesh.Rotation, center, Vector3d.One, |
| | 6 | 185 | | mesh.Rotation, Vector3d.Zero, Vector3d.One); |
| | | 186 | | } |
| | 16 | 187 | | if (scope.X == Fixed64.Zero || scope.Y == Fixed64.Zero || scope.Z == Fixed64.Zero) return false; |
| | 8 | 188 | | System.Span<Vector3d> boxVertices = stackalloc Vector3d[8]; |
| | 8 | 189 | | WideOrientedBox.GetBoxHullVertices(scope, boxVertices); |
| | 8 | 190 | | System.Span<Vector3d> points = stackalloc Vector3d[3]; |
| | 8 | 191 | | System.ReadOnlySpan<int> face = stackalloc int[3] { 0, 1, 2 }; |
| | 8 | 192 | | System.ReadOnlySpan<int> edges = stackalloc int[6] { 0, 1, 1, 2, 2, 0 }; |
| | 24 | 193 | | for (int index = 0; index < candidates.Count; index++) |
| | | 194 | | { |
| | 8 | 195 | | mesh.GetGeometryTriangleVertices(candidates[index], out points[0], out points[1], out points[2]); |
| | | 196 | | // Authored mesh admission rejects degenerate triangles, and the |
| | | 197 | | // positive-scale affine mapping preserves their exact nonzero area. |
| | 8 | 198 | | if (WideOrientedBox.TryGetConvexHullPenetration(boxFrame, boxVertices, |
| | 8 | 199 | | WideOrientedBox.BoxHullTriangles, WideOrientedBox.BoxHullEdges, mesh.GeometryFrame, |
| | 12 | 200 | | points, face, edges, out _)) return false; |
| | | 201 | | } |
| | 4 | 202 | | return true; |
| | | 203 | | } |
| | | 204 | | |
| | | 205 | | } |