| | | 1 | | //======================================================================= |
| | | 2 | | // MeshSurfaceBoundaryTopology.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 | | |
| | | 13 | | namespace Gravitas.CollisionHandling; |
| | | 14 | | |
| | | 15 | | /// <summary>Exact inward sectors of a trusted planar surface's genuine perimeter.</summary> |
| | | 16 | | internal static class MeshSurfaceBoundaryTopology |
| | | 17 | | { |
| | | 18 | | internal static int FindEdgeRegion(PhysicsMesh mesh, int owner, int first, int second, |
| | | 19 | | MeshSurfaceConnectivity connectivity, in ConePlaneRayFrame frame) => |
| | 1479 | 20 | | FindEdgeRegion(mesh, owner, first, second, connectivity, new ConeSection(frame)); |
| | | 21 | | |
| | | 22 | | internal static int FindEdgeRegion(PhysicsMesh mesh, int owner, int first, int second, |
| | | 23 | | MeshSurfaceConnectivity connectivity, in CapsuleFiniteSectionFrame frame) => |
| | 10285 | 24 | | FindEdgeRegion(mesh, owner, first, second, connectivity, new CapsuleSection(frame)); |
| | | 25 | | |
| | | 26 | | internal static int FindEdgeRegion(PhysicsMesh mesh, int owner, int first, int second, |
| | | 27 | | MeshSurfaceConnectivity connectivity, in FiniteSlabSectionFrame frame, LSCollider2D? embedded) => |
| | 48 | 28 | | FindEdgeRegion(mesh, owner, first, second, connectivity, new SlabSection(frame, embedded)); |
| | | 29 | | |
| | | 30 | | internal static int FindEdgeRegion(PhysicsMesh mesh, int owner, int first, int second, |
| | | 31 | | MeshSurfaceConnectivity connectivity, in FiniteSlabSectionFrame frame, ReadOnlyMemory<Vector2d> offsets) => |
| | 38784 | 32 | | FindEdgeRegion(mesh, owner, first, second, connectivity, new SlabSection(frame, null, offsets)); |
| | | 33 | | |
| | | 34 | | internal static int FindSupportEdgeRegion(PhysicsMesh mesh, int owner, int first, int second, |
| | | 35 | | MeshSurfaceConnectivity connectivity, MeshSlabFaceRegions faces, LSCollider2D? embedded) => |
| | 319 | 36 | | FindEdgeRegion(mesh, owner, first, second, connectivity, new SlabSupport(faces, embedded)); |
| | | 37 | | |
| | | 38 | | internal static int FindSupportEdgeRegion(PhysicsMesh mesh, int owner, int first, int second, |
| | | 39 | | MeshSurfaceConnectivity connectivity, MeshHullSurfaceContacts faces) => |
| | 56 | 40 | | FindEdgeRegion(mesh, owner, first, second, connectivity, new HullSupport(faces)); |
| | | 41 | | |
| | | 42 | | private static int FindEdgeRegion<TSection>(PhysicsMesh mesh, int owner, int first, int second, |
| | | 43 | | MeshSurfaceConnectivity connectivity, in TSection frame) where TSection : struct, ISection |
| | | 44 | | { |
| | 50971 | 45 | | ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices; |
| | | 46 | | // Reduced edges can span many authored subedges. Their first corner's |
| | | 47 | | // triangle need not touch the finite shape; locate an admitted piece. |
| | 50971 | 48 | | ReadOnlySpan<int> perimeter = mesh.GetManifoldSurfaceBoundaryVertexPairs(owner); |
| | 51518 | 49 | | if (perimeter.IsEmpty) return connectivity.GetRegionOrdinal(owner); |
| | 50424 | 50 | | mesh.GetGeometryTriangleVertices(owner, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 50424 | 51 | | new FixedTriangle(a, b, c).GetExactNormal(out Signed192 nx, out Signed192 ny, out _, out _); |
| | 50424 | 52 | | int axis = !nx.IsZero ? 0 : !ny.IsZero ? 1 : 2; |
| | 100848 | 53 | | Vector2d p = Project(vertices[first], axis), q = Project(vertices[second], axis); |
| | 50424 | 54 | | bool useX = p.X != q.X; |
| | 50424 | 55 | | Fixed64 low = FixedMath.Min(useX ? p.X : p.Y, useX ? q.X : q.Y); |
| | 50424 | 56 | | Fixed64 high = FixedMath.Max(useX ? p.X : p.Y, useX ? q.X : q.Y); |
| | 747908 | 57 | | for (int edge = 0; edge < perimeter.Length; edge += 2) |
| | | 58 | | { |
| | 674490 | 59 | | int start = perimeter[edge], end = perimeter[edge + 1]; |
| | 674490 | 60 | | Vector2d s = Project(vertices[start], axis), t = Project(vertices[end], axis); |
| | 337245 | 61 | | if (Vector2d.OrientationSign(p, q, s) != 0 || Vector2d.OrientationSign(p, q, t) != 0 |
| | 337245 | 62 | | || (useX ? s.X : s.Y) < low || (useX ? s.X : s.Y) > high |
| | 337245 | 63 | | || !frame.IntersectsSegment(new FixedSegment(vertices[start], vertices[end]))) continue; |
| | | 64 | | // A trusted exposed subedge with its start in this canonical span |
| | | 65 | | // cannot leave it: reduction merges monotone collinear chains, |
| | | 66 | | // while the embedding certificate rejects overlap/backtracking. |
| | 61031 | 67 | | foreach (int incident in mesh.GetWeldedVertexTriangleIndices(start)) |
| | | 68 | | { |
| | 23658 | 69 | | if (mesh.GetWeldedTriangleVertexIndices(incident).IndexOf(end) < 0) continue; |
| | 14004 | 70 | | int region = connectivity.GetRegionOrdinal(incident); |
| | 27719 | 71 | | if (region >= 0) return region; |
| | | 72 | | } |
| | | 73 | | } |
| | 36709 | 74 | | return -1; |
| | | 75 | | } |
| | | 76 | | |
| | | 77 | | internal static int GetIncidentFan(PhysicsMesh mesh, int owner, int origin, int opposite, |
| | | 78 | | bool interior, Span<WideAxis3> fan) |
| | | 79 | | { |
| | 19699 | 80 | | ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices; |
| | 39398 | 81 | | Vector3d p = vertices[origin], q = vertices[opposite]; |
| | 19699 | 82 | | mesh.GetGeometryTriangleVertices(owner, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 19699 | 83 | | new FixedTriangle(a, b, c).GetExactNormal(out Signed192 nx, out Signed192 ny, out _, out _); |
| | 19699 | 84 | | int axis = !nx.IsZero ? 0 : !ny.IsZero ? 1 : 2; |
| | 39398 | 85 | | Vector2d projectedOrigin = Project(p, axis), projectedOpposite = Project(q, axis); |
| | 19699 | 86 | | fan[0] = Ray(p, q); |
| | 19699 | 87 | | if (interior) |
| | | 88 | | { |
| | 11035 | 89 | | fan[1] = Negate(fan[0]); |
| | 11035 | 90 | | int inward = -1; |
| | 11035 | 91 | | bool useX = projectedOpposite.X != projectedOrigin.X; |
| | 11035 | 92 | | int direction = useX ? projectedOpposite.X.CompareTo(projectedOrigin.X) |
| | 11035 | 93 | | : projectedOpposite.Y.CompareTo(projectedOrigin.Y); |
| | 48349 | 94 | | foreach (int incident in mesh.GetWeldedVertexTriangleIndices(origin)) |
| | | 95 | | { |
| | 18657 | 96 | | if (mesh.GetManifoldSurfaceOwner(incident) != owner) continue; |
| | 15884 | 97 | | ReadOnlySpan<int> triangle = mesh.GetWeldedTriangleVertexIndices(incident); |
| | 15884 | 98 | | bool onEdge = false; |
| | 127072 | 99 | | foreach (int vertex in triangle) |
| | | 100 | | { |
| | 47652 | 101 | | Vector2d v = Project(vertices[vertex], axis); |
| | | 102 | | // Only the local ray direction matters. A positive |
| | | 103 | | // collinear incident edge supplies the same boundary |
| | | 104 | | // half-neighborhood after ±edge enforces tangency. |
| | 47652 | 105 | | if (v != projectedOrigin && Vector2d.OrientationSign(projectedOrigin, projectedOpposite, v) == 0 |
| | 58687 | 106 | | && (useX ? v.X.CompareTo(projectedOrigin.X) : v.Y.CompareTo(projectedOrigin.Y)) == direction) on |
| | | 107 | | } |
| | 15884 | 108 | | if (!onEdge) continue; |
| | 88280 | 109 | | foreach (int vertex in triangle) |
| | 33105 | 110 | | if (Vector2d.OrientationSign(projectedOrigin, projectedOpposite, Project(vertices[vertex], axis)) != |
| | 11035 | 111 | | inward = vertex; |
| | 11035 | 112 | | break; |
| | | 113 | | } |
| | | 114 | | System.Diagnostics.Debug.Assert(inward >= 0); |
| | 11035 | 115 | | fan[2] = Ray(p, vertices[inward]); |
| | 11035 | 116 | | return 3; |
| | | 117 | | } |
| | 8664 | 118 | | int neighbor = -1; |
| | 8664 | 119 | | ReadOnlySpan<int> boundary = mesh.GetCanonicalSurfaceBoundaryVertexPairs(owner); |
| | 180746 | 120 | | for (int edge = 0; edge < boundary.Length; edge += 2) |
| | | 121 | | { |
| | 163418 | 122 | | int first = boundary[edge], second = boundary[edge + 1]; |
| | 88789 | 123 | | if (first == origin && second != opposite) neighbor = second; |
| | 76213 | 124 | | else if (second == origin && first != opposite) neighbor = first; |
| | | 125 | | } |
| | | 126 | | System.Diagnostics.Debug.Assert(neighbor >= 0); |
| | 8664 | 127 | | fan[1] = Ray(p, vertices[neighbor]); |
| | 8664 | 128 | | Vector2d projectedNeighbor = Project(vertices[neighbor], axis); |
| | 8664 | 129 | | int orientation = Vector2d.OrientationSign(projectedOrigin, projectedOpposite, projectedNeighbor); |
| | | 130 | | // A certified simple planar boundary has exactly two noncollinear |
| | | 131 | | // corner rays. Check the actual incident sectors before choosing their |
| | | 132 | | // minor positive hull: a reflex fan has only plane-normal support. |
| | 61913 | 133 | | foreach (int incident in mesh.GetWeldedVertexTriangleIndices(origin)) |
| | | 134 | | { |
| | 22375 | 135 | | if (mesh.GetManifoldSurfaceOwner(incident) != owner) continue; |
| | 114957 | 136 | | foreach (int vertex in mesh.GetWeldedTriangleVertexIndices(incident)) |
| | | 137 | | { |
| | 43104 | 138 | | Vector2d v = Project(vertices[vertex], axis); |
| | 43104 | 139 | | if (orientation * Vector2d.OrientationSign(projectedOrigin, projectedOpposite, v) >= 0 |
| | 43104 | 140 | | && orientation * Vector2d.OrientationSign(projectedOrigin, v, projectedNeighbor) >= 0) continue; |
| | 330 | 141 | | fan[2] = Negate(fan[0]); fan[3] = Negate(fan[1]); |
| | 165 | 142 | | return 4; |
| | | 143 | | } |
| | | 144 | | } |
| | 8499 | 145 | | return 2; |
| | | 146 | | } |
| | | 147 | | |
| | 947261 | 148 | | private static Vector2d Project(Vector3d value, int axis) => axis == 0 |
| | 947261 | 149 | | ? new Vector2d(value.Y, value.Z) : axis == 1 ? new Vector2d(value.X, value.Z) : new Vector2d(value.X, value.Y); |
| | | 150 | | |
| | 11365 | 151 | | private static WideAxis3 Negate(WideAxis3 value) => new( |
| | 11365 | 152 | | WideArithmetic.Negate(value.X), WideArithmetic.Negate(value.Y), WideArithmetic.Negate(value.Z)); |
| | | 153 | | |
| | 40574 | 154 | | internal static WideAxis3 Ray(Vector3d first, Vector3d second) => new( |
| | 40574 | 155 | | Signed320.ExtendValue(WideArithmetic.SubtractSigned192(Signed192.Raw(second.X), Signed192.Raw(first.X))), |
| | 40574 | 156 | | Signed320.ExtendValue(WideArithmetic.SubtractSigned192(Signed192.Raw(second.Y), Signed192.Raw(first.Y))), |
| | 40574 | 157 | | Signed320.ExtendValue(WideArithmetic.SubtractSigned192(Signed192.Raw(second.Z), Signed192.Raw(first.Z)))); |
| | | 158 | | |
| | | 159 | | private interface ISection { bool IntersectsSegment(FixedSegment segment); } |
| | | 160 | | |
| | | 161 | | private readonly struct ConeSection : ISection |
| | | 162 | | { |
| | | 163 | | private readonly ConePlaneRayFrame _frame; |
| | 1479 | 164 | | internal ConeSection(in ConePlaneRayFrame frame) => _frame = frame; |
| | 1324 | 165 | | public bool IntersectsSegment(FixedSegment segment) => _frame.IntersectsSegment(segment); |
| | | 166 | | } |
| | | 167 | | |
| | | 168 | | private readonly struct CapsuleSection : ISection |
| | | 169 | | { |
| | | 170 | | private readonly CapsuleFiniteSectionFrame _frame; |
| | 10285 | 171 | | internal CapsuleSection(in CapsuleFiniteSectionFrame frame) => _frame = frame; |
| | 13803 | 172 | | public bool IntersectsSegment(FixedSegment segment) => _frame.IntersectsSegment(segment); |
| | | 173 | | } |
| | | 174 | | |
| | | 175 | | private readonly struct SlabSection : ISection |
| | | 176 | | { |
| | | 177 | | private readonly FiniteSlabSectionFrame frame; |
| | | 178 | | private readonly LSCollider2D? embedded; |
| | | 179 | | private readonly ReadOnlyMemory<Vector2d> offsets; |
| | | 180 | | internal SlabSection(in FiniteSlabSectionFrame frame, LSCollider2D? embedded, ReadOnlyMemory<Vector2d> offsets = |
| | 155328 | 181 | | { this.frame = frame; this.embedded = embedded; this.offsets = offsets; } |
| | | 182 | | public bool IntersectsSegment(FixedSegment segment) |
| | | 183 | | { |
| | 38788 | 184 | | Span<Vector2d> box = stackalloc Vector2d[4]; |
| | 38788 | 185 | | return frame.IntersectsSegment(segment, offsets.IsEmpty ? CollisionDetectionMixed.GetConvexPrismOffsets(embe |
| | | 186 | | } |
| | | 187 | | } |
| | | 188 | | |
| | | 189 | | private readonly struct SlabSupport : ISection |
| | | 190 | | { |
| | | 191 | | private readonly MeshSlabFaceRegions faces; |
| | | 192 | | private readonly LSCollider2D? embedded; |
| | | 193 | | internal SlabSupport(MeshSlabFaceRegions faces, LSCollider2D? embedded) |
| | 957 | 194 | | { this.faces = faces; this.embedded = embedded; } |
| | | 195 | | public bool IntersectsSegment(FixedSegment segment) |
| | | 196 | | { |
| | 383 | 197 | | Span<Vector2d> box = stackalloc Vector2d[4]; |
| | 383 | 198 | | return faces.IntersectsSupport(segment, faces.PrismOffsets.IsEmpty |
| | 383 | 199 | | ? CollisionDetectionMixed.GetConvexPrismOffsets(embedded, box) : faces.PrismOffsets.Span); |
| | | 200 | | } |
| | | 201 | | } |
| | | 202 | | |
| | | 203 | | private readonly struct HullSupport : ISection |
| | | 204 | | { |
| | | 205 | | private readonly MeshHullSurfaceContacts faces; |
| | 56 | 206 | | internal HullSupport(MeshHullSurfaceContacts faces) => this.faces = faces; |
| | 80 | 207 | | public bool IntersectsSegment(FixedSegment segment) => faces.IntersectsSupport(segment); |
| | | 208 | | } |
| | | 209 | | } |