| | | 1 | | //======================================================================= |
| | | 2 | | // MeshCapsuleSurfaceBoundary.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 finite radial contacts on a trusted surface's genuine perimeter.</summary> |
| | | 18 | | internal sealed class MeshCapsuleSurfaceBoundary |
| | | 19 | | { |
| | 10191 | 20 | | private readonly SwiftHashSet<int> _visitedCorners = new(); |
| | 10191 | 21 | | private WideAxis3[] _joinedFan = Array.Empty<WideAxis3>(); |
| | 10191 | 22 | | private WideDirection3d[] _transformedFan = Array.Empty<WideDirection3d>(); |
| | | 23 | | internal void BuildSurfaceContacts(LSMeshCollider collider, PhysicsMaterial otherMaterial, |
| | | 24 | | Vector3d center, FixedQuaternion rotation, Fixed64 coreLength, Fixed64 radius, |
| | | 25 | | int surfaceTriangle, MeshSurfaceConnectivity connectivity, MeshCapsuleFaceRegions faces, |
| | | 26 | | ContactManifold manifold) |
| | | 27 | | { |
| | 440 | 28 | | PhysicsMesh mesh = collider.Mesh; |
| | 440 | 29 | | int owner = mesh.GetManifoldSurfaceOwner(surfaceTriangle); |
| | 440 | 30 | | int surface = mesh.GetCanonicalSurfaceOrdinal(surfaceTriangle); |
| | 440 | 31 | | ReadOnlySpan<int> boundary = mesh.GetCanonicalSurfaceBoundaryVertexPairs(surfaceTriangle); |
| | 440 | 32 | | ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices; |
| | 440 | 33 | | CapsulePlaneRayFrame frame = faces.GetBoundaryFrame(collider, surfaceTriangle, out Vector3d normal); |
| | 440 | 34 | | _visitedCorners.Clear(); |
| | 440 | 35 | | MeshSurfaceBoundaryDomain? domain = faces.GetJoinedBoundary(collider, surfaceTriangle); |
| | 440 | 36 | | if (domain != null) |
| | | 37 | | { |
| | 66 | 38 | | ReadOnlySpan<BoundarySpan3D> spans = domain.Spans; |
| | 688 | 39 | | for (int span = 0; span < spans.Length; span++) |
| | | 40 | | { |
| | 278 | 41 | | var piece = spans[span]; |
| | 278 | 42 | | if (!frame.Finite.IntersectsSegment(new FixedSegment(piece.Start, piece.End), piece.Interval)) continue; |
| | 145 | 43 | | int region = connectivity.RegionCount == 1 ? 0 : MeshSurfaceBoundaryTopology.FindEdgeRegion(mesh, |
| | 145 | 44 | | owner, piece.First, piece.Second, connectivity, frame.Finite); |
| | | 45 | | // Exact admission of a certified boundary span implies an |
| | | 46 | | // admitted incident triangle in the same local component. |
| | | 47 | | System.Diagnostics.Debug.Assert(region >= 0); |
| | 145 | 48 | | int ordinal = checked(3 * span); |
| | | 49 | | // Joined interiors disappear, but their endpoints may own a |
| | | 50 | | // genuine outer corner whose other sectors have higher IDs. |
| | 145 | 51 | | if (!faces.IsJoinedSpan(collider, surfaceTriangle, span)) |
| | 81 | 52 | | KeepFeature(piece.First, piece.Second, ordinal, region, false, piece.Interval, span); |
| | 870 | 53 | | for (int end = 0; end < 2; end++) |
| | | 54 | | { |
| | 290 | 55 | | bool upper = end != 0; |
| | 290 | 56 | | Signed832 one = Signed832.ExtendValue(Signed192.One); |
| | 290 | 57 | | Signed832 n = !piece.Interval.IsClipped ? upper ? one : default |
| | 290 | 58 | | : upper ? piece.Interval.WideUpperNumerator : piece.Interval.WideLowerNumerator; |
| | 290 | 59 | | Signed832 d = !piece.Interval.IsClipped ? one |
| | 290 | 60 | | : upper ? piece.Interval.WideUpperDenominator : piece.Interval.WideLowerDenominator; |
| | | 61 | | // Authored corners share vertex identity; consecutive pieces |
| | | 62 | | // of one canonical edge share their exact cut endpoint. |
| | 290 | 63 | | int vertex = n.IsZero ? piece.First : n.Equals(d) ? piece.Second : -1; |
| | 290 | 64 | | if (vertex >= 0) |
| | 316 | 65 | | { if (!_visitedCorners.Add(vertex)) continue; } |
| | | 66 | | // Every interior lower cut is the preceding span's upper |
| | | 67 | | // cut. Closed admission visits that shared point there. |
| | 36 | 68 | | else if (!upper) continue; |
| | 206 | 69 | | var point = new RationalSegmentInterval(n, d, n, d); |
| | 206 | 70 | | KeepFeature(piece.First, piece.Second, ordinal + end + 1, region, true, point, span, upper); |
| | | 71 | | } |
| | | 72 | | } |
| | 66 | 73 | | return; |
| | | 74 | | } |
| | 13144 | 75 | | for (int edge = 0; edge < boundary.Length; edge += 2) |
| | | 76 | | { |
| | 12396 | 77 | | int first = boundary[edge], second = boundary[edge + 1]; |
| | 6198 | 78 | | int feature = checked(3 * (edge / 2)); |
| | | 79 | | // A straight edge's admitted interval needs no subedge routing |
| | | 80 | | // when the surface has just one connected finite region. |
| | 6198 | 81 | | int region = connectivity.RegionCount == 1 |
| | 6198 | 82 | | ? frame.Finite.IntersectsSegment(new FixedSegment(vertices[first], vertices[second])) ? 0 : -1 |
| | 6198 | 83 | | : MeshSurfaceBoundaryTopology.FindEdgeRegion(mesh, owner, first, second, |
| | 6198 | 84 | | connectivity, frame.Finite); |
| | 6198 | 85 | | if (region < 0) continue; |
| | 4514 | 86 | | KeepFeature(first, second, feature, region, pointOnly: false); |
| | 4514 | 87 | | if (_visitedCorners.Add(first) && frame.Finite.ContainsPoint(vertices[first])) |
| | 1487 | 88 | | KeepFeature(first, second, feature + 1, region, pointOnly: true); |
| | 4514 | 89 | | if (_visitedCorners.Add(second) && frame.Finite.ContainsPoint(vertices[second])) |
| | 1478 | 90 | | KeepFeature(second, first, feature + 2, region, pointOnly: true); |
| | | 91 | | } |
| | | 92 | | |
| | | 93 | | void KeepFeature(int first, int second, int ordinal, int region, bool pointOnly, |
| | | 94 | | in RationalSegmentInterval clip = default, int span = -1, bool upper = false) |
| | | 95 | | { |
| | | 96 | | int other = pointOnly && domain == null ? first : second; |
| | | 97 | | var triangle = new FixedTriangle(mesh.GeometryVertices[first], |
| | | 98 | | mesh.GeometryVertices[other], mesh.GeometryVertices[other]); |
| | | 99 | | int orientation = faces.GetBoundaryOrientation(collider, surfaceTriangle, region); |
| | | 100 | | Vector3d fallback = -normal * orientation; |
| | | 101 | | int endpoint = 0; |
| | | 102 | | FixedContactAnchors contact; |
| | | 103 | | AxisTriangleContactFeature feature; |
| | | 104 | | bool localChart; |
| | | 105 | | bool found = domain == null |
| | | 106 | | ? triangle.TryGetCenteredCapsuleContact(mesh.GeometryFrame, center, rotation, |
| | | 107 | | coreLength, radius, fallback, out contact, out feature, out localChart) |
| | | 108 | | : triangle.TryGetClippedCenteredCapsuleContact(mesh.GeometryFrame, center, rotation, |
| | | 109 | | coreLength, radius, fallback, clip, out contact, out feature, out endpoint, out localChart); |
| | | 110 | | if (!found || !pointOnly && (endpoint != 0 || feature.TriangleVertex >= 0)) return; |
| | | 111 | | bool suppress = false; |
| | | 112 | | ReadOnlySpan<WideAxis3> joinedFan = domain != null && pointOnly |
| | | 113 | | ? faces.GetJoinedEndpointFan(collider, surfaceTriangle, span, upper, out suppress) : default; |
| | | 114 | | if (suppress) return; |
| | | 115 | | if (joinedFan.Length + 1 > _joinedFan.Length && !joinedFan.IsEmpty) |
| | | 116 | | Array.Resize(ref _joinedFan, Math.Max(joinedFan.Length + 1, 2 * _joinedFan.Length)); |
| | | 117 | | Span<WideAxis3> fan = joinedFan.IsEmpty ? stackalloc WideAxis3[5] : _joinedFan; |
| | | 118 | | int count; |
| | | 119 | | if (joinedFan.IsEmpty) |
| | | 120 | | { |
| | | 121 | | bool interior = !pointOnly || domain != null && !clip.WideLowerNumerator.IsZero |
| | | 122 | | && !clip.WideLowerNumerator.Equals(clip.WideLowerDenominator); |
| | | 123 | | bool reversed = domain != null && pointOnly && clip.WideLowerNumerator.Equals(clip.WideLowerDenominator) |
| | | 124 | | count = MeshSurfaceBoundaryTopology.GetIncidentFan(mesh, owner, |
| | | 125 | | reversed ? second : first, reversed ? first : second, interior, fan); |
| | | 126 | | } |
| | | 127 | | else { joinedFan.CopyTo(fan); count = joinedFan.Length; } |
| | | 128 | | // Joined rays have world orientation. Preserve the ordinary local |
| | | 129 | | // chart when possible; otherwise map both exact separation and |
| | | 130 | | // native incident rays into the same world chart before testing. |
| | | 131 | | bool worldChart = !localChart || !joinedFan.IsEmpty; |
| | | 132 | | WideAxis3 canonicalNormal = frame.AuthoredNormal; |
| | | 133 | | if (worldChart) |
| | | 134 | | { |
| | | 135 | | if (localChart) feature = feature.GetWorldFeatureFromRigidLocal(mesh.GeometryFrame.Rotation); |
| | | 136 | | if (joinedFan.IsEmpty) |
| | | 137 | | for (int index = 0; index < count; index++) |
| | | 138 | | fan[index] = mesh.GeometryFrame.GetWorldDirectionNumerators(fan[index]); |
| | | 139 | | WideDirection3d worldNormal = new WideDirection3d(frame.AuthoredNormal).Transform(mesh.GeometryFrame.Geo |
| | | 140 | | canonicalNormal = new WideAxis3(Signed320.NarrowValue(worldNormal.X), |
| | | 141 | | Signed320.NarrowValue(worldNormal.Y), Signed320.NarrowValue(worldNormal.Z)); |
| | | 142 | | } |
| | | 143 | | if (!SupportsFan(feature, fan[..count], worldChart) |
| | | 144 | | || !SupportsSelectedHemisphere(feature, canonicalNormal, orientation, worldChart)) return; |
| | | 145 | | fan[count++] = orientation > 0 ? canonicalNormal : -canonicalNormal; |
| | | 146 | | if (coreLength > Fixed64.Zero && feature.IsZeroSeparation |
| | | 147 | | && TryGetPerpendicularFallback(frame.Finite, rotation, fan[..count], worldChart, out fallback)) |
| | | 148 | | { |
| | | 149 | | // Select a perpendicular direction in the exact capsule chart, |
| | | 150 | | // then construct the actual ray using its final Q32.32 normal. |
| | | 151 | | // The inherited closest query can quantize its core direction; |
| | | 152 | | // do not infer the final ray depth from the ideal radius alone. |
| | | 153 | | found = domain == null |
| | | 154 | | ? triangle.TryGetCenteredCapsuleContact(mesh.GeometryFrame, |
| | | 155 | | center, rotation, coreLength, radius, fallback, out contact, out _, out _) |
| | | 156 | | : triangle.TryGetClippedCenteredCapsuleContact(mesh.GeometryFrame, |
| | | 157 | | center, rotation, coreLength, radius, fallback, clip, out contact, out _, out _, out _); |
| | | 158 | | System.Diagnostics.Debug.Assert(found); |
| | | 159 | | } |
| | | 160 | | var group = new ContactGroupKey(0, 0, surface, 0, checked(-ordinal - 1)); |
| | | 161 | | manifold.AddContact(new ContactAnchor(contact.FirstAnchor), new ContactAnchor(contact.SecondAnchor), |
| | | 162 | | contact.Depth, contact.Normal, collider.Material, otherMaterial, contact.DepthIsClamped, group: group); |
| | | 163 | | } |
| | 374 | 164 | | } |
| | | 165 | | |
| | | 166 | | private static bool SupportsFan(in AxisTriangleContactFeature feature, ReadOnlySpan<WideAxis3> fan, bool wide) |
| | | 167 | | { |
| | 17641 | 168 | | foreach (WideAxis3 inward in fan) |
| | | 169 | | { |
| | 6330 | 170 | | if ((wide ? feature.CompareSeparationProjection(inward) |
| | 6330 | 171 | | : feature.CompareSeparationProjection(Signed192.NarrowProven(inward.X), |
| | 8129 | 172 | | Signed192.NarrowProven(inward.Y), Signed192.NarrowProven(inward.Z))) > 0) return false; |
| | | 173 | | } |
| | 1591 | 174 | | return true; |
| | | 175 | | } |
| | | 176 | | |
| | | 177 | | private static bool SupportsSelectedHemisphere(in AxisTriangleContactFeature feature, |
| | | 178 | | in WideAxis3 canonicalNormal, int orientation, bool wide) |
| | | 179 | | { |
| | 1591 | 180 | | return orientation * (wide ? feature.CompareSeparationProjection(canonicalNormal) : feature.CompareSeparationPro |
| | 1591 | 181 | | Signed192.NarrowProven(canonicalNormal.X), Signed192.NarrowProven(canonicalNormal.Y), |
| | 1591 | 182 | | Signed192.NarrowProven(canonicalNormal.Z))) <= 0; |
| | | 183 | | } |
| | | 184 | | |
| | | 185 | | private bool TryGetPerpendicularFallback(in CapsuleFiniteSectionFrame finite, |
| | | 186 | | FixedQuaternion rotation, ReadOnlySpan<WideAxis3> authoredFan, bool worldChart, out Vector3d fallback) |
| | | 187 | | { |
| | 36 | 188 | | fallback = default; |
| | 36 | 189 | | if (authoredFan.Length > 5 && _transformedFan.Length < authoredFan.Length) |
| | 3 | 190 | | Array.Resize(ref _transformedFan, Math.Max(authoredFan.Length, 2 * _transformedFan.Length)); |
| | 36 | 191 | | Span<WideDirection3d> fan = authoredFan.Length <= 5 ? stackalloc WideDirection3d[5] : _transformedFan; |
| | 36 | 192 | | WideRationalBasis3d basis = worldChart ? WideRationalBasis3d.CreateRelative(new WideRationalBasis3d(rotation), n |
| | | 193 | | // Authored rays need the relative source basis; joined rays already |
| | | 194 | | // have world orientation and need only the capsule's inverse basis. |
| | | 195 | | // Complete transformed rays fit Signed576, including affine scale. |
| | 374 | 196 | | for (int index = 0; index < authoredFan.Length; index++) |
| | 151 | 197 | | fan[index] = new WideDirection3d(authoredFan[index]).Transform(basis); |
| | 36 | 198 | | ReadOnlySpan<WideDirection3d> constraints = fan[..authoredFan.Length]; |
| | 36 | 199 | | FixedQuaternion targetRotation = rotation; |
| | 36 | 200 | | WideDirection3d selected = -constraints[^1]; |
| | 42 | 201 | | if (KeepDirection(new WideDirection3d(selected.X, default, selected.Z), constraints, targetRotation, out fallbac |
| | 136 | 202 | | foreach (WideDirection3d wall in constraints) |
| | | 203 | | { |
| | 51 | 204 | | WideDirection3d direction = new(wall.Z, default, WideArithmetic.SubtractSigned576(default, wall.X)); |
| | 51 | 205 | | if (KeepDirection(direction, constraints, targetRotation, out fallback) |
| | 77 | 206 | | || KeepDirection(-direction, constraints, targetRotation, out fallback)) return true; |
| | | 207 | | } |
| | | 208 | | // A genuine perimeter fan has two noncollinear rays, so its circle |
| | | 209 | | // constraints cannot all vanish. Every nonempty clipped circle has |
| | | 210 | | // an active wall already represented by the signed crosses above. |
| | 4 | 211 | | return false; |
| | | 212 | | } |
| | | 213 | | |
| | | 214 | | private static bool KeepDirection(WideDirection3d direction, ReadOnlySpan<WideDirection3d> fan, |
| | | 215 | | FixedQuaternion rotation, out Vector3d result) |
| | | 216 | | { |
| | 127 | 217 | | result = default; |
| | 151 | 218 | | if (direction.IsZero) return false; |
| | 717 | 219 | | foreach (WideDirection3d inward in fan) |
| | 362 | 220 | | if (WideDirection3d.Dot(direction, inward).Sign > 0) return false; |
| | | 221 | | // Local-Up projection preserves the complete fan width. The final |
| | | 222 | | // normalized rigid transform still fits Signed576, including the |
| | | 223 | | // exact world rays supplied by differently scaled joined members. |
| | 32 | 224 | | result = direction.Transform(new WideRationalBasis3d(rotation)).GetNormal(false); |
| | 32 | 225 | | return true; |
| | | 226 | | } |
| | | 227 | | |
| | | 228 | | } |