| | | 1 | | //======================================================================= |
| | | 2 | | // MeshConeFaceRegions.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>Chooses the smaller exact face-ray maximum independently for each admitted surface component.</summary> |
| | | 16 | | /// <content>Exact component orientation and retained triangle grouping.</content> |
| | | 17 | | internal sealed partial class MeshConeFaceRegions |
| | | 18 | | { |
| | 10746 | 19 | | private Region[] _regions = Array.Empty<Region>(); |
| | | 20 | | private ConePlaneRayFrame _frame; |
| | | 21 | | private Vector3d _worldNormal; |
| | | 22 | | private Vector3d _samplingOrigin; |
| | 10746 | 23 | | private ulong[] _selectionValues = Array.Empty<ulong>(); |
| | 10746 | 24 | | private int[] _selectionSigns = Array.Empty<int>(); |
| | 10746 | 25 | | private Winner[] _winners = Array.Empty<Winner>(); |
| | 21492 | 26 | | private int _fieldWords = ConePlaneRaySelection.FieldWords, _rootWords = ConePlaneRaySelection.RootWords; |
| | 320271 | 27 | | private int SelectionWords => 12 * _fieldWords + _rootWords; |
| | 39729 | 28 | | private int PointWords => 8 * _fieldWords; |
| | | 29 | | private readonly ConePlaneRayEventVisitor _eventVisitor; |
| | | 30 | | private PhysicsMesh? _gatherMesh; |
| | | 31 | | private MeshSurfaceConnectivity? _gatherConnectivity; |
| | | 32 | | private int _gatherSurfaceTriangle; |
| | | 33 | | private bool _gatherUsesConvexFan; |
| | | 34 | | private bool _gatherContainsSection; |
| | | 35 | | private bool _wholeConeSamplingRangeRepresentable; |
| | | 36 | | |
| | 10746 | 37 | | internal MeshConeFaceRegions() |
| | | 38 | | { |
| | 10746 | 39 | | _eventVisitor = AdmitEvaluatedPoint; |
| | 10746 | 40 | | _sampleComparer = System.Collections.Generic.Comparer<Sample>.Create(ComparePoints); |
| | 10746 | 41 | | } |
| | | 42 | | |
| | | 43 | | internal int RegionCount { get; private set; } |
| | | 44 | | internal bool HasBoundaryIntersection { get; private set; } |
| | 1506 | 45 | | internal Vector3d SamplingOrigin => _samplingOrigin; |
| | | 46 | | |
| | | 47 | | internal void Build(PhysicsMesh mesh, int surfaceTriangle, Vector3d coneCenter, |
| | | 48 | | FixedQuaternion coneRotation, Fixed64 height, Fixed64 radius, |
| | | 49 | | MeshSurfaceConnectivity connectivity, ReadOnlySpan<int> admittedTriangles, Vector3d samplingOrigin = default) |
| | | 50 | | { |
| | 375 | 51 | | mesh.GetGeometryTriangleVertices(surfaceTriangle, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 375 | 52 | | var frame = new ConePlaneRayFrame(new FixedTriangle(a, b, c), mesh.GeometryFrame, |
| | 375 | 53 | | coneCenter, coneRotation, height, radius); |
| | 375 | 54 | | Build(mesh, surfaceTriangle, frame, connectivity, admittedTriangles, samplingOrigin); |
| | 375 | 55 | | } |
| | | 56 | | |
| | | 57 | | /// <remarks> |
| | | 58 | | /// Connectivity must have been built from this same mesh, surface, cone |
| | | 59 | | /// frame and complete admitted list. Descriptors and grouped indices are |
| | | 60 | | /// valid until the next Build; only the prepared surface owner is consumed. |
| | | 61 | | /// This selects an orientation and its extremal ray, before sample coverage |
| | | 62 | | /// or rigid-body response policy is applied. |
| | | 63 | | /// </remarks> |
| | | 64 | | internal void Build(PhysicsMesh mesh, int surfaceTriangle, in ConePlaneRayFrame frame, |
| | | 65 | | MeshSurfaceConnectivity connectivity, ReadOnlySpan<int> admittedTriangles, Vector3d samplingOrigin = default) |
| | | 66 | | { |
| | 6102 | 67 | | _joined = null; |
| | 6102 | 68 | | _currentMember = _currentSupplier = _currentSpan = -1; |
| | 6102 | 69 | | RegionCount = connectivity.RegionCount; |
| | 6102 | 70 | | HasBoundaryIntersection = false; |
| | 6103 | 71 | | if (RegionCount == 0) return; |
| | 12202 | 72 | | _fieldWords = frame.FieldWords; _rootWords = frame.RootWords; |
| | 6101 | 73 | | Reserve(RegionCount); |
| | 6101 | 74 | | mesh.GetGeometryTriangleVertices(surfaceTriangle, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 6101 | 75 | | _frame = frame; |
| | 6101 | 76 | | _samplingOrigin = samplingOrigin; |
| | 6101 | 77 | | _wholeConeSamplingRangeRepresentable = ConePlaneRayPointMaterialization.IsRangeRepresentable(_frame, samplingOri |
| | 6101 | 78 | | _worldNormal = _frame.GetWorldNormal(mesh.Rotation); |
| | 6101 | 79 | | var metrics = new WidePlaneMetrics(new FixedTriangle(a, b, c), mesh.GeometryFrame); |
| | 6101 | 80 | | Array.Clear(_winners, 0, 2 * RegionCount); |
| | 6101 | 81 | | _poolEvents.FastClear(); |
| | 6101 | 82 | | GatherEvents(mesh, surfaceTriangle, connectivity); |
| | 24450 | 83 | | for (int region = 0; region < RegionCount; region++) |
| | | 84 | | { |
| | 6124 | 85 | | ReduceRegion(region); |
| | 6124 | 86 | | ReduceSamples(region, metrics); |
| | | 87 | | } |
| | 6101 | 88 | | } |
| | | 89 | | |
| | | 90 | | private void Reserve(int count) |
| | | 91 | | { |
| | 9141 | 92 | | if (_regions.Length < count) |
| | | 93 | | { |
| | 303 | 94 | | int capacity = Math.Max(count, _regions.Length * 2); |
| | 303 | 95 | | Array.Resize(ref _regions, capacity); |
| | 303 | 96 | | Array.Resize(ref _sampleCounts, capacity); |
| | 303 | 97 | | Array.Resize(ref _selectionSigns, 2 * capacity * ConePlaneRaySelection.SignCount); |
| | 303 | 98 | | Array.Resize(ref _winners, 2 * capacity); |
| | 303 | 99 | | Array.Resize(ref _samples, capacity * ContactManifold.MaxContactsPerGroup); |
| | | 100 | | } |
| | 9141 | 101 | | int required = 2 * _regions.Length * SelectionWords; |
| | 9444 | 102 | | if (_selectionValues.Length < required) Array.Resize(ref _selectionValues, required); |
| | 9141 | 103 | | } |
| | | 104 | | |
| | | 105 | | private void GatherEvents(PhysicsMesh mesh, int surfaceTriangle, MeshSurfaceConnectivity connectivity) |
| | | 106 | | { |
| | 6101 | 107 | | _gatherSurfaceTriangle = surfaceTriangle; |
| | 6101 | 108 | | _gatherMesh = mesh; |
| | 6101 | 109 | | _gatherConnectivity = connectivity; |
| | | 110 | | try |
| | | 111 | | { |
| | 6101 | 112 | | ReadOnlySpan<int> boundary = mesh.GetCanonicalSurfaceBoundaryVertexPairs(surfaceTriangle); |
| | 6101 | 113 | | ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices; |
| | 6101 | 114 | | int firstIntersectingEdge = 0; |
| | 50217 | 115 | | for (; firstIntersectingEdge < boundary.Length; firstIntersectingEdge += 2) |
| | 22696 | 116 | | if (_frame.IntersectsSegment(new FixedSegment(vertices[boundary[firstIntersectingEdge]], |
| | 22696 | 117 | | vertices[boundary[firstIntersectingEdge + 1]]))) break; |
| | 6101 | 118 | | HasBoundaryIntersection = firstIntersectingEdge < boundary.Length; |
| | | 119 | | // The finite cone section is connected and at least one triangle |
| | | 120 | | // is admitted. A complete owner with a disjoint true boundary |
| | | 121 | | // contains that whole section. Neighbor closure also preserves |
| | | 122 | | // callers' deliberately partial admitted domains; hole boundaries |
| | | 123 | | // and closed touch prevent the containment certificate. |
| | 6101 | 124 | | if (RegionCount == 1 && firstIntersectingEdge == boundary.Length) |
| | | 125 | | { |
| | 5463 | 126 | | _gatherContainsSection = connectivity.IsCompleteSingleRegion(mesh); |
| | | 127 | | // A complete section needs no fan, and an incomplete owner |
| | | 128 | | // cannot use one. Do not repeat the same closure check. |
| | 5463 | 129 | | _gatherUsesConvexFan = false; |
| | | 130 | | } |
| | | 131 | | else |
| | | 132 | | { |
| | 638 | 133 | | _gatherContainsSection = false; |
| | 638 | 134 | | _gatherUsesConvexFan = !GetCompleteConvexFan(mesh, surfaceTriangle, connectivity).IsEmpty; |
| | | 135 | | } |
| | | 136 | | // Synchronous visitation borrows each exact construction directly, |
| | | 137 | | // avoiding chart reconstruction per descriptor. Only compact pool |
| | | 138 | | // metadata and the two owned winner banks survive the callback. |
| | 6101 | 139 | | ConePlaneRayEvents.VisitEvents(ConePlaneRayEventSource.Plane, _frame, _eventVisitor); |
| | 17036 | 140 | | for (int edge = firstIntersectingEdge; edge < boundary.Length; edge += 2) |
| | | 141 | | { |
| | 2417 | 142 | | var segment = new FixedSegment(vertices[boundary[edge]], vertices[boundary[edge + 1]]); |
| | 2417 | 143 | | if (edge != firstIntersectingEdge && !_frame.IntersectsSegment(segment)) continue; |
| | 2035 | 144 | | var source = new ConePlaneRayEventSource(segment); |
| | 2035 | 145 | | ConePlaneRayEvents.VisitEvents(source, _frame, _eventVisitor); |
| | | 146 | | } |
| | 6101 | 147 | | } |
| | | 148 | | finally |
| | | 149 | | { |
| | | 150 | | // Context scratch must not retain the last queried mesh, including |
| | | 151 | | // when exact geometry or materialization rejects the operation. |
| | 6101 | 152 | | _gatherMesh = null; |
| | 6101 | 153 | | _gatherConnectivity = null; |
| | 6101 | 154 | | _gatherUsesConvexFan = false; |
| | 6101 | 155 | | _gatherContainsSection = false; |
| | 6101 | 156 | | } |
| | 6101 | 157 | | } |
| | | 158 | | |
| | | 159 | | private void AdmitEvaluatedPoint(in ConePlaneRayEventSource source, in ConePlaneRayFrame frame, |
| | | 160 | | ConePlaneRayEvent descriptor, scoped ConePlaneRayPoint point, scoped ReadOnlySpan<ulong> root, |
| | | 161 | | scoped in ConePlaneRaySelection positive, scoped in ConePlaneRaySelection negative) |
| | | 162 | | { |
| | 37307 | 163 | | PhysicsMesh mesh = _gatherMesh!; |
| | 37307 | 164 | | ReadOnlySpan<int> corners = _gatherUsesConvexFan |
| | 37307 | 165 | | ? mesh.GetConvexCoplanarPatchCornerVertexIndices(_gatherSurfaceTriangle) : ReadOnlySpan<int>.Empty; |
| | 37307 | 166 | | int region = FindPointRegion(mesh, _gatherConnectivity!, corners, point, root); |
| | 39849 | 167 | | if (region < 0) return; |
| | 34765 | 168 | | KeepWinner(2 * region, positive); |
| | 34765 | 169 | | KeepWinner(2 * region + 1, negative); |
| | 34765 | 170 | | bool pointRepresentable = ConePlaneRayPointMaterialization.TryGetPointInFrame(frame, point, root, _samplingOrigi |
| | | 171 | | // Distinct constructions can certify the same exact point. Merge |
| | | 172 | | // their available directions before duplicate range checks; each |
| | | 173 | | // direction retains its own certificate for final materialization. |
| | | 174 | | // The callback already owns the incoming exact point; reconstruct |
| | | 175 | | // only the retained event when checking coordinate equality. |
| | 228044 | 176 | | for (int index = 0; index < _poolEvents.Count; index++) |
| | | 177 | | { |
| | 81844 | 178 | | PoolEvent retained = _poolEvents[index]; |
| | 81844 | 179 | | if (retained.Region != region || retained.MeshPoint != meshPoint || ConePlaneRayEvents.CompareEventAnchors( |
| | 81844 | 180 | | point, root, retained.Source, retained.Event, frame) != 0) continue; |
| | 2587 | 181 | | _poolEvents[index] = new PoolEvent(retained.Source, retained.Event, region, meshPoint, |
| | 2587 | 182 | | retained.Positive.HasValue ? retained.Positive : CheckRay(positive, 1, pointRepresentable), |
| | 2587 | 183 | | retained.Negative.HasValue ? retained.Negative : CheckRay(negative, -1, pointRepresentable)); |
| | 2587 | 184 | | return; |
| | | 185 | | } |
| | 32178 | 186 | | _poolEvents.Add(new PoolEvent(source, descriptor, region, meshPoint, |
| | 32178 | 187 | | CheckRay(positive, 1, pointRepresentable), CheckRay(negative, -1, pointRepresentable))); |
| | 32178 | 188 | | } |
| | | 189 | | |
| | | 190 | | private ReadOnlySpan<int> GetCompleteConvexFan(PhysicsMesh mesh, int surfaceTriangle, MeshSurfaceConnectivity connec |
| | | 191 | | { |
| | 661 | 192 | | if (RegionCount != 1) return ReadOnlySpan<int>.Empty; |
| | 615 | 193 | | ReadOnlySpan<int> corners = mesh.GetConvexCoplanarPatchCornerVertexIndices(surfaceTriangle); |
| | 940 | 194 | | if (corners.IsEmpty || corners.Length - 2 >= connectivity.TriangleIndices.Length) return ReadOnlySpan<int>.Empty |
| | 290 | 195 | | return connectivity.IsCompleteSingleRegion(mesh) ? corners : ReadOnlySpan<int>.Empty; |
| | | 196 | | } |
| | | 197 | | |
| | | 198 | | private int FindPointRegion(PhysicsMesh mesh, MeshSurfaceConnectivity connectivity, ReadOnlySpan<int> corners, |
| | | 199 | | scoped ConePlaneRayPoint point, scoped ReadOnlySpan<ulong> root) |
| | | 200 | | { |
| | 38437 | 201 | | if (_gatherContainsSection) return 0; |
| | 36177 | 202 | | if (!corners.IsEmpty) |
| | | 203 | | { |
| | 4896 | 204 | | ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices; |
| | 16704 | 205 | | for (int corner = 1; corner < corners.Length - 1; corner++) |
| | 7200 | 206 | | if (ConePlaneRayPointExits.ContainsTrianglePoint(new FixedTriangle(vertices[corners[0]], |
| | 10944 | 207 | | vertices[corners[corner]], vertices[corners[corner + 1]]), _frame, point, root)) return 0; |
| | 1152 | 208 | | return -1; |
| | | 209 | | } |
| | | 210 | | // A shared admitted point joins its incident triangles, so its first |
| | | 211 | | // closed-triangle match identifies the unique connectivity component. |
| | | 212 | | // A complete convex owner uses O(T + K*(B-2)) assignment. |
| | | 213 | | // ponytail: O(K*T) for incomplete, nonconvex and multiple-region owners; |
| | | 214 | | // use prepared incidence if measured large patches need it. |
| | 131981 | 215 | | foreach (int triangle in connectivity.TriangleIndices) |
| | | 216 | | { |
| | 49655 | 217 | | mesh.GetGeometryTriangleVertices(triangle, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 49655 | 218 | | if (ConePlaneRayPointExits.ContainsTrianglePoint(new FixedTriangle(a, b, c), _frame, point, root)) |
| | 29891 | 219 | | return connectivity.GetRegionOrdinal(triangle); |
| | | 220 | | } |
| | 1390 | 221 | | return -1; |
| | | 222 | | } |
| | | 223 | | |
| | | 224 | | private ConePlaneRaySelection BorrowWinner(int index) |
| | | 225 | | { |
| | 128256 | 226 | | var selection = new ConePlaneRaySelection( |
| | 128256 | 227 | | _selectionValues.AsSpan(index * SelectionWords, SelectionWords), |
| | 128256 | 228 | | _selectionSigns.AsSpan(index * ConePlaneRaySelection.SignCount, ConePlaneRaySelection.SignCount), _fieldWord |
| | 128256 | 229 | | Winner winner = _winners[index]; |
| | 238124 | 230 | | if (winner.HasValue) selection.Restore(GetFrame(winner.Member), winner.Source, winner.Event); |
| | 128256 | 231 | | return selection; |
| | | 232 | | } |
| | | 233 | | |
| | | 234 | | private void KeepWinner(int index, scoped in ConePlaneRaySelection candidate) |
| | | 235 | | { |
| | 69530 | 236 | | var selection = BorrowWinner(index); |
| | 69530 | 237 | | if (selection.KeepEvaluated(candidate, _frame)) |
| | 40885 | 238 | | _winners[index] = new Winner(selection.MaximumSource, selection.MaximumEvent); |
| | 69530 | 239 | | } |
| | | 240 | | |
| | | 241 | | private void ReduceRegion(int region) |
| | | 242 | | { |
| | 9187 | 243 | | var positive = BorrowWinner(2 * region); |
| | 9187 | 244 | | var negative = BorrowWinner(2 * region + 1); |
| | | 245 | | // Each closed component's maximum is intrinsic to its common plane or |
| | | 246 | | // lies on its true boundary; authored internal seams add no extrema. |
| | | 247 | | // Shared normal metric and frame scale cancel in the exact comparison. |
| | | 248 | | System.Diagnostics.Debug.Assert(positive.HasValue && negative.HasValue); |
| | 9187 | 249 | | int comparison = _joined?.Active == true ? positive.CompareDepth(negative) : ContactQuadratic.CompareRatios( |
| | 9187 | 250 | | ContactQuadratic.At(positive.Values, positive.Signs, 0, _fieldWords), |
| | 9187 | 251 | | ContactQuadratic.At(positive.Values, positive.Signs, 1, _fieldWords), positive.Root, |
| | 9187 | 252 | | ContactQuadratic.At(negative.Values, negative.Signs, 0, _fieldWords), |
| | 9187 | 253 | | ContactQuadratic.At(negative.Values, negative.Signs, 1, _fieldWords), negative.Root); |
| | 9187 | 254 | | int orientation = comparison <= 0 ? 1 : -1; |
| | 9187 | 255 | | _regions[region] = new Region(orientation); |
| | 9187 | 256 | | } |
| | | 257 | | |
| | | 258 | | // The cone witness lies along the exit ray from the mesh. Response moves |
| | | 259 | | // the cone in the opposite direction, including at exact zero-depth touch. |
| | 18402 | 260 | | internal Vector3d GetNormal(int region) => _joined?.Active == true |
| | 18402 | 261 | | ? -GetRegionFrame(region).GetWorldNormal(_joined.Members[_joined.NodeMembers[_joined.Roots[region]]].Collider.Me |
| | 18402 | 262 | | : -_worldNormal * GetOrientation(region); |
| | | 263 | | |
| | 19934 | 264 | | internal int GetOrientation(int region) => _regions[ValidateRegion(region)].Orientation; |
| | | 265 | | |
| | | 266 | | internal bool TryGetSelectedRay(int region, out Vector3d meshPoint, out Vector3d conePoint, out Fixed64 depth) |
| | | 267 | | { |
| | 142 | 268 | | meshPoint = conePoint = default; depth = default; |
| | 73 | 269 | | if ((uint)region >= (uint)RegionCount) return false; |
| | 69 | 270 | | if (_sampleCounts[region] > 0) |
| | | 271 | | { |
| | 66 | 272 | | Sample maximum = _samples[region * ContactManifold.MaxContactsPerGroup]; |
| | 198 | 273 | | meshPoint = maximum.MeshPoint; conePoint = maximum.ConePoint; depth = maximum.Depth; |
| | 66 | 274 | | return true; |
| | | 275 | | } |
| | 3 | 276 | | int orientation = _regions[region].Orientation; |
| | 3 | 277 | | var selected = BorrowWinner(2 * region + (orientation > 0 ? 0 : 1)); |
| | 3 | 278 | | return selected.TryMaterialize(_frame, orientation, _samplingOrigin, out meshPoint, out conePoint, out depth); |
| | | 279 | | } |
| | | 280 | | |
| | | 281 | | private int ValidateRegion(int region) |
| | | 282 | | { |
| | 77328 | 283 | | if ((uint)region >= (uint)RegionCount) throw new ArgumentOutOfRangeException(nameof(region)); |
| | 77318 | 284 | | return region; |
| | | 285 | | } |
| | | 286 | | |
| | | 287 | | private readonly struct Region |
| | | 288 | | { |
| | | 289 | | internal readonly int Orientation; |
| | 9187 | 290 | | internal Region(int orientation) => Orientation = orientation; |
| | | 291 | | } |
| | | 292 | | |
| | | 293 | | private readonly struct Winner |
| | | 294 | | { |
| | | 295 | | internal readonly ConePlaneRayEventSource Source; |
| | | 296 | | internal readonly ConePlaneRayEvent Event; |
| | | 297 | | internal readonly bool HasValue; |
| | | 298 | | internal readonly int Member, Supplier; |
| | | 299 | | internal readonly int Span; |
| | | 300 | | internal Winner(ConePlaneRayEventSource source, ConePlaneRayEvent descriptor, |
| | | 301 | | int member = -1, int supplier = -1, int span = -1) |
| | 436282 | 302 | | { Source = source; Event = descriptor; HasValue = true; Member = member; Supplier = supplier; Span = span; } |
| | | 303 | | } |
| | | 304 | | } |