| | | 1 | | //======================================================================= |
| | | 2 | | // MeshSurfaceBoundaryDomain.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 SwiftCollections; |
| | | 13 | | using SwiftCollections.Query; |
| | | 14 | | |
| | | 15 | | namespace Gravitas.CollisionHandling; |
| | | 16 | | |
| | | 17 | | /// <summary> |
| | | 18 | | /// Classifies one surface's canonical perimeter against a bounded set of |
| | | 19 | | /// committed members, without materializing or merging their world vertices. |
| | | 20 | | /// This local span certificate does not establish endpoint fans or global |
| | | 21 | | /// embedding; ambiguous coverage retains the original boundary. |
| | | 22 | | /// </summary> |
| | | 23 | | internal sealed partial class MeshSurfaceBoundaryDomain |
| | | 24 | | { |
| | 3021 | 25 | | private readonly SwiftList<BoundarySpan3D> _spans = new(); |
| | 3021 | 26 | | private readonly SwiftList<Coverage> _coverage = new(); |
| | 3021 | 27 | | private readonly SwiftList<Parameter> _cuts = new(); |
| | 3021 | 28 | | private readonly SwiftList<int> _triangles = new(); |
| | 6042 | 29 | | private readonly SwiftHashSet<(int Member, int Surface)> _own = new(), _opposite = new(); |
| | | 30 | | |
| | 934952 | 31 | | internal ReadOnlySpan<BoundarySpan3D> Spans => _spans.AsReadOnlySpan(); |
| | | 32 | | internal int SourceMember { get; private set; } |
| | | 33 | | internal int SourceSurface { get; private set; } |
| | | 34 | | internal Vector3d Origin { get; private set; } |
| | | 35 | | internal FixedQuaternion Rotation { get; private set; } |
| | | 36 | | |
| | | 37 | | /// <summary>Replaces prepared spans; member indices are caller-owned stable identities.</summary> |
| | | 38 | | internal void Build(ReadOnlySpan<PhysicsMesh> members, int sourceMember, int sourceTriangle) |
| | | 39 | | { |
| | 1566 | 40 | | PhysicsMesh source = members[sourceMember]; |
| | 1566 | 41 | | int owner = source.GetManifoldSurfaceOwner(sourceTriangle); |
| | 3132 | 42 | | SourceMember = sourceMember; SourceSurface = owner; |
| | 3132 | 43 | | Origin = source.Origin; Rotation = source.Rotation; |
| | 1566 | 44 | | ReadOnlySpan<int> edges = source.GetCanonicalSurfaceBoundaryVertexPairs(owner); |
| | 1566 | 45 | | ReadOnlySpan<Vector3d> vertices = source.GeometryVertices; |
| | 1566 | 46 | | Span<WideAxis3> fan = stackalloc WideAxis3[3]; |
| | 1566 | 47 | | _spans.FastClear(); |
| | 15672 | 48 | | for (int edge = 0; edge < edges.Length; edge += 2) |
| | | 49 | | { |
| | 12540 | 50 | | int first = edges[edge], second = edges[edge + 1]; |
| | 6270 | 51 | | MeshSurfaceBoundaryTopology.GetIncidentFan(source, owner, first, second, true, fan); |
| | 6270 | 52 | | Vector3d start = vertices[first]; |
| | | 53 | | // The unnormalized fan ray is raw(inward) - raw(start), so this |
| | | 54 | | // wide sum recovers the original representable vertex exactly. |
| | 6270 | 55 | | var inward = new Vector3d(RecoverCoordinate(start.X, fan[2].X), |
| | 6270 | 56 | | RecoverCoordinate(start.Y, fan[2].Y), RecoverCoordinate(start.Z, fan[2].Z)); |
| | 6270 | 57 | | var face = new FixedTriangle(start, vertices[second], inward); |
| | 6270 | 58 | | Gather(members, sourceMember, owner, face); |
| | 6270 | 59 | | Classify(sourceMember, owner, first, second, face.A, face.B); |
| | | 60 | | } |
| | 1566 | 61 | | } |
| | | 62 | | |
| | | 63 | | private static Fixed64 RecoverCoordinate(Fixed64 start, Signed320 ray) => |
| | 18810 | 64 | | Fixed64.GetSignedRawRatio(WideArithmetic.AddSigned320(Signed320.ExtendValue(Signed192.Raw(start)), ray), |
| | 18810 | 65 | | Signed192.Signed(1)); |
| | | 66 | | |
| | | 67 | | private void Gather(ReadOnlySpan<PhysicsMesh> members, int sourceMember, int owner, in FixedTriangle face) |
| | | 68 | | { |
| | 12540 | 69 | | _coverage.FastClear(); _cuts.FastClear(); |
| | 12540 | 70 | | _cuts.Add(Parameter.Zero); _cuts.Add(Parameter.One); |
| | 6270 | 71 | | PhysicsMesh source = members[sourceMember]; |
| | 6270 | 72 | | FixedBoundBox edgeBounds = new FixedSegment(face.A, face.B).Bounds; |
| | 46834 | 73 | | for (int member = 0; member < members.Length; member++) |
| | | 74 | | { |
| | 17147 | 75 | | PhysicsMesh target = members[member]; |
| | | 76 | | // Use the existing local BVH, including when world endpoints cannot |
| | | 77 | | // be materialized. Exact coverage still decides every candidate. |
| | 17147 | 78 | | FixedBoundBox worldBounds = source.GeometryFrame.GetWorldBounds(edgeBounds.Min, edgeBounds.Max); |
| | 17147 | 79 | | target.GetTrianglesInWorldBounds(new FixedBoundVolume(worldBounds.Min, worldBounds.Max), _triangles); |
| | 84666 | 80 | | foreach (int triangle in _triangles.AsReadOnlySpan()) |
| | | 81 | | { |
| | 25186 | 82 | | int surface = target.GetManifoldSurfaceOwner(triangle); |
| | 25186 | 83 | | if (member == sourceMember && surface == owner) continue; |
| | 10538 | 84 | | target.GetGeometryTriangleVertices(triangle, out Vector3d a, out Vector3d b, out Vector3d c); |
| | | 85 | | RationalSegmentInterval interval; |
| | | 86 | | int sides; |
| | 10538 | 87 | | if (source.GeometryFrame.IsRigid && target.GeometryFrame.IsRigid) |
| | | 88 | | { |
| | 10156 | 89 | | if (!WideTriangleRelations.TryGetEdgeCoverage(face, source.Origin, source.Rotation, |
| | 12194 | 90 | | new FixedTriangle(a, b, c), target.Origin, target.Rotation, out interval, out sides)) continue; |
| | | 91 | | } |
| | | 92 | | else |
| | | 93 | | { |
| | 382 | 94 | | if (!WideTriangleRelations.TryGetEdgeCoverage(face, source.GeometryFrame, |
| | 382 | 95 | | new FixedTriangle(a, b, c), target.GeometryFrame, |
| | 382 | 96 | | out Signed832 lowerN, out Signed832 lowerD, out Signed832 upperN, out Signed832 upperD, |
| | 382 | 97 | | out sides)) continue; |
| | 285 | 98 | | interval = new RationalSegmentInterval(lowerN, lowerD, upperN, upperD); |
| | | 99 | | } |
| | 16806 | 100 | | var lower = Parameter.Lower(interval); var upper = Parameter.Upper(interval); |
| | | 101 | | // Closed point contacts do not fill an open half-neighborhood. |
| | 8403 | 102 | | if (lower.CompareTo(upper) == 0) continue; |
| | 1998 | 103 | | _coverage.Add(new Coverage(member, surface, sides, lower, upper)); |
| | 3996 | 104 | | _cuts.Add(lower); _cuts.Add(upper); |
| | | 105 | | } |
| | | 106 | | } |
| | 6270 | 107 | | _cuts.SortInPlace(); |
| | 6270 | 108 | | } |
| | | 109 | | |
| | | 110 | | private void Classify(int sourceMember, int owner, int first, int second, Vector3d start, Vector3d end) |
| | | 111 | | { |
| | 6270 | 112 | | int edgeStart = _spans.Count; |
| | 6270 | 113 | | Parameter lower = _cuts[0]; |
| | 33072 | 114 | | for (int cut = 1; cut < _cuts.Count; cut++) |
| | | 115 | | { |
| | 10266 | 116 | | Parameter upper = _cuts[cut]; |
| | 10266 | 117 | | if (lower.CompareTo(upper) == 0) continue; |
| | 6758 | 118 | | SpanKind kind = ClassifyInterval(lower, upper, sourceMember, owner, out int otherMember, out int otherSurfac |
| | | 119 | | // Avoid artificial event endpoints at irrelevant triangle cuts. |
| | 6758 | 120 | | if (_spans.Count > edgeStart) |
| | | 121 | | { |
| | 488 | 122 | | BoundarySpan3D previous = _spans[_spans.Count - 1]; |
| | 488 | 123 | | if (previous.Kind == kind && previous.OtherMember == otherMember && previous.OtherSurface == otherSurfac |
| | | 124 | | { |
| | 148 | 125 | | var merged = new RationalSegmentInterval(Parameter.Lower(previous.Interval).N, |
| | 148 | 126 | | Parameter.Lower(previous.Interval).D, upper.N, upper.D); |
| | 148 | 127 | | _spans[_spans.Count - 1] = new BoundarySpan3D(first, second, start, end, merged, kind, otherMember, |
| | 148 | 128 | | lower = upper; |
| | 148 | 129 | | continue; |
| | | 130 | | } |
| | | 131 | | } |
| | 6610 | 132 | | var interval = new RationalSegmentInterval(lower.N, lower.D, upper.N, upper.D); |
| | 6610 | 133 | | _spans.Add(new BoundarySpan3D(first, second, start, end, interval, kind, otherMember, otherSurface)); |
| | 6610 | 134 | | lower = upper; |
| | | 135 | | } |
| | 6270 | 136 | | } |
| | | 137 | | |
| | | 138 | | private SpanKind ClassifyInterval(Parameter lower, Parameter upper, int sourceMember, int owner, |
| | | 139 | | out int otherMember, out int otherSurface) |
| | | 140 | | { |
| | 20274 | 141 | | _own.Clear(); _opposite.Clear(); _own.Add((sourceMember, owner)); |
| | 13516 | 142 | | otherMember = -1; otherSurface = -1; |
| | 6758 | 143 | | bool folded = false; |
| | 26290 | 144 | | foreach (Coverage coverage in _coverage.AsReadOnlySpan()) |
| | | 145 | | { |
| | 6387 | 146 | | if (coverage.Lower.CompareTo(lower) > 0 || coverage.Upper.CompareTo(upper) < 0) continue; |
| | 2015 | 147 | | folded |= (coverage.Sides & 4) != 0; |
| | | 148 | | // Trusted patches partition their interiors. Count a patch's |
| | | 149 | | // filled side once even along authored internal diagonals. |
| | 2171 | 150 | | if ((coverage.Sides & 1) != 0) _own.Add((coverage.Member, coverage.Surface)); |
| | 2015 | 151 | | if ((coverage.Sides & 2) == 0) continue; |
| | 1795 | 152 | | _opposite.Add((coverage.Member, coverage.Surface)); |
| | 3590 | 153 | | otherMember = coverage.Member; otherSurface = coverage.Surface; |
| | | 154 | | } |
| | 8481 | 155 | | if (!folded && _own.Count == 1 && _opposite.Count == 1 && otherMember != sourceMember) return SpanKind.Joined; |
| | 10070 | 156 | | otherMember = -1; otherSurface = -1; |
| | 5035 | 157 | | return !folded && _own.Count == 1 && _opposite.Count == 0 ? SpanKind.Exposed : SpanKind.Ambiguous; |
| | | 158 | | } |
| | | 159 | | |
| | | 160 | | private readonly struct Coverage |
| | | 161 | | { |
| | | 162 | | internal readonly int Member, Surface, Sides; |
| | | 163 | | internal readonly Parameter Lower, Upper; |
| | | 164 | | internal Coverage(int member, int surface, int sides, Parameter lower, Parameter upper) |
| | 11988 | 165 | | { Member = member; Surface = surface; Sides = sides; Lower = lower; Upper = upper; } |
| | | 166 | | } |
| | | 167 | | |
| | | 168 | | private readonly struct Parameter : IComparable<Parameter> |
| | | 169 | | { |
| | | 170 | | internal readonly Signed832 N, D; |
| | 7915 | 171 | | internal static Parameter Zero => new(default, Signed832.ExtendValue(Signed192.One)); |
| | 7915 | 172 | | internal static Parameter One => new(Signed832.ExtendValue(Signed192.One), Signed832.ExtendValue(Signed192.One)) |
| | 88926 | 173 | | private Parameter(Signed832 n, Signed832 d) { N = n; D = d; } |
| | 8699 | 174 | | internal static Parameter Lower(in RationalSegmentInterval interval) => interval.IsClipped |
| | 8699 | 175 | | ? new(interval.WideLowerNumerator, interval.WideLowerDenominator) : Zero; |
| | 8403 | 176 | | internal static Parameter Upper(in RationalSegmentInterval interval) => interval.IsClipped |
| | 8403 | 177 | | ? new(interval.WideUpperNumerator, interval.WideUpperDenominator) : One; |
| | 43488 | 178 | | public int CompareTo(Parameter other) => WideTriangleRelations.CompareCoreParameters(N, D, other.N, other.D); |
| | | 179 | | } |
| | | 180 | | } |