| | | 1 | | //======================================================================= |
| | | 2 | | // MeshSlabSurfaceBoundary.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 FixedMathSharp; |
| | | 10 | | using FixedMathSharp.Geometry; |
| | | 11 | | using Gravitas.Colliders; |
| | | 12 | | using Gravitas.Materials; |
| | | 13 | | using SwiftCollections; |
| | | 14 | | |
| | | 15 | | namespace Gravitas.CollisionHandling; |
| | | 16 | | |
| | | 17 | | /// <summary>Preserves matched finite slab contacts on a trusted surface's genuine perimeter.</summary> |
| | | 18 | | internal sealed class MeshSlabSurfaceBoundary |
| | | 19 | | { |
| | 10191 | 20 | | private readonly SwiftHashSet<int> _visitedCorners = new(); |
| | 10191 | 21 | | private readonly SwiftList<int> _supportVertices = new(); |
| | 10191 | 22 | | private readonly SwiftList<WideAxis3> _supportFan = new(); |
| | | 23 | | |
| | | 24 | | internal void AppendSupportFallback(LSMeshCollider collider, LSCollider2D? embedded, |
| | | 25 | | int surfaceTriangle, MeshSlabFaceRegions faces, ContactManifold manifold, int namespace3D, int namespace2D, |
| | | 26 | | PhysicsMaterial targetMaterial) |
| | | 27 | | { |
| | 12 | 28 | | PhysicsMesh mesh = collider.Mesh; |
| | 12 | 29 | | ReadOnlySpan<int> boundary = mesh.GetCanonicalSurfaceBoundaryVertexPairs(surfaceTriangle); |
| | 12 | 30 | | ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices; |
| | 12 | 31 | | FiniteSlabRayFrame frame = faces.Frame; |
| | 24 | 32 | | _supportVertices.FastClear(); _visitedCorners.Clear(); |
| | 344 | 33 | | foreach (int vertex in boundary) |
| | 240 | 34 | | if (_visitedCorners.Add(vertex)) _supportVertices.Add(vertex); |
| | 12 | 35 | | _supportFan.EnsureCapacity(_supportVertices.Count); |
| | 12 | 36 | | bool found = false; |
| | 12 | 37 | | FixedContactAnchors best = default; |
| | 12 | 38 | | int bestFeature = 0; |
| | 184 | 39 | | for (int edge = 0; edge < boundary.Length; edge += 2) |
| | | 40 | | { |
| | 160 | 41 | | int first = boundary[edge], second = boundary[edge + 1]; |
| | 80 | 42 | | Keep(first, second, checked(3 * (edge / 2)), false); |
| | 100 | 43 | | if (_visitedCorners.Remove(first)) Keep(first, first, checked(3 * (edge / 2) + 1), true); |
| | 140 | 44 | | if (_visitedCorners.Remove(second)) Keep(second, second, checked(3 * (edge / 2) + 2), true); |
| | | 45 | | } |
| | | 46 | | // A farthest owner/slab pair supplies an actual boundary vertex with a |
| | | 47 | | // global support normal, so the complete vertex charts cannot be empty. |
| | | 48 | | System.Diagnostics.Debug.Assert(found); |
| | 12 | 49 | | manifold.AddContact(new ContactAnchor(best.FirstAnchor), new ContactAnchor(best.SecondAnchor), |
| | 12 | 50 | | best.Depth, best.Normal, collider.Material, targetMaterial, best.DepthIsClamped, |
| | 12 | 51 | | featureNamespaceA: namespace3D, featureNamespaceB: namespace2D, |
| | 12 | 52 | | group: new ContactGroupKey(namespace3D, namespace2D, mesh.GetCanonicalSurfaceOrdinal(surfaceTriangle), 0, -b |
| | | 53 | | |
| | | 54 | | void Keep(int first, int second, int feature, bool pointOnly) |
| | | 55 | | { |
| | | 56 | | _supportFan.FastClear(); |
| | | 57 | | foreach (int vertex in _supportVertices.AsReadOnlySpan()) |
| | | 58 | | if (vertex != first) _supportFan.Add(-mesh.GeometryFrame.GetWorldDirectionNumerators( |
| | | 59 | | MeshSurfaceBoundaryTopology.Ray(mesh.GeometryVertices[first], mesh.GeometryVertices[vertex]))); |
| | | 60 | | // Global support makes the matched exit a separating plane for the |
| | | 61 | | // whole fill; a local hole-edge exit need not clear another region. |
| | | 62 | | Span<Vector2d> localBox = stackalloc Vector2d[4]; |
| | | 63 | | if (!SegmentSlabSurfaceContact.TryGetFeaturePenetration(new FixedSegment(mesh.GeometryVertices[first], mesh. |
| | | 64 | | mesh.GeometryFrame, frame.Center, frame.SlabRotation, frame.Section.RadialFrame.Length, |
| | | 65 | | frame.RadiusValue, frame.HalfThickness, CollisionDetectionMixed.GetConvexPrismOffsets(embedded, localBox |
| | | 66 | | _supportFan.AsReadOnlySpan(), !pointOnly, out FixedContactAnchors contact, |
| | | 67 | | fullHeight: frame.FullHeight != Fixed64.Zero)) return; |
| | | 68 | | if (found && (contact.DepthIsClamped && !best.DepthIsClamped |
| | | 69 | | || contact.DepthIsClamped == best.DepthIsClamped && contact.Depth >= best.Depth)) return; |
| | | 70 | | // Only final depth ranking is Q32: unclamped candidate order differs |
| | | 71 | | // from exact ranking by at most one raw unit; canonical ties stay first. |
| | | 72 | | found = true; best = contact; bestFeature = feature; |
| | | 73 | | } |
| | 12 | 74 | | } |
| | | 75 | | |
| | | 76 | | internal void BuildSurfaceContacts(LSMeshCollider collider, LSCollider2D? embedded, |
| | | 77 | | int surfaceTriangle, MeshSurfaceConnectivity connectivity, MeshSlabFaceRegions faces, |
| | | 78 | | ContactManifold manifold, int namespace3D, int namespace2D, PhysicsMaterial targetMaterial) |
| | | 79 | | { |
| | 11017 | 80 | | PhysicsMesh mesh = collider.Mesh; |
| | 11017 | 81 | | int owner = mesh.GetManifoldSurfaceOwner(surfaceTriangle); |
| | 11017 | 82 | | int surface = mesh.GetCanonicalSurfaceOrdinal(surfaceTriangle); |
| | 11017 | 83 | | ReadOnlySpan<int> boundary = mesh.GetCanonicalSurfaceBoundaryVertexPairs(surfaceTriangle); |
| | 11017 | 84 | | ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices; |
| | 11017 | 85 | | FiniteSlabRayFrame frame = faces.Frame; |
| | 11017 | 86 | | Fixed64 length = frame.Section.RadialFrame.Length; |
| | 11017 | 87 | | Fixed64 radius = frame.RadiusValue; |
| | 11017 | 88 | | Fixed64 halfThickness = frame.HalfThickness; |
| | 11017 | 89 | | Span<Vector2d> box = stackalloc Vector2d[4]; |
| | 11017 | 90 | | ReadOnlySpan<Vector2d> offsets = !faces.PrismOffsets.IsEmpty ? faces.PrismOffsets.Span |
| | 11017 | 91 | | : CollisionDetectionMixed.GetConvexPrismOffsets(embedded, box); |
| | | 92 | | // Orientation belongs to the ray frame's canonical plane, not authored winding. |
| | 11017 | 93 | | WideDirection3d worldNormal = new WideDirection3d(frame.Plane.AuthoredNormal).Transform(mesh.GeometryFrame.Geome |
| | 11017 | 94 | | WideAxis3 normal = new(Signed320.NarrowValue(worldNormal.X), Signed320.NarrowValue(worldNormal.Y), Signed320.Nar |
| | 11017 | 95 | | _visitedCorners.Clear(); |
| | 11017 | 96 | | MeshSurfaceBoundaryDomain? domain = faces.JoinedBoundary; |
| | 11017 | 97 | | if (domain != null) |
| | | 98 | | { |
| | 96482 | 99 | | for (int span = 0; span < domain.Spans.Length; span++) |
| | | 100 | | { |
| | 38612 | 101 | | var piece = domain.Spans[span]; |
| | 38612 | 102 | | if (faces.IsJoinedSpan(span) || !frame.Section.IntersectsSegment(new FixedSegment(piece.Start, piece.End |
| | 224 | 103 | | int region = MeshSurfaceBoundaryTopology.FindEdgeRegion(mesh, owner, piece.First, piece.Second, |
| | 224 | 104 | | connectivity, frame.Section, faces.PrismOffsets); |
| | 224 | 105 | | int feature = checked(3 * span); |
| | 224 | 106 | | RationalSegmentInterval clip = faces.GetRetainedInterval(ref span); |
| | | 107 | | // An intersected trusted perimeter has an admitted incident triangle. |
| | | 108 | | System.Diagnostics.Debug.Assert(region >= 0); |
| | 224 | 109 | | KeepFeature(piece.First, piece.Second, feature, region, false, offsets, clip); |
| | | 110 | | } |
| | 96378 | 111 | | for (int edge = 0; edge < boundary.Length; edge += 2) |
| | | 112 | | { |
| | 77120 | 113 | | int first = boundary[edge], second = boundary[edge + 1]; |
| | 38560 | 114 | | int region = MeshSurfaceBoundaryTopology.FindEdgeRegion(mesh, owner, first, second, |
| | 38560 | 115 | | connectivity, frame.Section, faces.PrismOffsets); |
| | 38560 | 116 | | if (region < 0) continue; |
| | 4000 | 117 | | int feature = checked(3 * (domain.Spans.Length + edge / 2)); |
| | 4000 | 118 | | if (_visitedCorners.Add(first) && frame.Section.ContainsPoint(vertices[first], offsets)) |
| | 42 | 119 | | KeepFeature(first, second, feature + 1, region, true, offsets); |
| | 4000 | 120 | | if (_visitedCorners.Add(second) && frame.Section.ContainsPoint(vertices[second], offsets)) |
| | 150 | 121 | | KeepFeature(second, first, feature + 2, region, true, offsets); |
| | | 122 | | } |
| | 9629 | 123 | | return; |
| | | 124 | | } |
| | | 125 | | // Joined prism frames always retain their prepared boundary above. |
| | | 126 | | // Isolated Prepare clears this member-owned offset memory. |
| | | 127 | | System.Diagnostics.Debug.Assert(faces.PrismOffsets.IsEmpty); |
| | 14142 | 128 | | for (int edge = 0; edge < boundary.Length; edge += 2) |
| | | 129 | | { |
| | 11366 | 130 | | int first = boundary[edge], second = boundary[edge + 1]; |
| | 5683 | 131 | | int feature = checked(3 * (edge / 2)); |
| | 5683 | 132 | | int region = connectivity.RegionCount == 1 |
| | 5683 | 133 | | ? frame.Section.IntersectsSegment(new FixedSegment(vertices[first], vertices[second]), offsets) ? 0 : -1 |
| | 5683 | 134 | | : MeshSurfaceBoundaryTopology.FindEdgeRegion(mesh, owner, first, second, connectivity, frame.Section, em |
| | 5683 | 135 | | if (region < 0) continue; |
| | 723 | 136 | | KeepFeature(first, second, feature, region, pointOnly: false, offsets); |
| | 723 | 137 | | if (_visitedCorners.Add(first) && frame.Section.ContainsPoint(vertices[first], offsets)) |
| | 22 | 138 | | KeepFeature(first, second, feature + 1, region, pointOnly: true, offsets); |
| | 723 | 139 | | if (_visitedCorners.Add(second) && frame.Section.ContainsPoint(vertices[second], offsets)) |
| | 34 | 140 | | KeepFeature(second, first, feature + 2, region, pointOnly: true, offsets); |
| | | 141 | | } |
| | | 142 | | |
| | | 143 | | void KeepFeature(int first, int second, int ordinal, int region, bool pointOnly, ReadOnlySpan<Vector2d> prismOff |
| | | 144 | | in RationalSegmentInterval clip = default) |
| | | 145 | | { |
| | | 146 | | bool suppress = false; |
| | | 147 | | ReadOnlySpan<WideAxis3> joinedFan = domain != null && pointOnly |
| | | 148 | | ? faces.GetJoinedCornerFan(collider, surfaceTriangle, first, out suppress) : default; |
| | | 149 | | if (suppress) return; |
| | | 150 | | Span<WideAxis3> fan = stackalloc WideAxis3[Math.Max(5, joinedFan.Length + 1)]; |
| | | 151 | | int count; |
| | | 152 | | if (joinedFan.IsEmpty) |
| | | 153 | | { |
| | | 154 | | count = MeshSurfaceBoundaryTopology.GetIncidentFan(mesh, owner, first, second, !pointOnly, fan); |
| | | 155 | | for (int index = 0; index < count; index++) |
| | | 156 | | fan[index] = mesh.GeometryFrame.GetWorldDirectionNumerators(fan[index]); |
| | | 157 | | } |
| | | 158 | | else { joinedFan.CopyTo(fan); count = joinedFan.Length; } |
| | | 159 | | // Topology admits the contact normal p-q. The mathematics owner |
| | | 160 | | // admits the opposite exit ray q-p; negate its halfspaces once. |
| | | 161 | | for (int index = 0; index < count; index++) fan[index] = -fan[index]; |
| | | 162 | | fan[count++] = faces.Orientation > 0 ? -normal : normal; |
| | | 163 | | var segment = new FixedSegment(mesh.GeometryVertices[first], |
| | | 164 | | mesh.GeometryVertices[pointOnly ? first : second]); |
| | | 165 | | if (!SegmentSlabSurfaceContact.TryGetContact(segment, mesh.GeometryFrame, |
| | | 166 | | frame.Center, frame.SlabRotation, length, radius, halfThickness, prismOffsets, fan[..count], |
| | | 167 | | interiorOnly: !pointOnly, clip, out FixedContactAnchors contact, fullHeight: frame.FullHeight != Fixed64 |
| | | 168 | | var group = new ContactGroupKey(namespace3D, namespace2D, surface, 0, checked(-ordinal - 1), |
| | | 169 | | sourceIdA: faces.SourceId, sourceOrderA: faces.SourceOrder); |
| | | 170 | | manifold.AddContact(new ContactAnchor(contact.FirstAnchor), new ContactAnchor(contact.SecondAnchor), |
| | | 171 | | contact.Depth, contact.Normal, collider.Material, targetMaterial, |
| | | 172 | | contact.DepthIsClamped, featureNamespaceA: namespace3D, featureNamespaceB: namespace2D, group: group); |
| | | 173 | | } |
| | 1388 | 174 | | } |
| | | 175 | | } |