| | | 1 | | //======================================================================= |
| | | 2 | | // MeshSurfaceBoundaryDomain2D.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 | | |
| | | 14 | | namespace Gravitas.CollisionHandling; |
| | | 15 | | |
| | | 16 | | /// <summary> |
| | | 17 | | /// Classifies original convex perimeters against explicitly supplied committed |
| | | 18 | | /// members, preserving exact span provenance and ambiguous boundaries. The |
| | | 19 | | /// dimensional surface owner manages declarations, endpoint fans and response. |
| | | 20 | | /// </summary> |
| | | 21 | | internal sealed partial class MeshSurfaceBoundaryDomain2D |
| | | 22 | | { |
| | 1622 | 23 | | private readonly SwiftList<BoundarySpan2D> _spans = new(); |
| | 1622 | 24 | | private readonly SwiftList<FixedBoundArea> _vertexBounds = new(); |
| | 1622 | 25 | | private readonly SwiftList<Coverage> _coverage = new(); |
| | 1622 | 26 | | private readonly SwiftList<Parameter> _cuts = new(); |
| | 3244 | 27 | | private readonly SwiftHashSet<int> _own = new(), _opposite = new(); |
| | | 28 | | |
| | | 29 | | |
| | | 30 | | |
| | 180471 | 31 | | internal ReadOnlySpan<BoundarySpan2D> Spans => _spans.AsReadOnlySpan(); |
| | 13100 | 32 | | internal ReadOnlySpan<FixedBoundArea> VertexBounds => _vertexBounds.AsReadOnlySpan(); |
| | | 33 | | internal Vector2d Origin { get; private set; } |
| | | 34 | | internal Fixed64 Rotation { get; private set; } |
| | | 35 | | |
| | | 36 | | internal void Build(ReadOnlySpan<LSCollider2D> members, int sourceMember, bool mixed = false, |
| | | 37 | | Fixed64? lowerHeight = null, Fixed64? upperHeight = null) |
| | | 38 | | { |
| | 20891 | 39 | | foreach (LSCollider2D member in members) |
| | 7988 | 40 | | if (member is not (LSAABBoxCollider2D or LSPolygonCollider2D)) |
| | 1 | 41 | | throw new ArgumentException("Boundary preparation requires committed AABB or convex polygon members.", n |
| | 2457 | 42 | | LSCollider2D source = members[sourceMember]; |
| | 4914 | 43 | | Origin = source.Center; Rotation = source.ConvexRotation; |
| | 2457 | 44 | | _spans.FastClear(); |
| | 2457 | 45 | | _vertexBounds.FastClear(); |
| | 2457 | 46 | | Span<Vector2d> points = stackalloc Vector2d[2]; |
| | 24446 | 47 | | for (int first = 0; first < source.VertexCount; first++) |
| | | 48 | | { |
| | 9766 | 49 | | int second = (first + 1) % source.VertexCount; |
| | 19532 | 50 | | Vector2d start = source.GetScaledLocalVertexUnchecked(first), end = source.GetScaledLocalVertexUnchecked(sec |
| | 9766 | 51 | | points[0] = start; |
| | 9766 | 52 | | _vertexBounds.Add(WideConvex2dRelations.GetBoundsClippedToDomain(Origin, Rotation, points.Slice(0, 1))); |
| | 9766 | 53 | | points[1] = end; |
| | 9766 | 54 | | FixedBoundArea bounds = WideConvex2dRelations.GetBoundsClippedToDomain(Origin, Rotation, points); |
| | 9766 | 55 | | Vector2d inward = source.GetScaledLocalVertexUnchecked((first + 2) % source.VertexCount); |
| | 9766 | 56 | | Gather(members, sourceMember, new FixedSegment2d(start, end), inward, bounds, mixed, lowerHeight, upperHeigh |
| | 9766 | 57 | | Classify(sourceMember, first, second, start, end, bounds); |
| | | 58 | | } |
| | 2457 | 59 | | } |
| | | 60 | | |
| | | 61 | | private void Gather(ReadOnlySpan<LSCollider2D> members, int sourceMember, FixedSegment2d edge, |
| | | 62 | | Vector2d inward, FixedBoundArea bounds, bool mixed, Fixed64? lowerHeight, Fixed64? upperHeight) |
| | | 63 | | { |
| | 19532 | 64 | | _coverage.FastClear(); _cuts.FastClear(); |
| | 19532 | 65 | | _cuts.Add(Parameter.Zero); _cuts.Add(Parameter.One); |
| | 9766 | 66 | | LSCollider2D source = members[sourceMember]; |
| | 9766 | 67 | | Span<Vector2d> box = stackalloc Vector2d[4]; |
| | 82940 | 68 | | for (int member = 0; member < members.Length; member++) |
| | | 69 | | { |
| | 31704 | 70 | | if (member == sourceMember) continue; |
| | 21938 | 71 | | LSCollider2D target = members[member]; |
| | | 72 | | // Zero yaw shares the same exact chart in both paths. Nonzero mixed |
| | | 73 | | // quaternion coverage cannot use scalar-yaw bounds as a rejection proof. |
| | 21938 | 74 | | if ((!mixed || source.ConvexRotation == Fixed64.Zero && target.ConvexRotation == Fixed64.Zero) |
| | 21938 | 75 | | && !bounds.Intersects(target.Bounds)) continue; |
| | | 76 | | // Equal extrusions allow a planar proof to cover their entire |
| | | 77 | | // height. Different slabs retain their real step/side boundaries. |
| | 9459 | 78 | | if (lowerHeight.HasValue) |
| | | 79 | | { |
| | 8898 | 80 | | if (source.MixedBounds3D.Max.Y <= target.MixedBounds3D.Min.Y |
| | 8898 | 81 | | || source.MixedBounds3D.Min.Y >= target.MixedBounds3D.Max.Y) continue; |
| | 8708 | 82 | | Fixed64 delta = target.MixedSlabCenterY - source.MixedSlabCenterY; |
| | 8708 | 83 | | if (delta + target.MixedHalfThickness <= lowerHeight.Value |
| | 9521 | 84 | | || delta - target.MixedHalfThickness >= upperHeight!.Value) continue; |
| | | 85 | | } |
| | 561 | 86 | | else if (mixed && (source.MixedSlabCenterY != target.MixedSlabCenterY |
| | 561 | 87 | | || source.MixedHalfThickness != target.MixedHalfThickness)) continue; |
| | 7568 | 88 | | ReadOnlySpan<Vector2d> vertices = Offsets(target, box); |
| | 7568 | 89 | | if (!mixed) |
| | | 90 | | { |
| | 6398 | 91 | | if (WideConvex2dRelations.TryGetEdgeCoverage(source.Center, source.ConvexRotation, edge, inward, |
| | 6398 | 92 | | target.Center, target.ConvexRotation, vertices, out RationalSegmentInterval interval, out int sides) |
| | 6378 | 93 | | Include(member, interval, sides); |
| | 6378 | 94 | | continue; |
| | | 95 | | } |
| | | 96 | | // Mixed prisms use a normalized quaternion chart, whereas pure |
| | | 97 | | // 2D relations use scalar Sin/Cos. Reuse the actual prism frame; |
| | | 98 | | // a transient convex fan does not merge or round world geometry. |
| | 1170 | 99 | | var face = new FixedTriangle(Embed(edge.Start), Embed(edge.End), Embed(inward)); |
| | 1170 | 100 | | FixedQuaternion sourceRotation = FixedQuaternion.FromAxisAngle(Vector3d.Up, -source.ConvexRotation); |
| | 1170 | 101 | | FixedQuaternion targetRotation = FixedQuaternion.FromAxisAngle(Vector3d.Up, -target.ConvexRotation); |
| | 6718 | 102 | | for (int triangle = 1; triangle < vertices.Length - 1; triangle++) |
| | | 103 | | // Height bands classify actual extrusion overlap separately; |
| | | 104 | | // footprint incidence uses one common Y plane even for offset slabs. |
| | 2189 | 105 | | if (WideTriangleRelations.TryGetEdgeCoverage(face, new Vector3d(source.Center.X, Fixed64.Zero, source.Ce |
| | 2189 | 106 | | new FixedTriangle(Embed(vertices[0]), Embed(vertices[triangle]), Embed(vertices[triangle + 1])), |
| | 2189 | 107 | | new Vector3d(target.Center.X, Fixed64.Zero, target.Center.Y), targetRotation, out RationalSegmentInt |
| | 1751 | 108 | | Include(member, interval, sides); |
| | | 109 | | } |
| | 9766 | 110 | | _cuts.SortInPlace(); |
| | 9766 | 111 | | } |
| | | 112 | | |
| | | 113 | | private static ReadOnlySpan<Vector2d> Offsets(LSCollider2D collider, Span<Vector2d> box) |
| | | 114 | | { |
| | 8848 | 115 | | if (collider is LSPolygonCollider2D polygon) return polygon.ScaledLocalVertices; |
| | 88032 | 116 | | for (int vertex = 0; vertex < 4; vertex++) box[vertex] = collider.GetScaledLocalVertexUnchecked(vertex); |
| | 6288 | 117 | | return box; |
| | | 118 | | } |
| | | 119 | | |
| | 10077 | 120 | | private static Vector3d Embed(Vector2d point) => new(point.X, Fixed64.Zero, point.Y); |
| | | 121 | | |
| | | 122 | | private void Include(int member, in RationalSegmentInterval interval, int sides) |
| | | 123 | | { |
| | 16258 | 124 | | var lower = Parameter.Lower(interval); var upper = Parameter.Upper(interval); |
| | | 125 | | // A shared endpoint does not fill either open half-neighborhood. |
| | 13747 | 126 | | if (lower.CompareTo(upper) == 0) return; |
| | 2511 | 127 | | _coverage.Add(new Coverage(member, sides, lower, upper)); |
| | 5022 | 128 | | _cuts.Add(lower); _cuts.Add(upper); |
| | 2511 | 129 | | } |
| | | 130 | | |
| | | 131 | | private void Classify(int sourceMember, int first, int second, Vector2d start, Vector2d end, FixedBoundArea bounds) |
| | | 132 | | { |
| | 9766 | 133 | | int edgeStart = _spans.Count; |
| | 9766 | 134 | | Parameter lower = _cuts[0]; |
| | 49108 | 135 | | for (int cut = 1; cut < _cuts.Count; cut++) |
| | | 136 | | { |
| | 14788 | 137 | | Parameter upper = _cuts[cut]; |
| | 14788 | 138 | | if (lower.CompareTo(upper) == 0) continue; |
| | 10412 | 139 | | SpanKind kind = ClassifyInterval(lower, upper, sourceMember, out int otherMember); |
| | 10412 | 140 | | if (_spans.Count > edgeStart) |
| | | 141 | | { |
| | 646 | 142 | | BoundarySpan2D previous = _spans[_spans.Count - 1]; |
| | 646 | 143 | | if (previous.Kind == kind && previous.OtherMember == otherMember) |
| | | 144 | | { |
| | 23 | 145 | | var merged = new RationalSegmentInterval(Parameter.Lower(previous.Interval).N, |
| | 23 | 146 | | Parameter.Lower(previous.Interval).D, upper.N, upper.D); |
| | 23 | 147 | | _spans[_spans.Count - 1] = new BoundarySpan2D(first, second, start, end, merged, kind, otherMember, |
| | 23 | 148 | | lower = upper; |
| | 23 | 149 | | continue; |
| | | 150 | | } |
| | | 151 | | } |
| | 10389 | 152 | | var interval = new RationalSegmentInterval(lower.N, lower.D, upper.N, upper.D); |
| | 10389 | 153 | | _spans.Add(new BoundarySpan2D(first, second, start, end, interval, kind, otherMember, bounds)); |
| | 10389 | 154 | | lower = upper; |
| | | 155 | | } |
| | 9766 | 156 | | } |
| | | 157 | | |
| | | 158 | | private SpanKind ClassifyInterval(Parameter lower, Parameter upper, int sourceMember, out int otherMember) |
| | | 159 | | { |
| | 41648 | 160 | | _own.Clear(); _opposite.Clear(); _own.Add(sourceMember); otherMember = -1; |
| | 27522 | 161 | | foreach (Coverage coverage in _coverage.AsReadOnlySpan()) |
| | | 162 | | { |
| | 3349 | 163 | | if (coverage.Lower.CompareTo(lower) > 0 || coverage.Upper.CompareTo(upper) < 0) continue; |
| | | 164 | | // A mixed convex fan contributes one member, including along |
| | | 165 | | // internal diagonals; duplicate triangles are not extra solids. |
| | 2648 | 166 | | if ((coverage.Sides & 1) != 0) _own.Add(coverage.Member); |
| | 2549 | 167 | | if ((coverage.Sides & 2) == 0) continue; |
| | 5064 | 168 | | _opposite.Add(coverage.Member); otherMember = coverage.Member; |
| | | 169 | | } |
| | 12827 | 170 | | if (_own.Count == 1 && _opposite.Count == 1) return SpanKind.Joined; |
| | 7997 | 171 | | otherMember = -1; |
| | 7997 | 172 | | return _own.Count == 1 && _opposite.Count == 0 ? SpanKind.Exposed : SpanKind.Ambiguous; |
| | | 173 | | } |
| | | 174 | | |
| | | 175 | | private readonly struct Coverage |
| | | 176 | | { |
| | | 177 | | internal readonly int Member, Sides; |
| | | 178 | | internal readonly Parameter Lower, Upper; |
| | | 179 | | internal Coverage(int member, int sides, Parameter lower, Parameter upper) |
| | 12555 | 180 | | { Member = member; Sides = sides; Lower = lower; Upper = upper; } |
| | | 181 | | } |
| | | 182 | | |
| | | 183 | | private readonly struct Parameter : IComparable<Parameter> |
| | | 184 | | { |
| | | 185 | | internal readonly Signed320 N, D; |
| | 11642 | 186 | | internal static Parameter Zero => new(default, Signed320.One); |
| | 11642 | 187 | | internal static Parameter One => new(Signed320.One, Signed320.One); |
| | 107508 | 188 | | private Parameter(Signed320 n, Signed320 d) { N = n; D = d; } |
| | 8175 | 189 | | internal static Parameter Lower(in RationalSegmentInterval interval) => interval.IsClipped |
| | 8175 | 190 | | ? new(interval.LowerNumerator, interval.LowerDenominator) : Zero; |
| | 8129 | 191 | | internal static Parameter Upper(in RationalSegmentInterval interval) => interval.IsClipped |
| | 8129 | 192 | | ? new(interval.UpperNumerator, interval.UpperDenominator) : One; |
| | 46827 | 193 | | public int CompareTo(Parameter other) => RationalSegmentInterval.CompareParameters(N, D, other.N, other.D); |
| | | 194 | | } |
| | | 195 | | } |