| | | 1 | | //======================================================================= |
| | | 2 | | // MeshTriangleContactGenerator.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; |
| | | 9 | | using System.Collections.Generic; |
| | | 10 | | using System.Runtime.CompilerServices; |
| | | 11 | | using FixedMathSharp; |
| | | 12 | | using FixedMathSharp.Geometry; |
| | | 13 | | using Gravitas.Colliders; |
| | | 14 | | using Gravitas.Materials; |
| | | 15 | | using SwiftCollections; |
| | | 16 | | using SwiftCollections.Query; |
| | | 17 | | |
| | | 18 | | namespace Gravitas.CollisionHandling; |
| | | 19 | | |
| | | 20 | | /// <summary> |
| | | 21 | | /// Builds finite mesh manifolds, retaining closed convex solids where available |
| | | 22 | | /// and using triangle contacts for the remaining surface owners. |
| | | 23 | | /// </summary> |
| | | 24 | | internal static partial class MeshTriangleContactGenerator |
| | | 25 | | { |
| | | 26 | | |
| | | 27 | | |
| | 1 | 28 | | private static readonly IComparer<HullSample> HullSampleComparer = Comparer<HullSample>.Create(static (a, b) => |
| | 1 | 29 | | { |
| | 1 | 30 | | int order = a.Contact.FirstAnchor.CompareLocalFeature(b.Contact.FirstAnchor); |
| | 1 | 31 | | return order != 0 ? order : a.Contact.SecondAnchor.CompareLocalFeature(b.Contact.SecondAnchor); |
| | 1 | 32 | | }); |
| | | 33 | | |
| | | 34 | | // True means the convex pair was handled, including separation; false requests finite triangle fallback. |
| | | 35 | | internal static bool TryAppendHullContacts(LSMeshCollider hull, ReadOnlySpan<Vector3d> hullVertices, |
| | | 36 | | ReadOnlySpan<int> hullTriangles, ReadOnlySpan<int> hullEdges, in ScaledRigidFrame3d frame, Vector3d origin, |
| | | 37 | | ReadOnlySpan<Vector3d> vertices, ReadOnlySpan<int> triangles, ReadOnlySpan<int> edges, |
| | | 38 | | ReadOnlySpan<int> satEdges, PhysicsMaterial material, ContactManifold manifold, bool reverse, |
| | | 39 | | int namespaceA = 0, int namespaceB = 0, ContactGroupKey group = default) |
| | | 40 | | { |
| | 3831 | 41 | | PhysicsMesh mesh = hull.Mesh; |
| | | 42 | | // Select the convex-pair exit once; finite witnesses must belong to that support. |
| | 3831 | 43 | | SwiftList<HullSample> samples = hull.Context.CollisionScratch.HullSamples; |
| | 3831 | 44 | | samples.FastClear(); |
| | 3831 | 45 | | bool exact = !mesh.GeometryFrame.IsRigid || !frame.IsRigid; |
| | 3831 | 46 | | if (exact) |
| | | 47 | | { |
| | 35 | 48 | | if (!WideOrientedBox.TryGetConvexHullPenetration(mesh.GeometryFrame, hullVertices, |
| | 35 | 49 | | hullTriangles, hullEdges, frame, vertices, triangles, satEdges, |
| | 41 | 50 | | out WideScaledHullPenetration penetration, skipParallelDirections: true)) return true; |
| | 29 | 51 | | WideOrientedBox.VisitConvexHullContacts(mesh.GeometryFrame, mesh.Origin, hullVertices, |
| | 29 | 52 | | hullTriangles, hullEdges, frame, origin, vertices, triangles, edges, |
| | 29 | 53 | | penetration, samples, static (pool, contact) => pool.Add(new HullSample(contact))); |
| | | 54 | | } |
| | | 55 | | else |
| | | 56 | | { |
| | 3796 | 57 | | if (!WideOrientedBox.TryGetConvexHullPenetration(mesh.Origin, mesh.Rotation, hullVertices, |
| | 3796 | 58 | | hullTriangles, hullEdges, origin, frame.Rotation, vertices, triangles, satEdges, |
| | 3811 | 59 | | out WidePointSpanPenetration penetration, skipParallelDirections: true)) return true; |
| | 3781 | 60 | | WideOrientedBox.VisitConvexHullContacts(mesh.Origin, mesh.Rotation, hullVertices, |
| | 3781 | 61 | | hullTriangles, hullEdges, origin, frame.Rotation, vertices, triangles, edges, |
| | 3781 | 62 | | penetration, samples, static (pool, contact) => pool.Add(new HullSample(contact))); |
| | | 63 | | } |
| | 3814 | 64 | | if (samples.Count == 0) return false; |
| | 3806 | 65 | | samples.SortInPlace(HullSampleComparer); |
| | 3806 | 66 | | Span<HullSample> pool = samples.AsSpan(); |
| | 3806 | 67 | | int unique = 1; |
| | 28780 | 68 | | for (int index = 1; index < pool.Length; index++) |
| | 10584 | 69 | | if (pool[index].Contact.FirstAnchor.CompareLocalFeature(pool[unique - 1].Contact.FirstAnchor) != 0) |
| | 8648 | 70 | | pool[unique++] = pool[index]; |
| | 3806 | 71 | | pool = pool[..unique]; |
| | 3806 | 72 | | Span<int> chosen = stackalloc int[ContactManifold.MaxContactsPerGroup]; |
| | 3806 | 73 | | int count = Math.Min(pool.Length, chosen.Length); |
| | 44893 | 74 | | for (int slot = 0; slot < count; slot++) chosen[slot] = slot; |
| | 3806 | 75 | | if (pool.Length > chosen.Length) |
| | | 76 | | { |
| | 7 | 77 | | Vector3d normal = pool[0].Contact.Normal; |
| | 7 | 78 | | var axis = new WideAxis3(Signed320.ExtendValue(Signed192.Raw(normal.X)), |
| | 7 | 79 | | Signed320.ExtendValue(Signed192.Raw(normal.Y)), Signed320.ExtendValue(Signed192.Raw(normal.Z))); |
| | | 80 | | // Coverage ranks final representable samples; admission and witnesses stay exact. |
| | | 81 | | // A 33-bit solver normal times the normalized basis fits well below 192 bits. |
| | 7 | 82 | | WideAxis3 localAxis = axis; |
| | 7 | 83 | | if (!exact) |
| | | 84 | | { |
| | 4 | 85 | | WideRigidProjection.GetLocalAxisProjections(axis, new WideRationalBasis3d(mesh.Rotation), |
| | 4 | 86 | | out Signed576 x, out Signed576 y, out Signed576 z); |
| | 4 | 87 | | localAxis = new WideAxis3(Signed320.NarrowValue(x), Signed320.NarrowValue(y), Signed320.NarrowValue(z)); |
| | | 88 | | } |
| | 7 | 89 | | var metrics = new WidePlaneMetrics(localAxis); |
| | 56 | 90 | | for (int slot = 1; slot < count; slot++) |
| | 21 | 91 | | chosen[slot] = MeshPlaneSampleReduction.ChooseNext<HullSample>(pool, chosen[..slot], metrics, HullSample |
| | | 92 | | // A support line needs only its two extreme velocity levers. |
| | 8 | 93 | | if (metrics.GetAreaSquared(pool[0].Point, pool[chosen[1]].Point, pool[chosen[2]].Point).IsZero) count = 2; |
| | | 94 | | } |
| | 32462 | 95 | | for (int slot = 0; slot < count; slot++) |
| | | 96 | | { |
| | 12425 | 97 | | FixedContactAnchors contact = pool[chosen[slot]].Contact; |
| | 12425 | 98 | | manifold.AddContact(new ContactAnchor(reverse ? contact.SecondAnchor : contact.FirstAnchor), |
| | 12425 | 99 | | new ContactAnchor(reverse ? contact.FirstAnchor : contact.SecondAnchor), contact.Depth, |
| | 12425 | 100 | | reverse ? -contact.Normal : contact.Normal, reverse ? material : hull.Material, |
| | 12425 | 101 | | reverse ? hull.Material : material, contact.DepthIsClamped, namespaceA, namespaceB, group); |
| | | 102 | | } |
| | 3806 | 103 | | return true; |
| | | 104 | | } |
| | | 105 | | |
| | | 106 | | public static bool TryBuildMeshSphereManifold( |
| | | 107 | | CollisionWorkItem pair, |
| | | 108 | | LSMeshCollider mesh, |
| | | 109 | | LSSphereCollider sphere, |
| | | 110 | | SwiftList<int> triangleBuffer) |
| | | 111 | | { |
| | 13511 | 112 | | return TryBuildRoundedAxisManifold(pair, mesh, sphere.Center, sphere.Rotation, |
| | 13511 | 113 | | Fixed64.Zero, sphere.ScaledRadius, CreateBounds(sphere.BoundsMin, sphere.BoundsMax), triangleBuffer); |
| | | 114 | | } |
| | | 115 | | |
| | | 116 | | public static bool TryBuildMeshCapsuleManifold( |
| | | 117 | | CollisionWorkItem pair, |
| | | 118 | | LSMeshCollider mesh, |
| | | 119 | | LSCapsuleCollider capsule, |
| | | 120 | | SwiftList<int> triangleBuffer) |
| | | 121 | | { |
| | 12803 | 122 | | return TryBuildRoundedAxisManifold(pair, mesh, capsule.Center, capsule.Rotation, |
| | 12803 | 123 | | capsule.AxisLength, capsule.ScaledRadius, CreateBounds(capsule.BoundsMin, capsule.BoundsMax), triangleBuffer |
| | | 124 | | } |
| | | 125 | | |
| | | 126 | | private static bool BuildJoinedRoundedManifold(CollisionWorkItem pair, LSMeshCollider collider, |
| | | 127 | | StaticSurface surface, Vector3d center, FixedQuaternion rotation, Fixed64 coreLength, Fixed64 radius) |
| | | 128 | | { |
| | 13192 | 129 | | surface.BuildRounded(center, rotation, coreLength, radius); |
| | 13192 | 130 | | MeshCapsuleFaceRegions faces = surface.Faces; |
| | 52814 | 131 | | for (int region = 0; region < faces.RegionCount; region++) |
| | 53448 | 132 | | for (int sample = 0; sample < faces.GetSampleCount(region); sample++) |
| | | 133 | | { |
| | 13509 | 134 | | bool found = faces.TryGetJoinedSample(region, sample, out FixedContactAnchors contact, out LSMeshCollide |
| | | 135 | | System.Diagnostics.Debug.Assert(found); |
| | | 136 | | // Selected rows belong to their supplier's pair; other overlapping members still raise events. |
| | 13509 | 137 | | if (supplier != collider) continue; |
| | 6703 | 138 | | pair.Manifold.AddContact(new ContactAnchor(contact.FirstAnchor), new ContactAnchor(contact.SecondAnchor) |
| | 6703 | 139 | | contact.Depth, contact.Normal, collider.Material, pair.ColliderB.Material, contact.DepthIsClamped, |
| | 6703 | 140 | | group: new ContactGroupKey(0, 0, 0, 0, region)); |
| | | 141 | | } |
| | 13192 | 142 | | return faces.AppendJoinedBoundaries(collider, pair.ColliderB.Material, center, rotation, coreLength, |
| | 13192 | 143 | | radius, pair.Context.CollisionScratch.MeshCapsuleBoundary, pair.Manifold); |
| | | 144 | | } |
| | | 145 | | |
| | | 146 | | private static bool TryBuildRoundedAxisManifold(CollisionWorkItem pair, LSMeshCollider collider, |
| | | 147 | | Vector3d center, FixedQuaternion rotation, Fixed64 coreLength, Fixed64 radius, |
| | | 148 | | FixedBoundVolume bounds, SwiftList<int> candidates) |
| | | 149 | | { |
| | 26314 | 150 | | PhysicsMesh mesh = collider.Mesh; |
| | | 151 | | // Zero-core capsules are spheres: body spin must not rotate their |
| | | 152 | | // geometry frame and introduce a rounded off-center impulse lever. |
| | 39836 | 153 | | if (coreLength == Fixed64.Zero) rotation = FixedQuaternion.Identity; |
| | 26314 | 154 | | if (collider.StaticSurface is { } staticSurface && !collider.IsTrigger && !pair.ColliderB.IsTrigger |
| | 26314 | 155 | | && staticSurface.Gather(pair.ColliderB, bounds) > 1) |
| | 13192 | 156 | | return BuildJoinedRoundedManifold(pair, collider, staticSurface, center, rotation, coreLength, radius); |
| | 13122 | 157 | | collider.GetTrianglesInBounds(bounds, candidates); |
| | 13122 | 158 | | CollisionSatScratch scratch = pair.Context.CollisionScratch; |
| | 13122 | 159 | | SwiftList<int> admitted = scratch.MeshTriangleCandidatesB; |
| | 13122 | 160 | | scratch.MeshSurfaces.Clear(); |
| | 13122 | 161 | | var frame = new CapsuleFiniteSectionFrame(mesh.GeometryFrame, center, rotation, coreLength, radius); |
| | 13122 | 162 | | Span<int> singleton = stackalloc int[1]; |
| | 122332 | 163 | | foreach (int triangle in candidates.AsReadOnlySpan()) |
| | | 164 | | { |
| | 48044 | 165 | | int owner = mesh.GetManifoldSurfaceOwner(triangle); |
| | 48044 | 166 | | if (!scratch.MeshSurfaces.Add(owner)) continue; |
| | 13113 | 167 | | admitted.FastClear(); |
| | | 168 | | // A trusted owner with no shared-edge neighbor is one triangle. |
| | | 169 | | // Individually owned facets therefore retain linear gathering. |
| | 13113 | 170 | | singleton[0] = triangle; |
| | 13113 | 171 | | ReadOnlySpan<int> surfaceCandidates = mesh.GetCoplanarTriangleNeighbors(triangle).IsEmpty |
| | 13113 | 172 | | ? singleton : candidates.AsReadOnlySpan(); |
| | 122410 | 173 | | foreach (int candidate in surfaceCandidates) |
| | | 174 | | { |
| | 48092 | 175 | | if (mesh.GetManifoldSurfaceOwner(candidate) != owner) continue; |
| | 48044 | 176 | | mesh.GetGeometryTriangleVertices(candidate, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 70488 | 177 | | if (CapsulePlaneRayEvents.IntersectsTriangle(frame, new FixedTriangle(a, b, c))) admitted.Add(candidate) |
| | | 178 | | } |
| | 13113 | 179 | | if (admitted.Count == 0) continue; |
| | 13105 | 180 | | MeshSurfaceConnectivity connectivity = scratch.MeshConnectivity; |
| | 13105 | 181 | | connectivity.BuildCapsule(mesh, triangle, frame, admitted.AsReadOnlySpan()); |
| | 13105 | 182 | | MeshCapsuleFaceRegions faces = scratch.MeshCapsuleFaces; |
| | 13105 | 183 | | faces.Build(mesh, triangle, frame, center, rotation, connectivity); |
| | 13105 | 184 | | int surface = mesh.GetCanonicalSurfaceOrdinal(triangle); |
| | 54768 | 185 | | for (int region = 0; region < faces.RegionCount; region++) |
| | | 186 | | { |
| | 14279 | 187 | | var group = new ContactGroupKey(0, 0, surface, 0, region); |
| | 14279 | 188 | | int count = faces.GetSampleCount(region); |
| | 66408 | 189 | | for (int sample = 0; sample < count; sample++) |
| | | 190 | | { |
| | 18925 | 191 | | bool found = faces.TryGetSample(region, sample, out FixedContactAnchors contact); |
| | | 192 | | System.Diagnostics.Debug.Assert(found); |
| | 18925 | 193 | | pair.Manifold.AddContact(new ContactAnchor(contact.FirstAnchor), new ContactAnchor(contact.SecondAnc |
| | 18925 | 194 | | contact.Depth, contact.Normal, collider.Material, pair.ColliderB.Material, |
| | 18925 | 195 | | contact.DepthIsClamped, group: group); |
| | | 196 | | } |
| | | 197 | | } |
| | | 198 | | // Boundary constraints retain their own feature direction. They |
| | | 199 | | // cannot replace or consume the region's finite face sample cap. |
| | 13105 | 200 | | if (faces.HasBoundaryIntersection) |
| | 374 | 201 | | scratch.MeshCapsuleBoundary.BuildSurfaceContacts(collider, pair.ColliderB.Material, |
| | 374 | 202 | | center, rotation, coreLength, radius, triangle, connectivity, faces, pair.Manifold); |
| | | 203 | | } |
| | 13122 | 204 | | return pair.Manifold.HasContact; |
| | | 205 | | } |
| | | 206 | | |
| | | 207 | | public static bool TryBuildMeshCuboidManifold( |
| | | 208 | | CollisionWorkItem pair, |
| | | 209 | | LSMeshCollider mesh, |
| | | 210 | | LSCuboidCollider cuboid, |
| | | 211 | | SwiftList<int> triangleBuffer) |
| | | 212 | | { |
| | | 213 | | // Declared surface ownership still applies when only one piece is near |
| | | 214 | | // the mover. Falling back to independent triangles exposes its internal |
| | | 215 | | // diagonals as pressure boundaries and changes response mid-crossing. |
| | 4723 | 216 | | if (mesh.StaticSurface is { } surface && !mesh.IsTrigger && !cuboid.IsTrigger |
| | 4723 | 217 | | && surface.GatherProjectedSupport(cuboid) > 0) |
| | | 218 | | { |
| | 3580 | 219 | | Vector3d half = cuboid.OrientedBox.HalfExtents; |
| | 3580 | 220 | | surface.BuildSlab(cuboid, Fixed64.Zero, half.Y, new(half.X, half.Z)); |
| | 3580 | 221 | | return surface.SlabFaces.AppendJoinedContacts(mesh, cuboid.Material, pair.Context.CollisionScratch.MeshSlabB |
| | | 222 | | } |
| | 1143 | 223 | | mesh.GetTrianglesInBounds(CreateBounds(cuboid.BoundsMin, cuboid.BoundsMax), triangleBuffer); |
| | 6206 | 224 | | for (int i = 0; i < triangleBuffer.Count; i++) |
| | 1960 | 225 | | TryAddCuboidTriangleContact(pair, mesh, triangleBuffer[i], cuboid); |
| | | 226 | | |
| | 1143 | 227 | | return pair.Manifold.HasContact; |
| | | 228 | | } |
| | | 229 | | |
| | | 230 | | public static bool TryBuildMeshCylinderManifold( |
| | | 231 | | CollisionWorkItem pair, |
| | | 232 | | LSMeshCollider mesh, |
| | | 233 | | LSCylinderCollider cylinder, |
| | | 234 | | SwiftList<int> triangleBuffer) |
| | | 235 | | { |
| | 4119 | 236 | | if (mesh.StaticSurface is { } surface && !mesh.IsTrigger && !cylinder.IsTrigger |
| | 4119 | 237 | | && surface.GatherProjectedSupport(cylinder) > 0) |
| | | 238 | | { |
| | 3530 | 239 | | surface.BuildSlab(cylinder, cylinder.ScaledRadius, cylinder.Height, default); |
| | 3530 | 240 | | return surface.SlabFaces.AppendJoinedContacts(mesh, cylinder.Material, pair.Context.CollisionScratch.MeshSla |
| | | 241 | | } |
| | 589 | 242 | | mesh.GetTrianglesInBounds(CreateBounds(cylinder.BoundsMin, cylinder.BoundsMax), triangleBuffer); |
| | 589 | 243 | | CollisionSatScratch scratch = pair.Context.CollisionScratch; |
| | 589 | 244 | | scratch.MeshSurfaces.Clear(); |
| | 3344 | 245 | | foreach (int triangle in triangleBuffer.AsReadOnlySpan()) |
| | | 246 | | { |
| | | 247 | | // Isolated/declined triangles have no covered diagonal and keep |
| | | 248 | | // their complete finite-triangle kernel and existing warm cost. |
| | 1083 | 249 | | if (mesh.Mesh.GetCoplanarTriangleNeighbors(triangle).IsEmpty) |
| | | 250 | | { |
| | 39 | 251 | | TryAddCylinderTriangleContact(pair, mesh, triangle, cylinder); |
| | 39 | 252 | | continue; |
| | | 253 | | } |
| | 1044 | 254 | | if (!scratch.MeshSurfaces.Add(mesh.Mesh.GetManifoldSurfaceOwner(triangle))) continue; |
| | 517 | 255 | | AppendMeshCylinderSurfaceContacts(pair, mesh, triangle, cylinder, triangleBuffer.AsReadOnlySpan()); |
| | | 256 | | } |
| | 589 | 257 | | return pair.Manifold.HasContact; |
| | | 258 | | } |
| | | 259 | | |
| | | 260 | | // Keep the wide frame's stack storage off the isolated-triangle hot path. |
| | | 261 | | private static void AppendMeshCylinderSurfaceContacts(CollisionWorkItem pair, LSMeshCollider mesh, |
| | | 262 | | int triangle, LSCylinderCollider cylinder, ReadOnlySpan<int> candidates) |
| | | 263 | | { |
| | 517 | 264 | | mesh.Mesh.GetGeometryTriangleVertices(triangle, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 517 | 265 | | var frame = new FiniteSlabRayFrame(new FixedTriangle(a, b, c), mesh.Mesh.GeometryFrame, |
| | 517 | 266 | | cylinder.Center, cylinder.Rotation, Fixed64.Zero, cylinder.ScaledRadius, cylinder.Height, fullHeight: true); |
| | 517 | 267 | | bool containsSection = MeshSlabFaceRegions.ContainsSection(mesh.Mesh, triangle, frame, ReadOnlySpan<Vector2d>.Em |
| | 517 | 268 | | SwiftList<int> admitted = pair.Context.CollisionScratch.MeshTriangleCandidatesB; |
| | 517 | 269 | | if (!GatherMeshSlabSurfaceTriangles(mesh.Mesh, triangle, frame.Section, ReadOnlySpan<Vector2d>.Empty, |
| | 519 | 270 | | candidates, admitted, containsSection)) return; |
| | 515 | 271 | | AppendMeshSlabSurfaceContacts(mesh, null, triangle, frame, ReadOnlySpan<Vector2d>.Empty, |
| | 515 | 272 | | admitted.AsReadOnlySpan(), containsSection, pair.Manifold, 0, 0, cylinder.Material); |
| | 515 | 273 | | } |
| | | 274 | | |
| | | 275 | | internal static void AppendMeshSlabSurfaceContacts(LSMeshCollider collider, LSCollider2D? embedded, |
| | | 276 | | int triangle, in FiniteSlabRayFrame frame, ReadOnlySpan<Vector2d> prismOffsets, |
| | | 277 | | ReadOnlySpan<int> admitted, bool containsSection, ContactManifold manifold, int namespace3D, int namespace2D, |
| | | 278 | | PhysicsMaterial? targetMaterial = null) |
| | | 279 | | { |
| | 2492 | 280 | | PhysicsMesh mesh = collider.Mesh; |
| | 2492 | 281 | | CollisionSatScratch scratch = collider.Context.CollisionScratch; |
| | 2492 | 282 | | MeshSurfaceConnectivity connectivity = scratch.MeshConnectivity; |
| | 2492 | 283 | | connectivity.BuildSlab(mesh, triangle, frame.Section, embedded, admitted); |
| | 2492 | 284 | | MeshSlabFaceRegions faces = scratch.MeshSlabFaces; |
| | 2492 | 285 | | faces.Prepare(frame, prismOffsets, containsSection); |
| | 2492 | 286 | | PhysicsMaterial material = targetMaterial ?? embedded?.Material ?? collider.Material; |
| | 2492 | 287 | | int contactCount = manifold.Count; |
| | | 288 | | // A covered axial footprint has no intersected physical rim. Reuse |
| | | 289 | | // that proof rather than repeating the same exact boundary queries. |
| | 2492 | 290 | | if (!containsSection) |
| | 1388 | 291 | | scratch.MeshSlabBoundary.BuildSurfaceContacts(collider, embedded, triangle, |
| | 1388 | 292 | | connectivity, faces, manifold, namespace3D, namespace2D, material); |
| | 2492 | 293 | | faces.Build(mesh, triangle, connectivity, prismOffsets, embedded, containsSection); |
| | 2492 | 294 | | faces.AppendContacts(collider, triangle, manifold, namespace3D, namespace2D, material); |
| | 2492 | 295 | | if (manifold.Count == contactCount) |
| | 12 | 296 | | scratch.MeshSlabBoundary.AppendSupportFallback(collider, embedded, triangle, faces, |
| | 12 | 297 | | manifold, namespace3D, namespace2D, material); |
| | 2492 | 298 | | } |
| | | 299 | | |
| | | 300 | | internal static bool GatherMeshSlabSurfaceTriangles(PhysicsMesh mesh, int triangle, |
| | | 301 | | in FiniteSlabSectionFrame section, ReadOnlySpan<Vector2d> prismOffsets, |
| | | 302 | | ReadOnlySpan<int> candidates, SwiftList<int> admitted, bool stopAfterFirst = false) |
| | | 303 | | { |
| | 2669 | 304 | | admitted.FastClear(); |
| | 2669 | 305 | | int owner = mesh.GetManifoldSurfaceOwner(triangle); |
| | 20863 | 306 | | foreach (int candidate in candidates) |
| | | 307 | | { |
| | 7891 | 308 | | if (mesh.GetManifoldSurfaceOwner(candidate) != owner) continue; |
| | 6569 | 309 | | mesh.GetGeometryTriangleVertices(candidate, out Vector3d first, out Vector3d second, out Vector3d third); |
| | 6569 | 310 | | if (!section.IntersectsTriangle(new FixedTriangle(first, second, third), prismOffsets)) continue; |
| | 5267 | 311 | | admitted.Add(candidate); |
| | 5267 | 312 | | if (stopAfterFirst) break; |
| | | 313 | | } |
| | 2669 | 314 | | return admitted.Count != 0; |
| | | 315 | | } |
| | | 316 | | |
| | | 317 | | public static bool TryBuildMeshMeshManifold( |
| | | 318 | | CollisionWorkItem pair, |
| | | 319 | | LSMeshCollider meshA, |
| | | 320 | | LSMeshCollider meshB, |
| | | 321 | | SwiftList<int> triangleBufferA, |
| | | 322 | | SwiftList<int> triangleBufferB) |
| | | 323 | | { |
| | 6758 | 324 | | CollisionSatScratch scratch = pair.Context.CollisionScratch; |
| | 6758 | 325 | | scratch.MeshSurfaces.Clear(); |
| | 6758 | 326 | | LSMeshCollider? declared = meshA.StaticSurface != null ? meshA : meshB.StaticSurface != null ? meshB : null; |
| | 6758 | 327 | | LSMeshCollider mover = declared == meshA ? meshB : meshA; |
| | 6758 | 328 | | if (declared != null |
| | 6758 | 329 | | && !meshA.IsTrigger && !meshB.IsTrigger && declared.StaticSurface!.GatherProjectedSupport(mover) > 0) |
| | | 330 | | { |
| | 5955 | 331 | | declared.StaticSurface.BuildHull(mover); |
| | 5955 | 332 | | return declared.StaticSurface.HullContacts.Append(declared, mover, pair.Manifold, declared == meshB); |
| | | 333 | | } |
| | | 334 | | // The sole caller routes closed convex/closed convex pairs through |
| | | 335 | | // its solid-hull owner before entering this finite-surface manifold. |
| | 803 | 336 | | LSMeshCollider? surface = meshA.Mode == MeshColliderMode.Convex && meshA.Mesh.IsClosedSurface |
| | 803 | 337 | | && !meshB.Mesh.IsClosedSurface ? meshB |
| | 803 | 338 | | : meshB.Mode == MeshColliderMode.Convex && meshB.Mesh.IsClosedSurface |
| | 803 | 339 | | && !meshA.Mesh.IsClosedSurface ? meshA : null; |
| | 803 | 340 | | if (surface != null) |
| | | 341 | | { |
| | 130 | 342 | | LSMeshCollider hull = surface == meshA ? meshB : meshA; |
| | 130 | 343 | | surface.GetTrianglesInBounds(CreateBounds(hull.BoundsMin, hull.BoundsMax), triangleBufferA); |
| | 468 | 344 | | foreach (int triangle in triangleBufferA.AsReadOnlySpan()) |
| | | 345 | | { |
| | 104 | 346 | | int owner = surface.Mesh.GetManifoldSurfaceOwner(triangle); |
| | 104 | 347 | | if (!scratch.MeshSurfaces.Add(owner)) continue; |
| | 44 | 348 | | int ordinal = surface.Mesh.GetCanonicalSurfaceOrdinal(triangle); |
| | 44 | 349 | | var group = new ContactGroupKey(0, 0, surface == meshA ? ordinal : 0, surface == meshB ? ordinal : 0); |
| | 44 | 350 | | ReadOnlySpan<int> corners = surface.Mesh.GetConvexCoplanarPatchCornerVertexIndices(triangle); |
| | 44 | 351 | | if (!corners.IsEmpty) |
| | | 352 | | { |
| | 36 | 353 | | int count = corners.Length; |
| | 70 | 354 | | if (scratch.PolyhedronVertices.Length < count) Array.Resize(ref scratch.PolyhedronVertices, count); |
| | 70 | 355 | | if (scratch.PolyhedronTriangles.Length < 3 * (count - 2)) Array.Resize(ref scratch.PolyhedronTriangl |
| | 70 | 356 | | if (scratch.PolyhedronEdges.Length < 2 * count) Array.Resize(ref scratch.PolyhedronEdges, 2 * count) |
| | 36 | 357 | | Span<Vector3d> vertices = scratch.PolyhedronVertices.AsSpan(0, count); |
| | 36 | 358 | | Span<int> triangles = scratch.PolyhedronTriangles.AsSpan(0, 3 * (count - 2)); |
| | 36 | 359 | | Span<int> edges = scratch.PolyhedronEdges.AsSpan(0, 2 * count); |
| | 360 | 360 | | for (int index = 0; index < count; index++) |
| | | 361 | | { |
| | 144 | 362 | | vertices[index] = surface.Mesh.GeometryVertices[corners[index]]; |
| | 144 | 363 | | edges[2 * index] = index; |
| | 144 | 364 | | edges[2 * index + 1] = (index + 1) % count; |
| | 144 | 365 | | if (index < 2) continue; |
| | 72 | 366 | | int face = 3 * (index - 2); |
| | 216 | 367 | | triangles[face] = 0; triangles[face + 1] = index - 1; triangles[face + 2] = index; |
| | | 368 | | } |
| | | 369 | | // Use the trusted finite patch once, not its artificial triangle diagonals. |
| | 36 | 370 | | if (TryAppendHullContacts(hull, hull.Mesh.GeometryVertices, hull.Mesh.Triangles, |
| | 36 | 371 | | hull.Mesh.ConvexSatEdgeVertexPairs, surface.Mesh.GeometryFrame, surface.Mesh.Origin, |
| | 36 | 372 | | vertices, triangles, edges, edges, surface.Material, pair.Manifold, hull == meshB, group: group) |
| | | 373 | | continue; |
| | | 374 | | } |
| | 120 | 375 | | foreach (int candidate in triangleBufferA.AsReadOnlySpan()) |
| | 48 | 376 | | if (surface.Mesh.GetManifoldSurfaceOwner(candidate) == owner) |
| | 40 | 377 | | AppendMeshTriangleContacts(pair, surface, candidate, hull, triangleBufferB, |
| | 40 | 378 | | surface == meshB, group); |
| | | 379 | | } |
| | 130 | 380 | | return pair.Manifold.HasContact; |
| | | 381 | | } |
| | 673 | 382 | | bool reverseContact = meshA.Id > meshB.Id; |
| | 673 | 383 | | LSMeshCollider firstMesh = reverseContact ? meshB : meshA; |
| | 673 | 384 | | LSMeshCollider secondMesh = reverseContact ? meshA : meshB; |
| | 673 | 385 | | firstMesh.GetTrianglesInBounds( |
| | 673 | 386 | | CreateBounds(secondMesh.BoundsMin, secondMesh.BoundsMax), |
| | 673 | 387 | | triangleBufferA); |
| | 16736 | 388 | | for (int i = 0; i < triangleBufferA.Count; i++) |
| | 7695 | 389 | | AppendMeshTriangleContacts(pair, firstMesh, triangleBufferA[i], secondMesh, triangleBufferB, reverseContact) |
| | | 390 | | |
| | 673 | 391 | | return pair.Manifold.HasContact; |
| | | 392 | | } |
| | | 393 | | |
| | | 394 | | private static void AppendMeshTriangleContacts(CollisionWorkItem pair, LSMeshCollider firstMesh, |
| | | 395 | | int triangle, LSMeshCollider secondMesh, SwiftList<int> candidates, bool reverse, |
| | | 396 | | ContactGroupKey? group = null) |
| | | 397 | | { |
| | 7735 | 398 | | firstMesh.Mesh.GetGeometryTriangleVertices(triangle, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 7735 | 399 | | var firstTriangle = new FixedTriangle(a, b, c); |
| | 7735 | 400 | | bool exact = !firstMesh.Mesh.GeometryFrame.IsRigid || !secondMesh.Mesh.GeometryFrame.IsRigid; |
| | 7735 | 401 | | if (exact) |
| | | 402 | | { |
| | 24 | 403 | | bool represented = firstMesh.Mesh.GeometryFrame.TryGetBoundsIn(firstTriangle.Bounds.Min, firstTriangle.Bound |
| | 24 | 404 | | secondMesh.Mesh.GeometryFrame, out FixedBoundBox bounds); |
| | | 405 | | // Collider admission guarantees nonzero target scale products; |
| | | 406 | | // the bounds owner clips only final source-chart endpoints. |
| | | 407 | | System.Diagnostics.Debug.Assert(represented); |
| | 24 | 408 | | secondMesh.Mesh.GetTrianglesInGeometryBounds(new FixedBoundVolume(bounds.Min, bounds.Max), candidates); |
| | | 409 | | } |
| | | 410 | | else |
| | | 411 | | { |
| | 7711 | 412 | | GetTriangleBoundsInFrame(firstTriangle.Bounds, firstMesh.Mesh.Origin, firstMesh.Mesh.Rotation, |
| | 7711 | 413 | | secondMesh.Mesh.Origin, secondMesh.Mesh.Rotation, out FixedBoundVolume bounds); |
| | 7711 | 414 | | secondMesh.Mesh.GetTrianglesInGeometryBounds(bounds, candidates); |
| | | 415 | | } |
| | 7735 | 416 | | bool canonicalReverse = firstMesh.Id > secondMesh.Id; |
| | 77880 | 417 | | for (int j = 0; j < candidates.Count; j++) |
| | | 418 | | { |
| | 31205 | 419 | | secondMesh.Mesh.GetGeometryTriangleVertices(candidates[j], out a, out b, out c); |
| | 31205 | 420 | | var secondTriangle = new FixedTriangle(a, b, c); |
| | 31205 | 421 | | if (exact) |
| | | 422 | | { |
| | 96 | 423 | | AppendExactTriangleContacts(pair, firstMesh, firstTriangle, secondMesh, secondTriangle, |
| | 96 | 424 | | canonicalReverse, reverse, group); |
| | 96 | 425 | | continue; |
| | | 426 | | } |
| | | 427 | | FixedContactAnchors contact; |
| | 31109 | 428 | | bool intersects = canonicalReverse |
| | 31109 | 429 | | ? secondTriangle.TryGetContact(secondMesh.Mesh.Origin, secondMesh.Mesh.Rotation, |
| | 31109 | 430 | | firstMesh.Mesh.Origin, firstMesh.Mesh.Rotation, firstTriangle, out contact) |
| | 31109 | 431 | | : firstTriangle.TryGetContact(firstMesh.Mesh.Origin, firstMesh.Mesh.Rotation, |
| | 31109 | 432 | | secondMesh.Mesh.Origin, secondMesh.Mesh.Rotation, secondTriangle, out contact); |
| | 31109 | 433 | | if (!intersects) continue; |
| | | 434 | | |
| | 19872 | 435 | | AddMeshTriangleContact(pair, contact, reverse != canonicalReverse, group); |
| | | 436 | | } |
| | 7735 | 437 | | } |
| | | 438 | | |
| | | 439 | | private static void AppendExactTriangleContacts(CollisionWorkItem pair, LSMeshCollider firstMesh, |
| | | 440 | | in FixedTriangle first, LSMeshCollider secondMesh, in FixedTriangle second, |
| | | 441 | | bool canonicalReverse, bool reverse, ContactGroupKey? group) |
| | | 442 | | { |
| | 96 | 443 | | ref readonly ScaledRigidFrame3d firstFrame = ref (canonicalReverse |
| | 96 | 444 | | ? ref secondMesh.Mesh.GeometryFrame : ref firstMesh.Mesh.GeometryFrame); |
| | 96 | 445 | | ref readonly ScaledRigidFrame3d secondFrame = ref (canonicalReverse |
| | 96 | 446 | | ? ref firstMesh.Mesh.GeometryFrame : ref secondMesh.Mesh.GeometryFrame); |
| | 192 | 447 | | FixedTriangle primary = canonicalReverse ? second : first, secondary = canonicalReverse ? first : second; |
| | 96 | 448 | | if (WideTriangleRelations.TryGetContact(primary, firstFrame, |
| | 96 | 449 | | canonicalReverse ? secondMesh.Mesh.Origin : firstMesh.Mesh.Origin, |
| | 96 | 450 | | secondary, secondFrame, canonicalReverse ? firstMesh.Mesh.Origin : secondMesh.Mesh.Origin, |
| | 96 | 451 | | out FixedContactAnchors contact)) |
| | 60 | 452 | | AddMeshTriangleContact(pair, contact, reverse != canonicalReverse, group); |
| | 96 | 453 | | } |
| | | 454 | | |
| | | 455 | | private static void AddMeshTriangleContact(CollisionWorkItem pair, FixedContactAnchors contact, bool reverse, |
| | | 456 | | ContactGroupKey? group) |
| | | 457 | | { |
| | 19932 | 458 | | ContactAnchor first = new(reverse ? contact.SecondAnchor : contact.FirstAnchor); |
| | 19932 | 459 | | ContactAnchor second = new(reverse ? contact.FirstAnchor : contact.SecondAnchor); |
| | 19932 | 460 | | Vector3d normal = reverse ? -contact.Normal : contact.Normal; |
| | 19932 | 461 | | if (group.HasValue) |
| | 116 | 462 | | pair.Manifold.AddContact(first, second, contact.Depth, normal, |
| | 116 | 463 | | pair.ColliderA.Material, pair.ColliderB.Material, contact.DepthIsClamped, group: group.Value); |
| | 19816 | 464 | | else AddContact(pair, first, second, contact.Depth, normal, contact.DepthIsClamped); |
| | 19816 | 465 | | } |
| | | 466 | | |
| | | 467 | | private static void TryAddCuboidTriangleContact( |
| | | 468 | | CollisionWorkItem pair, |
| | | 469 | | LSMeshCollider mesh, |
| | | 470 | | int triangleIndex, |
| | | 471 | | LSCuboidCollider cuboid) |
| | | 472 | | { |
| | 1960 | 473 | | mesh.Mesh.GetGeometryTriangleVertices( |
| | 1960 | 474 | | triangleIndex, |
| | 1960 | 475 | | out Vector3d first, |
| | 1960 | 476 | | out Vector3d second, |
| | 1960 | 477 | | out Vector3d third); |
| | 1960 | 478 | | var triangle = new FixedTriangle(first, second, third); |
| | 1960 | 479 | | if (!mesh.Mesh.GeometryFrame.IsRigid) |
| | | 480 | | { |
| | 8 | 481 | | Span<Vector3d> box = stackalloc Vector3d[8]; |
| | 8 | 482 | | WideOrientedBox.GetBoxHullVertices(cuboid.OrientedBox.HalfExtents, box); |
| | 8 | 483 | | ReadOnlySpan<Vector3d> points = stackalloc Vector3d[3] { first, second, third }; |
| | 8 | 484 | | ReadOnlySpan<int> face = stackalloc int[3] { 0, 1, 2 }; |
| | 8 | 485 | | ReadOnlySpan<int> edges = stackalloc int[6] { 0, 1, 1, 2, 2, 0 }; |
| | 8 | 486 | | var boxFrame = ScaledRigidFrame3d.Rigid(cuboid.Center, cuboid.Rotation); |
| | 8 | 487 | | if (!WideOrientedBox.TryGetConvexHullPenetration(boxFrame, box, |
| | 8 | 488 | | WideOrientedBox.BoxHullTriangles, WideOrientedBox.BoxHullEdges, |
| | 9 | 489 | | mesh.Mesh.GeometryFrame, points, face, edges, out var penetration)) return; |
| | 7 | 490 | | WideOrientedBox.VisitConvexHullContacts(boxFrame, cuboid.Center, box, |
| | 7 | 491 | | WideOrientedBox.BoxHullTriangles, WideOrientedBox.BoxHullEdges, mesh.Mesh.GeometryFrame, |
| | 7 | 492 | | mesh.Mesh.Origin, points, face, edges, penetration, pair, |
| | 7 | 493 | | static (state, contact) => AddReversedBoxTriangleContact(state, contact)); |
| | 7 | 494 | | return; |
| | | 495 | | } |
| | 1952 | 496 | | Span<FixedContactLocalPoints> faceContacts = |
| | 1952 | 497 | | stackalloc FixedContactLocalPoints[4]; |
| | 1952 | 498 | | if (!cuboid.OrientedBox.TryGetTriangleContact( |
| | 1952 | 499 | | mesh.Mesh.Origin, |
| | 1952 | 500 | | mesh.Mesh.Rotation, |
| | 1952 | 501 | | triangle, |
| | 1952 | 502 | | faceContacts, |
| | 1952 | 503 | | out FixedContactAnchors contact, |
| | 1952 | 504 | | out int faceContactCount)) |
| | | 505 | | { |
| | 454 | 506 | | return; |
| | | 507 | | } |
| | | 508 | | |
| | 1498 | 509 | | if (faceContactCount > 0) |
| | | 510 | | { |
| | 9972 | 511 | | for (int index = 0; index < faceContactCount; index++) |
| | | 512 | | { |
| | 3610 | 513 | | AddReversedBoxTriangleContact( |
| | 3610 | 514 | | pair, |
| | 3610 | 515 | | contact, |
| | 3610 | 516 | | faceContacts[index]); |
| | | 517 | | } |
| | 1376 | 518 | | return; |
| | | 519 | | } |
| | | 520 | | |
| | 122 | 521 | | AddReversedBoxTriangleContact(pair, contact); |
| | 122 | 522 | | } |
| | | 523 | | |
| | | 524 | | private static void AddReversedBoxTriangleContact( |
| | | 525 | | CollisionWorkItem pair, |
| | | 526 | | FixedContactAnchors contact) |
| | | 527 | | { |
| | 158 | 528 | | AddContact( |
| | 158 | 529 | | pair, |
| | 158 | 530 | | new ContactAnchor(contact.SecondAnchor), |
| | 158 | 531 | | new ContactAnchor(contact.FirstAnchor), |
| | 158 | 532 | | contact.Depth, |
| | 158 | 533 | | -contact.Normal, |
| | 158 | 534 | | contact.DepthIsClamped); |
| | 158 | 535 | | } |
| | | 536 | | |
| | | 537 | | private static void AddReversedBoxTriangleContact( |
| | | 538 | | CollisionWorkItem pair, |
| | | 539 | | FixedContactAnchors primary, |
| | | 540 | | FixedContactLocalPoints contact) |
| | | 541 | | { |
| | 3610 | 542 | | AddContact( |
| | 3610 | 543 | | pair, |
| | 3610 | 544 | | new ContactAnchor( |
| | 3610 | 545 | | primary.SecondAnchor.Origin, |
| | 3610 | 546 | | primary.SecondAnchor.Rotation, |
| | 3610 | 547 | | contact.SecondLocalPoint), |
| | 3610 | 548 | | new ContactAnchor( |
| | 3610 | 549 | | primary.FirstAnchor.Origin, |
| | 3610 | 550 | | primary.FirstAnchor.Rotation, |
| | 3610 | 551 | | contact.FirstLocalPoint), |
| | 3610 | 552 | | primary.Depth, |
| | 3610 | 553 | | -primary.Normal, |
| | 3610 | 554 | | primary.DepthIsClamped); |
| | 3610 | 555 | | } |
| | | 556 | | |
| | | 557 | | private static bool TryAddCylinderTriangleContact( |
| | | 558 | | CollisionWorkItem pair, |
| | | 559 | | LSMeshCollider mesh, |
| | | 560 | | int triangleIndex, |
| | | 561 | | LSCylinderCollider cylinder) |
| | | 562 | | { |
| | 39 | 563 | | mesh.Mesh.GetGeometryTriangleVertices( |
| | 39 | 564 | | triangleIndex, |
| | 39 | 565 | | out Vector3d first, |
| | 39 | 566 | | out Vector3d second, |
| | 39 | 567 | | out Vector3d third); |
| | 39 | 568 | | var triangle = new FixedTriangle(first, second, third); |
| | 39 | 569 | | if (!TriangleCylinderContact.TryGetContact(triangle, mesh.Mesh.GeometryFrame, |
| | 39 | 570 | | cylinder.Center, cylinder.Rotation, Signed192.Raw(cylinder.Height), cylinder.ScaledRadius, |
| | 39 | 571 | | out FixedContactAnchors contact, out bool isCapFaceContact)) |
| | | 572 | | { |
| | 2 | 573 | | return false; |
| | | 574 | | } |
| | | 575 | | |
| | | 576 | | // Enrichment belongs only to a certified cap/triangle-face winner. |
| | | 577 | | // Nearly parallel rim contacts must retain their selected witnesses. |
| | 37 | 578 | | var group = new ContactGroupKey(0, 0, mesh.Mesh.GetCanonicalSurfaceOrdinal(triangleIndex), 0); |
| | 37 | 579 | | if (isCapFaceContact && TryAddCylinderCapTriangleContacts( |
| | 37 | 580 | | pair, mesh, triangle, cylinder, contact, group)) |
| | | 581 | | { |
| | 11 | 582 | | return true; |
| | | 583 | | } |
| | | 584 | | |
| | 26 | 585 | | pair.Manifold.AddContact( |
| | 26 | 586 | | new ContactAnchor(contact.FirstAnchor), |
| | 26 | 587 | | new ContactAnchor(contact.SecondAnchor), |
| | 26 | 588 | | contact.Depth, |
| | 26 | 589 | | contact.Normal, |
| | 26 | 590 | | mesh.Material, cylinder.Material, contact.DepthIsClamped, group: group); |
| | 26 | 591 | | return true; |
| | | 592 | | } |
| | | 593 | | |
| | | 594 | | private static bool TryAddCylinderCapTriangleContacts( |
| | | 595 | | CollisionWorkItem pair, |
| | | 596 | | LSMeshCollider mesh, |
| | | 597 | | FixedTriangle triangle, |
| | | 598 | | LSCylinderCollider cylinder, |
| | | 599 | | in FixedContactAnchors primary, in ContactGroupKey group) |
| | | 600 | | { |
| | 12 | 601 | | Vector3d normal = primary.Normal; |
| | 12 | 602 | | CylinderContactGeometry.GetCapBasis(cylinder, out Vector3d tangentA, out Vector3d tangentB); |
| | | 603 | | // A valid support can be deduplicated or lose the four-contact reduction. |
| | | 604 | | // Geometric success must not depend on whether the manifold count grew. |
| | 12 | 605 | | bool found = TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal + tangentA, primary, group |
| | 12 | 606 | | found |= TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal - tangentA, primary, group); |
| | 12 | 607 | | found |= TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal + tangentB, primary, group); |
| | 12 | 608 | | found |= TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal - tangentB, primary, group); |
| | 12 | 609 | | return found || TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal, primary, group); |
| | | 610 | | } |
| | | 611 | | |
| | | 612 | | private static bool TryAddCylinderCapTriangleContact( |
| | | 613 | | CollisionWorkItem pair, |
| | | 614 | | LSMeshCollider mesh, |
| | | 615 | | FixedTriangle triangle, |
| | | 616 | | LSCylinderCollider cylinder, |
| | | 617 | | Vector3d supportDirection, |
| | | 618 | | in FixedContactAnchors primary, in ContactGroupKey group) |
| | | 619 | | { |
| | | 620 | | FixedContactAnchors contact; |
| | | 621 | | bool found; |
| | 49 | 622 | | if (mesh.Mesh.GeometryFrame.IsRigid) |
| | 41 | 623 | | found = triangle.TryGetCenteredFiniteCylinderSupportContact(mesh.Mesh.Origin, mesh.Mesh.Rotation, |
| | 41 | 624 | | cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius, |
| | 41 | 625 | | supportDirection, primary.Normal, out contact); |
| | | 626 | | else |
| | | 627 | | { |
| | 8 | 628 | | FixedPointAnchor support = WideGeometry.GetCenteredCylinderSupportAnchor(cylinder.Center, |
| | 8 | 629 | | cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius, supportDirection); |
| | 8 | 630 | | found = WideTriangleRelations.TryGetProjectedSupportContact(triangle, mesh.Mesh.GeometryFrame, |
| | 8 | 631 | | support, primary.Normal, mesh.Center, out contact); |
| | | 632 | | } |
| | 54 | 633 | | if (!found) return false; |
| | | 634 | | |
| | | 635 | | // Parallel cap/face samples share the selected exact plane gap. |
| | | 636 | | // The support query rounds into the triangle frame before measuring |
| | | 637 | | // depth; keep it for anchors only, avoiding a second depth rounding. |
| | 44 | 638 | | pair.Manifold.AddContact( |
| | 44 | 639 | | new ContactAnchor(contact.FirstAnchor), |
| | 44 | 640 | | new ContactAnchor(contact.SecondAnchor), |
| | 44 | 641 | | primary.Depth, |
| | 44 | 642 | | primary.Normal, |
| | 44 | 643 | | mesh.Material, cylinder.Material, primary.DepthIsClamped, group: group); |
| | 44 | 644 | | return true; |
| | | 645 | | } |
| | | 646 | | |
| | | 647 | | private static void GetTriangleBoundsInFrame( |
| | | 648 | | FixedBoundBox triangleBounds, |
| | | 649 | | Vector3d sourceOrigin, |
| | | 650 | | FixedQuaternion sourceRotation, |
| | | 651 | | Vector3d frameOrigin, |
| | | 652 | | FixedQuaternion frameRotation, |
| | | 653 | | out FixedBoundVolume bounds) |
| | | 654 | | { |
| | 7711 | 655 | | FixedBoundBox reframedBounds = |
| | 7711 | 656 | | FixedBoundBox.FromRelativeRotatedBoundsClippedToDomain( |
| | 7711 | 657 | | sourceOrigin, |
| | 7711 | 658 | | sourceRotation, |
| | 7711 | 659 | | triangleBounds.Min, |
| | 7711 | 660 | | triangleBounds.Max, |
| | 7711 | 661 | | frameOrigin, |
| | 7711 | 662 | | frameRotation); |
| | 7711 | 663 | | bounds = CreateBounds(reframedBounds.Min, reframedBounds.Max); |
| | 7711 | 664 | | } |
| | | 665 | | |
| | | 666 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 667 | | private static FixedBoundVolume CreateBounds(Vector3d min, Vector3d max) => |
| | 36560 | 668 | | new(min, max); |
| | | 669 | | |
| | | 670 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 671 | | private static void AddContact( |
| | | 672 | | CollisionWorkItem pair, |
| | | 673 | | ContactAnchor anchorOnFirst, |
| | | 674 | | ContactAnchor anchorOnSecond, |
| | | 675 | | Fixed64 depth, |
| | | 676 | | Vector3d normalFirstToSecond, |
| | | 677 | | bool depthIsClamped) |
| | | 678 | | { |
| | 23584 | 679 | | pair.Manifold.AddContact( |
| | 23584 | 680 | | anchorOnFirst, |
| | 23584 | 681 | | anchorOnSecond, |
| | 23584 | 682 | | depth, |
| | 23584 | 683 | | normalFirstToSecond, |
| | 23584 | 684 | | depthIsClamped); |
| | 23584 | 685 | | } |
| | | 686 | | |
| | | 687 | | } |