| | | 1 | | //======================================================================= |
| | | 2 | | // CollisionDetection.Cone.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 System.Runtime.CompilerServices; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using FixedMathSharp.Geometry; |
| | | 11 | | using Gravitas.Colliders; |
| | | 12 | | using SwiftCollections.Query; |
| | | 13 | | |
| | | 14 | | namespace Gravitas.CollisionHandling; |
| | | 15 | | |
| | | 16 | | /// <content>Finds finite cone contacts against spheres, convex shapes, and mesh surfaces.</content> |
| | | 17 | | public static partial class CollisionDetection |
| | | 18 | | { |
| | | 19 | | private static bool DoConeSphereCheck(CollisionWorkItem pair) |
| | | 20 | | { |
| | 98 | 21 | | var cone = (LSConeCollider)pair.ColliderA; |
| | 98 | 22 | | var sphere = (LSSphereCollider)pair.ColliderB; |
| | | 23 | | |
| | 98 | 24 | | if (!FixedSegment.TryGetClosestCenteredFiniteConeSurfaceAnchor( |
| | 98 | 25 | | sphere.Center, |
| | 98 | 26 | | cone.Center, |
| | 98 | 27 | | cone.Rotation, |
| | 98 | 28 | | Vector3d.Up, |
| | 98 | 29 | | cone.Height, |
| | 98 | 30 | | cone.ScaledRadius, |
| | 98 | 31 | | Vector3d.Right, |
| | 98 | 32 | | out FixedPointAnchor coneAnchor, |
| | 98 | 33 | | out Vector3d outwardNormal, |
| | 98 | 34 | | out Fixed64 signedDistance)) |
| | | 35 | | { |
| | 1 | 36 | | return false; |
| | | 37 | | } |
| | 97 | 38 | | if (signedDistance > sphere.ScaledRadius) |
| | 49 | 39 | | return false; |
| | | 40 | | |
| | | 41 | | // Containment increases penetration depth; the sphere still escapes |
| | | 42 | | // along the solid surface's outward normal, not toward the cone center. |
| | 48 | 43 | | Vector3d normal = outwardNormal; |
| | | 44 | | // Admitted cone and sphere reaches bound containment depth by their |
| | | 45 | | // sum, so no scalar clamping is needed for this complete difference. |
| | 48 | 46 | | Fixed64 penetrationDepth = sphere.ScaledRadius - signedDistance; |
| | 48 | 47 | | pair.Manifold.SetContact( |
| | 48 | 48 | | new ContactAnchor(coneAnchor), |
| | 48 | 49 | | new ContactAnchor( |
| | 48 | 50 | | FixedSegment.GetCenteredCapsuleSupportAnchor( |
| | 48 | 51 | | sphere.Center, |
| | 48 | 52 | | sphere.Rotation, |
| | 48 | 53 | | Fixed64.Zero, |
| | 48 | 54 | | sphere.ScaledRadius, |
| | 48 | 55 | | -normal)), |
| | 48 | 56 | | penetrationDepth, |
| | 48 | 57 | | normal, |
| | 48 | 58 | | depthIsClamped: false); |
| | | 59 | | |
| | 48 | 60 | | return true; |
| | | 61 | | } |
| | | 62 | | |
| | | 63 | | private static bool DoConeConvexCheck(CollisionWorkItem pair) |
| | | 64 | | { |
| | 299 | 65 | | GetConeConvexColliders(pair, out LSConeCollider cone, out LSCollider convex); |
| | | 66 | | |
| | 299 | 67 | | if (!ConvexColliderSupport.Intersects(cone, convex)) |
| | 193 | 68 | | return false; |
| | | 69 | | |
| | 106 | 70 | | Vector3d normalConeToConvex = ResolveNormal(convex.Center - cone.Center); |
| | 106 | 71 | | normalConeToConvex = normalConeToConvex.Normalized; |
| | 106 | 72 | | FixedPointAnchor pointOnCone = ConvexColliderSupport.GetSupportAnchor( |
| | 106 | 73 | | cone, |
| | 106 | 74 | | normalConeToConvex, |
| | 106 | 75 | | Vector3d.Zero); |
| | 106 | 76 | | FixedPointAnchor pointOnConvex = ConvexColliderSupport.GetSupportAnchor( |
| | 106 | 77 | | convex, |
| | 106 | 78 | | -normalConeToConvex, |
| | 106 | 79 | | Vector3d.Zero); |
| | 106 | 80 | | Fixed64 depth = pointOnCone.ProjectNonNegativeOffsetFrom( |
| | 106 | 81 | | pointOnConvex, |
| | 106 | 82 | | normalConeToConvex); |
| | | 83 | | |
| | 106 | 84 | | SetContactInPairOrder( |
| | 106 | 85 | | pair, |
| | 106 | 86 | | cone, |
| | 106 | 87 | | new ContactAnchor(pointOnCone), |
| | 106 | 88 | | convex, |
| | 106 | 89 | | new ContactAnchor(pointOnConvex), |
| | 106 | 90 | | depth, |
| | 106 | 91 | | normalConeToConvex, |
| | 106 | 92 | | depthIsClamped: false); |
| | 106 | 93 | | return true; |
| | | 94 | | } |
| | | 95 | | |
| | | 96 | | private static bool DoMeshConeCheck(CollisionWorkItem pair) |
| | | 97 | | { |
| | 12094 | 98 | | var mesh = (LSMeshCollider)pair.ColliderA; |
| | 12094 | 99 | | var cone = (LSConeCollider)pair.ColliderB; |
| | | 100 | | |
| | 12094 | 101 | | if (mesh.StaticSurface is { } surface && !mesh.IsTrigger && !cone.IsTrigger |
| | 12094 | 102 | | && surface.Gather(cone, new FixedBoundVolume(cone.BoundsMin, cone.BoundsMax)) > 1) |
| | | 103 | | { |
| | 6381 | 104 | | surface.BuildCone(cone); |
| | 6381 | 105 | | var faces = surface.ConeFaces; |
| | 25598 | 106 | | for (int region = 0; region < faces.RegionCount; region++) |
| | | 107 | | { |
| | 6418 | 108 | | if (!faces.IsResponseCarrier(mesh, region)) continue; |
| | 31690 | 109 | | for (int sample = 0; sample < faces.GetSampleCount(region); sample++) |
| | | 110 | | { |
| | 12674 | 111 | | FixedContactAnchors contact = faces.GetJoinedSample(region, sample, out LSMeshCollider supplier, out |
| | 12674 | 112 | | pair.Manifold.AddContact(new ContactAnchor(contact.FirstAnchor), new ContactAnchor(contact.SecondAnc |
| | 12674 | 113 | | contact.Depth, contact.Normal, supplier.Material, cone.Material, contact.DepthIsClamped, |
| | 12674 | 114 | | group: new ContactGroupKey(0, 0, owner + 1, 0, region, sourceIdA: supplier.Id + 1, |
| | 12674 | 115 | | sourceOrderA: ((IPhysicsColliderRegistryItem)supplier).ReplayOrder + 1), |
| | 12674 | 116 | | contactIdentity: faces.GetSampleIdentity(region, sample)); |
| | | 117 | | } |
| | | 118 | | } |
| | 6381 | 119 | | return faces.AppendJoinedBoundaries(mesh, cone, pair.Context.CollisionScratch.MeshConeBoundary, pair.Manifol |
| | | 120 | | } |
| | 5713 | 121 | | if (BuildMeshConeSurfaceContacts(mesh, cone, pair.Context.CollisionScratch, pair.Manifold)) |
| | 5541 | 122 | | return true; |
| | | 123 | | |
| | | 124 | | // An open sheet has no solid interior, even when its bounds were |
| | | 125 | | // authored in convex mode. Only a closed convex volume owns containment. |
| | 172 | 126 | | if (mesh.Mode == MeshColliderMode.Concave || !mesh.IsClosedSurface |
| | 172 | 127 | | || !ConvexColliderSupport.Intersects(mesh, cone)) |
| | 169 | 128 | | return false; |
| | | 129 | | |
| | 3 | 130 | | Vector3d normalMeshToCone = ResolveNormal(cone.Center - mesh.Center); |
| | 3 | 131 | | FixedPointAnchor pointOnMesh = ConvexColliderSupport.GetSupportAnchor( |
| | 3 | 132 | | mesh, |
| | 3 | 133 | | normalMeshToCone, |
| | 3 | 134 | | Vector3d.Zero); |
| | 3 | 135 | | FixedPointAnchor pointOnCone = ConvexColliderSupport.GetSupportAnchor( |
| | 3 | 136 | | cone, |
| | 3 | 137 | | -normalMeshToCone, |
| | 3 | 138 | | Vector3d.Zero); |
| | 3 | 139 | | Fixed64 depth = pointOnMesh.ProjectNonNegativeOffsetFrom( |
| | 3 | 140 | | pointOnCone, |
| | 3 | 141 | | normalMeshToCone); |
| | | 142 | | |
| | 3 | 143 | | pair.Manifold.SetContact( |
| | 3 | 144 | | new ContactAnchor(pointOnMesh), |
| | 3 | 145 | | new ContactAnchor(pointOnCone), |
| | 3 | 146 | | depth, |
| | 3 | 147 | | normalMeshToCone); |
| | 3 | 148 | | return true; |
| | | 149 | | } |
| | | 150 | | |
| | | 151 | | /// <summary>Tests finite triangle surfaces without containment, joined response ownership or contact materializatio |
| | | 152 | | internal static bool IntersectsMeshConeSurface(LSMeshCollider meshCollider, LSConeCollider cone, |
| | | 153 | | CollisionSatScratch scratch) |
| | | 154 | | { |
| | 273 | 155 | | var candidates = scratch.MeshTriangleCandidatesA; |
| | 273 | 156 | | meshCollider.GetTrianglesInBounds(new FixedBoundVolume(cone.BoundsMin, cone.BoundsMax), candidates); |
| | 273 | 157 | | PhysicsMesh mesh = meshCollider.Mesh; |
| | 273 | 158 | | int preparedOwner = -1; |
| | 273 | 159 | | ConePlaneRayFrame frame = default; |
| | 933 | 160 | | foreach (int triangle in candidates.AsReadOnlySpan()) |
| | | 161 | | { |
| | 269 | 162 | | mesh.GetGeometryTriangleVertices(triangle, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 269 | 163 | | var face = new FixedTriangle(a, b, c); |
| | 269 | 164 | | int owner = mesh.GetManifoldSurfaceOwner(triangle); |
| | 269 | 165 | | if (owner != preparedOwner) |
| | | 166 | | { |
| | 257 | 167 | | frame = new ConePlaneRayFrame(face, mesh.GeometryFrame, |
| | 257 | 168 | | cone.Center, cone.Rotation, cone.Height, cone.ScaledRadius); |
| | 257 | 169 | | preparedOwner = owner; |
| | | 170 | | } |
| | 420 | 171 | | if (ConePlaneRayEvents.IntersectsTriangle(frame, face)) return true; |
| | | 172 | | } |
| | 122 | 173 | | return false; |
| | | 174 | | } |
| | | 175 | | |
| | | 176 | | private static bool BuildMeshConeSurfaceContacts( |
| | | 177 | | LSMeshCollider meshCollider, LSConeCollider cone, CollisionSatScratch scratch, ContactManifold manifold) |
| | | 178 | | { |
| | 5713 | 179 | | PhysicsMesh mesh = meshCollider.Mesh; |
| | 5713 | 180 | | var candidates = scratch.MeshTriangleCandidatesA; |
| | 5713 | 181 | | var admitted = scratch.MeshTriangleCandidatesB; |
| | 5713 | 182 | | meshCollider.GetTrianglesInBounds(new FixedBoundVolume(cone.BoundsMin, cone.BoundsMax), candidates); |
| | 5713 | 183 | | scratch.MeshSurfaces.Clear(); |
| | 5713 | 184 | | ContactManifold staged = scratch.MeshSurfaceManifold; |
| | 5713 | 185 | | staged.Reset(); |
| | 52028 | 186 | | foreach (int triangleIndex in candidates.AsReadOnlySpan()) |
| | | 187 | | { |
| | 20301 | 188 | | int owner = mesh.GetManifoldSurfaceOwner(triangleIndex); |
| | 20301 | 189 | | if (!scratch.MeshSurfaces.Add(owner)) continue; |
| | 5991 | 190 | | mesh.GetGeometryTriangleVertices(triangleIndex, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 5991 | 191 | | var frame = new ConePlaneRayFrame(new FixedTriangle(a, b, c), mesh.GeometryFrame, |
| | 5991 | 192 | | cone.Center, cone.Rotation, cone.Height, cone.ScaledRadius); |
| | 5991 | 193 | | admitted.FastClear(); |
| | | 194 | | // Reuse one exact plane frame per surface rather than rebuilding |
| | | 195 | | // its reduced normal and rigid basis for each subdivided triangle. |
| | | 196 | | // ponytail: O(candidate count * surface count) owner comparisons; |
| | | 197 | | // bucket candidates if many distinct planes become a measured cost. |
| | 60072 | 198 | | foreach (int candidate in candidates.AsReadOnlySpan()) |
| | | 199 | | { |
| | 24045 | 200 | | if (mesh.GetManifoldSurfaceOwner(candidate) != owner) continue; |
| | 20301 | 201 | | mesh.GetGeometryTriangleVertices(candidate, out a, out b, out c); |
| | 33651 | 202 | | if (ConePlaneRayEvents.IntersectsTriangle(frame, new FixedTriangle(a, b, c))) admitted.Add(candidate); |
| | | 203 | | } |
| | 5991 | 204 | | if (admitted.Count == 0) continue; |
| | 5727 | 205 | | var connectivity = scratch.MeshConnectivity; |
| | 5727 | 206 | | connectivity.Build(mesh, triangleIndex, frame, admitted.AsReadOnlySpan()); |
| | 5727 | 207 | | var faces = scratch.MeshConeFaces; |
| | | 208 | | // Admission bounds radius + half-height to a few thousand units, |
| | | 209 | | // so centered sampling fits even when the cone touches a scalar face. |
| | 5727 | 210 | | Vector3d samplingOrigin = cone.Center; |
| | 5727 | 211 | | faces.Build(mesh, triangleIndex, frame, connectivity, admitted.AsReadOnlySpan(), samplingOrigin); |
| | 5727 | 212 | | int surface = mesh.GetCanonicalSurfaceOrdinal(triangleIndex); |
| | 22910 | 213 | | for (int region = 0; region < faces.RegionCount; region++) |
| | | 214 | | { |
| | 5728 | 215 | | var key = new ContactGroupKey(0, 0, surface, 0, region); |
| | 5728 | 216 | | Vector3d normal = faces.GetNormal(region); |
| | 5728 | 217 | | int count = faces.GetSampleCount(region); |
| | 56852 | 218 | | for (int sample = 0; sample < count; sample++) |
| | | 219 | | { |
| | 22698 | 220 | | bool found = faces.TryGetSample(region, sample, out Vector3d p, out Vector3d q, out Fixed64 depth); |
| | | 221 | | System.Diagnostics.Debug.Assert(found); |
| | | 222 | | // Preserve the sampled world-lattice point without narrowing |
| | | 223 | | // its offset from a collider origin. Existing additive anchor |
| | | 224 | | // terms retain any large cancellation exactly. |
| | 22698 | 225 | | var meshAnchor = new ContactAnchor(FixedPointAnchor.FromTranslatedPoint(meshCollider.Center, samplin |
| | 22698 | 226 | | var coneAnchor = new ContactAnchor(FixedPointAnchor.FromTranslatedPoint(cone.Center, samplingOrigin, |
| | 22698 | 227 | | staged.AddContact(meshAnchor, coneAnchor, |
| | 22698 | 228 | | depth, normal, meshCollider.Material, cone.Material, group: key, |
| | 22698 | 229 | | contactIdentity: faces.GetSampleIdentity(region, sample)); |
| | | 230 | | } |
| | | 231 | | } |
| | | 232 | | // Finite exposed-feature witnesses lie in the cone. Reuse the |
| | | 233 | | // face owner's exact boundary admission rather than constructing |
| | | 234 | | // support candidates for an entirely disjoint perimeter. |
| | 5727 | 235 | | if (faces.HasBoundaryIntersection) |
| | 309 | 236 | | scratch.MeshConeBoundary.BuildSurfaceContacts(meshCollider, cone, triangleIndex, connectivity, faces, st |
| | | 237 | | } |
| | | 238 | | // Geometry/range failures leave the pair unpublished. Reserve the |
| | | 239 | | // actual grouped result before copying; both owners retain high water. |
| | 5713 | 240 | | manifold.ReserveGroups(staged.GroupCount); |
| | 24400 | 241 | | for (int group = 0; group < staged.GroupCount; group++) |
| | | 242 | | { |
| | 6487 | 243 | | ref ContactGroup source = ref staged.GetGroup(group); |
| | 59946 | 244 | | for (int sample = 0; sample < source.Count; sample++) |
| | 23486 | 245 | | manifold.AddContact(source.Key, source[sample]); |
| | | 246 | | } |
| | 5713 | 247 | | return staged.HasContact; |
| | | 248 | | } |
| | | 249 | | |
| | | 250 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 251 | | private static void GetConeConvexColliders( |
| | | 252 | | CollisionWorkItem pair, |
| | | 253 | | out LSConeCollider cone, |
| | | 254 | | out LSCollider convex) |
| | | 255 | | { |
| | 299 | 256 | | if (pair.ColliderA is LSConeCollider coneA && ConvexColliderSupport.IsSupported(pair.ColliderB)) |
| | | 257 | | { |
| | 173 | 258 | | cone = coneA; |
| | 173 | 259 | | convex = pair.ColliderB; |
| | 173 | 260 | | return; |
| | | 261 | | } |
| | | 262 | | |
| | 126 | 263 | | cone = (LSConeCollider)pair.ColliderB; |
| | 126 | 264 | | convex = pair.ColliderA; |
| | 126 | 265 | | } |
| | | 266 | | |
| | | 267 | | } |