| | | 1 | | //======================================================================= |
| | | 2 | | // MeshHullSurfaceContacts.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 System.Collections.Generic; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | using FixedMathSharp.Geometry; |
| | | 12 | | using Gravitas.Colliders; |
| | | 13 | | using SwiftCollections; |
| | | 14 | | using SwiftCollections.Query; |
| | | 15 | | |
| | | 16 | | namespace Gravitas.CollisionHandling; |
| | | 17 | | |
| | | 18 | | /// <summary>Admits native mesh supports against a declared surface's retained finite features.</summary> |
| | | 19 | | internal sealed class MeshHullSurfaceContacts : IConvexHullContactDomain, IScaledConvexHullContactDomain |
| | | 20 | | { |
| | 144 | 21 | | private MeshJoinedSurfaceRegions<WideDirection3d> _joined = new(); |
| | 144 | 22 | | private readonly SwiftList<Vector3d> _points = new(); |
| | 432 | 23 | | private readonly SwiftList<int> _vertices = new(), _triangles = new(), _edges = new(); |
| | 144 | 24 | | private readonly SwiftList<Vector3d> _facePoints = new(); |
| | 288 | 25 | | private readonly SwiftList<int> _faceTriangles = new(), _faceEdges = new(); |
| | 144 | 26 | | private readonly SwiftList<RationalSegmentInterval> _intervals = new(); |
| | 144 | 27 | | private readonly SwiftList<int> _supportCandidates = new(); |
| | 144 | 28 | | private readonly SwiftList<int> _supportTriangles = new(); |
| | 144 | 29 | | private readonly RationalSegmentInterval[] _supportClip = new RationalSegmentInterval[1]; |
| | 288 | 30 | | private readonly SwiftList<Sample> _samples = new(), _pool = new(); |
| | 144 | 31 | | private readonly SwiftList<ResponseGroup> _groups = new(); |
| | 144 | 32 | | private readonly SwiftList<(int Member, int Triangle, NativePenetration Penetration)> _admitted = new(); |
| | 144 | 33 | | private readonly SwiftHashSet<int> _overlappingMembers = new(); |
| | 144 | 34 | | private readonly SwiftList<Vector3d> _movingPoints = new(); |
| | 288 | 35 | | private readonly SwiftList<int> _movingTriangles = new(), _movingEdges = new(); |
| | 144 | 36 | | private readonly SwiftHashSet<int> _movingOwners = new(); |
| | | 37 | | private readonly PointSpanPenetrationAdmission _admit; |
| | | 38 | | private readonly ScaledHullPenetrationAdmission _admitScaled; |
| | 1 | 39 | | private static readonly RationalSegmentInterval[] FullInterval = { default }; |
| | 2 | 40 | | private static readonly int[] TriangleIndices = { 0, 1, 2 }, TriangleEdges = { 0, 1, 1, 2, 2, 0 }; |
| | 1 | 41 | | private static readonly IComparer<Sample> PointComparer = Comparer<Sample>.Create(CompareSamples); |
| | | 42 | | private PhysicsMesh _movingMesh = null!; |
| | | 43 | | private Vector3d _movingCenter; |
| | | 44 | | private FixedBoundVolume _supportBounds; |
| | | 45 | | private NativePenetration _supportPenetration; |
| | | 46 | | private bool _supportQuery, _movingPatch; |
| | 288 | 47 | | private int _member, _group, _movingFeature = -1, _sampleRegion = -1; |
| | 51518 | 48 | | private ReadOnlySpan<Vector3d> MovingPoints => _movingFeature < 0 ? _movingMesh.GeometryVertices : _movingPoints.AsR |
| | 51518 | 49 | | private ReadOnlySpan<int> MovingTriangles => _movingFeature < 0 ? _movingMesh.Triangles |
| | 51518 | 50 | | : _movingPatch ? _movingTriangles.AsReadOnlySpan() : TriangleIndices; |
| | 51518 | 51 | | private ReadOnlySpan<int> MovingEdges => _movingFeature < 0 ? _movingMesh.ConvexSatEdgeVertexPairs |
| | 51518 | 52 | | : _movingPatch ? _movingEdges.AsReadOnlySpan() : TriangleEdges; |
| | | 53 | | |
| | 576 | 54 | | internal MeshHullSurfaceContacts() { _admit = AdmitAxis; _admitScaled = AdmitScaledAxis; } |
| | | 55 | | |
| | | 56 | | internal void Release() |
| | 730 | 57 | | { _joined = null!; _movingMesh = null!; _samples.FastClear(); _groups.FastClear(); } |
| | | 58 | | |
| | | 59 | | internal void Build(ReadOnlySpan<(LSMeshCollider Collider, int Triangle)> sources, LSMeshCollider mover, |
| | | 60 | | ReadOnlySpan<PhysicsMesh> meshes, SwiftDictionary<(PhysicsMesh, int), MeshSurfaceBoundaryDomain> boundaries) |
| | | 61 | | { |
| | 23315 | 62 | | _movingMesh = mover.Mesh; _movingCenter = mover.Center; _samples.FastClear(); _groups.FastClear(); _overlappingM |
| | 4663 | 63 | | FixedBoundBox supportBounds = FixedBoundBox.FromCenterAndScopeClippedToDomain(mover.Center, |
| | 4663 | 64 | | Vector3d.One * (mover.CanonicalCenteredProxyRadius * Fixed64.Two)); |
| | 4663 | 65 | | _supportBounds = new(supportBounds.Min, supportBounds.Max); |
| | 4663 | 66 | | _joined ??= new(); |
| | 4663 | 67 | | _joined.Prepare(sources, meshes, boundaries); |
| | 26270 | 68 | | for (int member = 0; member < _joined.Inputs.Count; member++) |
| | | 69 | | { |
| | 16944 | 70 | | var input = _joined.Members[member]; var mesh = input.Collider.Mesh; |
| | 8472 | 71 | | mesh.GetGeometryTriangleVertices(input.Triangle, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 8472 | 72 | | input.Frame = WideTriangleRelations.GetScaledTriangleLocalNormal(new FixedTriangle(a, b, c), |
| | 8472 | 73 | | mesh.GeometryFrame).Transform(mesh.GeometryFrame.GeometryBasis); |
| | | 74 | | } |
| | 4663 | 75 | | var bounds = new FixedBoundVolume(mover.BoundsMin, mover.BoundsMax); |
| | 4663 | 76 | | _movingPatch = false; |
| | 4663 | 77 | | if (mover.Mode == MeshColliderMode.Convex && _movingMesh.IsClosedSurface) |
| | 5874 | 78 | | { _movingFeature = -1; BuildPart(bounds); } |
| | | 79 | | else |
| | | 80 | | { |
| | 1726 | 81 | | _movingOwners.Clear(); |
| | 44306 | 82 | | for (int triangle = 0; triangle < _movingMesh.TriangleCount; triangle++) |
| | | 83 | | { |
| | 20427 | 84 | | int owner = _movingMesh.GetManifoldSurfaceOwner(triangle); |
| | 20427 | 85 | | ReadOnlySpan<int> corners = _movingMesh.GetConvexCoplanarPatchCornerVertexIndices(triangle); |
| | 20427 | 86 | | _movingPatch = !corners.IsEmpty; |
| | 20427 | 87 | | if (_movingPatch) |
| | | 88 | | { |
| | 20380 | 89 | | if (!_movingOwners.Add(owner)) continue; |
| | 10190 | 90 | | _movingFeature = owner; |
| | 10190 | 91 | | FillPatch(_movingMesh, corners, _movingPoints, _movingTriangles, _movingEdges); |
| | | 92 | | } |
| | | 93 | | else |
| | | 94 | | { |
| | 47 | 95 | | _movingFeature = triangle; |
| | 47 | 96 | | _movingMesh.GetGeometryTriangleVertices(triangle, out var a, out var b, out var c); |
| | 188 | 97 | | _movingPoints.FastClear(); _movingPoints.Add(a); _movingPoints.Add(b); _movingPoints.Add(c); |
| | | 98 | | } |
| | 10237 | 99 | | BuildPart(bounds); |
| | | 100 | | } |
| | | 101 | | } |
| | 4663 | 102 | | _joined.Commit(); |
| | 4663 | 103 | | } |
| | | 104 | | |
| | | 105 | | private void BuildPart(FixedBoundVolume bounds) |
| | | 106 | | { |
| | 26348 | 107 | | _admitted.FastClear(); _joined.PrepareResponseComponents(); |
| | 74024 | 108 | | for (_member = 0; _member < _joined.Inputs.Count; _member++) |
| | | 109 | | { |
| | 23838 | 110 | | var member = _joined.Members[_member]; |
| | 23838 | 111 | | member.Admitted.FastClear(); |
| | 23838 | 112 | | ReadOnlySpan<int> corners = _movingFeature < 0 || _movingPatch |
| | 23838 | 113 | | ? member.Collider.Mesh.GetConvexCoplanarPatchCornerVertexIndices(member.Triangle) : default; |
| | 23838 | 114 | | if (!corners.IsEmpty) |
| | | 115 | | { |
| | 23743 | 116 | | SetPatch(corners); |
| | 23743 | 117 | | if (FindPenetration(out var penetration, filtered: false)) |
| | 22756 | 118 | | { member.Admitted.Add(member.Triangle); _admitted.Add((_member, -1, penetration)); } |
| | | 119 | | } |
| | | 120 | | else |
| | | 121 | | { |
| | 95 | 122 | | member.Collider.GetTrianglesInBounds(bounds, _joined.Candidates); |
| | 620 | 123 | | foreach (int triangle in _joined.Candidates.AsReadOnlySpan()) |
| | | 124 | | { |
| | 215 | 125 | | if (member.Collider.Mesh.GetManifoldSurfaceOwner(triangle) != member.Owner) continue; |
| | 213 | 126 | | SetTriangle(triangle); |
| | 213 | 127 | | if (FindPenetration(out var penetration, filtered: false)) |
| | 162 | 128 | | { member.Admitted.Add(triangle); _admitted.Add((_member, triangle, penetration)); } |
| | | 129 | | } |
| | | 130 | | } |
| | 23838 | 131 | | if (member.Admitted.Count > 0) |
| | 22832 | 132 | | { _joined.AddAdmittedMember(_member); _overlappingMembers.Add(member.Collider.Id); } |
| | | 133 | | } |
| | 49266 | 134 | | foreach (var admitted in _admitted.AsReadOnlySpan()) |
| | | 135 | | { |
| | 11459 | 136 | | _member = admitted.Member; |
| | 22918 | 137 | | var member = _joined.Members[_member]; PhysicsMesh mesh = member.Collider.Mesh; |
| | 22837 | 138 | | if (admitted.Triangle < 0) SetPatch(mesh.GetConvexCoplanarPatchCornerVertexIndices(member.Triangle)); |
| | 81 | 139 | | else SetTriangle(admitted.Triangle); |
| | 11459 | 140 | | BuildContacts(admitted.Penetration); |
| | | 141 | | } |
| | 13174 | 142 | | } |
| | | 143 | | |
| | | 144 | | private void SetPatch(ReadOnlySpan<int> corners) |
| | | 145 | | { |
| | 70242 | 146 | | _vertices.FastClear(); _vertices.AddRange(corners); |
| | 35121 | 147 | | FillPatch(_joined.Members[_member].Collider.Mesh, corners, _points, _triangles, _edges); |
| | 35121 | 148 | | } |
| | | 149 | | |
| | | 150 | | internal static void FillPatch(PhysicsMesh mesh, ReadOnlySpan<int> corners, SwiftList<Vector3d> points, |
| | | 151 | | SwiftList<int> triangles, SwiftList<int> edges) |
| | | 152 | | { |
| | 207027 | 153 | | points.FastClear(); triangles.FastClear(); edges.FastClear(); |
| | 690090 | 154 | | for (int index = 0; index < corners.Length; index++) |
| | | 155 | | { |
| | 276036 | 156 | | points.Add(mesh.GeometryVertices[corners[index]]); |
| | 552072 | 157 | | edges.Add(index); edges.Add((index + 1) % corners.Length); |
| | 276036 | 158 | | if (index < 2) continue; |
| | 414054 | 159 | | triangles.Add(0); triangles.Add(index - 1); triangles.Add(index); |
| | | 160 | | } |
| | 69009 | 161 | | } |
| | | 162 | | |
| | | 163 | | private void SetTriangle(int triangle) |
| | | 164 | | { |
| | 294 | 165 | | PhysicsMesh mesh = _joined.Members[_member].Collider.Mesh; |
| | 1176 | 166 | | _vertices.FastClear(); _points.FastClear(); _triangles.FastClear(); _edges.FastClear(); |
| | 2352 | 167 | | foreach (int vertex in mesh.GetWeldedTriangleVertexIndices(triangle)) |
| | 1764 | 168 | | { _vertices.Add(vertex); _points.Add(mesh.GeometryVertices[vertex]); } |
| | 588 | 169 | | _triangles.AddRange(TriangleIndices); _edges.AddRange(TriangleEdges); |
| | 294 | 170 | | } |
| | | 171 | | |
| | | 172 | | private bool FindPenetration(out NativePenetration best, bool filtered) |
| | | 173 | | { |
| | 24385 | 174 | | PhysicsMesh target = _joined.Members[_member].Collider.Mesh; |
| | 24385 | 175 | | if (!_movingMesh.GeometryFrame.IsRigid || !target.GeometryFrame.IsRigid) |
| | | 176 | | { |
| | 88 | 177 | | bool overlaps = WideOrientedBox.TryGetConvexHullPenetration(_movingMesh.GeometryFrame, |
| | 88 | 178 | | MovingPoints, MovingTriangles, MovingEdges, target.GeometryFrame, |
| | 88 | 179 | | _points.AsReadOnlySpan(), _triangles.AsReadOnlySpan(), _edges.AsReadOnlySpan(), |
| | 88 | 180 | | out WideScaledHullPenetration scaled, skipParallelDirections: true, |
| | 88 | 181 | | admission: filtered ? _admitScaled : null, planarPair: _movingFeature >= 0); |
| | 88 | 182 | | best = new NativePenetration(scaled); |
| | 88 | 183 | | return overlaps; |
| | | 184 | | } |
| | 24297 | 185 | | bool rigidOverlap = _movingFeature >= 0 && !_movingPatch |
| | 24297 | 186 | | ? WideTriangleRelations.TryGetPenetration(new(_movingPoints[0], _movingPoints[1], _movingPoints[2]), |
| | 24297 | 187 | | _movingMesh.Origin, _movingMesh.Rotation, new(_points[0], _points[1], _points[2]), |
| | 24297 | 188 | | target.Origin, target.Rotation, |
| | 24297 | 189 | | out var rigid, filtered ? _admit : null) |
| | 24297 | 190 | | : WideOrientedBox.TryGetConvexHullPenetration(_movingMesh.Origin, _movingMesh.Rotation, |
| | 24297 | 191 | | MovingPoints, MovingTriangles, MovingEdges, |
| | 24297 | 192 | | target.Origin, target.Rotation, |
| | 24297 | 193 | | _points.AsReadOnlySpan(), _triangles.AsReadOnlySpan(), _edges.AsReadOnlySpan(), |
| | 24297 | 194 | | out rigid, skipParallelDirections: true, admission: filtered ? _admit : null, planarPair: _movingPatch); |
| | 24297 | 195 | | best = new NativePenetration(rigid); |
| | 24297 | 196 | | return rigidOverlap; |
| | | 197 | | } |
| | | 198 | | |
| | 2123 | 199 | | private bool AdmitAxis(in WidePointSpanPenetration candidate) => AdmitAxis(new NativePenetration(candidate)); |
| | 206 | 200 | | private bool AdmitScaledAxis(in WideScaledHullPenetration candidate) => AdmitAxis(new NativePenetration(candidate)); |
| | 12329 | 201 | | private bool AdmitAxis(in NativePenetration candidate) => IsFaceNormal(candidate.Axis) |
| | 12329 | 202 | | ? FindGroup(candidate) < _groups.Count || VisitFace(candidate, admissionOnly: true) > 0 |
| | 12329 | 203 | | : Visit(candidate, admissionOnly: true) > 0; |
| | | 204 | | |
| | | 205 | | private int VisitFace(in NativePenetration penetration, bool admissionOnly) |
| | | 206 | | { |
| | 58257 | 207 | | int source = _member, component = _joined.FindResponseComponent(source), count = 0; |
| | 19419 | 208 | | int firstSample = _samples.Count; |
| | 19419 | 209 | | if (!admissionOnly) |
| | | 210 | | { |
| | 9698 | 211 | | _supportPenetration = penetration; |
| | 38792 | 212 | | _joined.Parents.FastClear(); _joined.Roots.FastClear(); _joined.RegionByRoot.FastClear(); _joined.NodeMember |
| | | 213 | | } |
| | 19419 | 214 | | Span<Vector3d> points = stackalloc Vector3d[3]; |
| | | 215 | | // A plane's support may project onto a member with no initial overlap. |
| | | 216 | | // Its genuine overlap carrier owns response; finite suppliers own anchors. |
| | 80508 | 217 | | for (int member = 0; member < _joined.Inputs.Count; member++) |
| | | 218 | | { |
| | 30544 | 219 | | if (_joined.FindResponseComponent(member) != component |
| | 30544 | 220 | | || !MaySupplyFace(_joined.Members[member].Collider, penetration.Axis)) |
| | 18794 | 221 | | { if (!admissionOnly) _joined.AddRegions(member, 0); continue; } |
| | 22414 | 222 | | _member = member; |
| | 44828 | 223 | | var supplier = _joined.Members[member]; var mesh = supplier.Collider.Mesh; |
| | 22414 | 224 | | ReadOnlySpan<int> corners = mesh.GetConvexCoplanarPatchCornerVertexIndices(supplier.Triangle); |
| | 22414 | 225 | | if (!corners.IsEmpty) |
| | | 226 | | { |
| | 22376 | 227 | | FillPatch(mesh, corners, _facePoints, _faceTriangles, _faceEdges); |
| | 22376 | 228 | | _sampleRegion = admissionOnly ? -1 : _joined.Parents.Count; |
| | 22376 | 229 | | int supplied = VisitFacePatch(mesh, _facePoints.AsReadOnlySpan(), _faceTriangles.AsReadOnlySpan(), |
| | 22376 | 230 | | _faceEdges.AsReadOnlySpan(), penetration, admissionOnly); |
| | 22376 | 231 | | count += supplied; |
| | 34639 | 232 | | if (!admissionOnly) _joined.AddRegions(member, supplied > 0 ? 1 : 0); |
| | 41768 | 233 | | if (admissionOnly && count > 0) { _member = source; return count; } |
| | | 234 | | continue; |
| | | 235 | | } |
| | 38 | 236 | | supplier.Collider.GetTrianglesInBounds(_supportBounds, _supportCandidates); |
| | 38 | 237 | | _supportTriangles.FastClear(); |
| | 1341 | 238 | | foreach (int triangle in _supportCandidates.AsReadOnlySpan()) |
| | | 239 | | { |
| | 639 | 240 | | if (mesh.GetManifoldSurfaceOwner(triangle) != supplier.Owner) continue; |
| | 637 | 241 | | mesh.GetGeometryTriangleVertices(triangle, out points[0], out points[1], out points[2]); |
| | 637 | 242 | | int supplied = VisitFacePatch(mesh, points, TriangleIndices, TriangleEdges, penetration, admissionOnly: |
| | 637 | 243 | | count += supplied; |
| | 698 | 244 | | if (supplied > 0) _supportTriangles.Add(triangle); |
| | 663 | 245 | | if (admissionOnly && count > 0) { _member = source; return count; } |
| | | 246 | | } |
| | 25 | 247 | | if (admissionOnly) continue; |
| | 13 | 248 | | supplier.Connectivity.BuildHullSupport(mesh, supplier.Triangle, this, _supportTriangles.AsReadOnlySpan()); |
| | 13 | 249 | | _joined.AddRegions(member); |
| | 122 | 250 | | foreach (int triangle in _supportTriangles.AsReadOnlySpan()) |
| | | 251 | | { |
| | 48 | 252 | | _sampleRegion = supplier.Offset + supplier.Connectivity.GetRegionOrdinal(triangle); |
| | 48 | 253 | | mesh.GetGeometryTriangleVertices(triangle, out points[0], out points[1], out points[2]); |
| | 48 | 254 | | VisitFacePatch(mesh, points, TriangleIndices, TriangleEdges, penetration, admissionOnly: false); |
| | | 255 | | } |
| | | 256 | | } |
| | 19408 | 257 | | if (!admissionOnly) CompleteFaceRegions(firstSample); |
| | 9710 | 258 | | _sampleRegion = -1; |
| | 9710 | 259 | | _member = source; |
| | 9710 | 260 | | return count; |
| | | 261 | | } |
| | | 262 | | |
| | | 263 | | private bool MaySupplyFace(LSMeshCollider supplier, in WideDirection3d normal) |
| | | 264 | | { |
| | | 265 | | // Plane projection preserves coordinates whose exact normal component |
| | | 266 | | // is zero. Cull only strict separation there, including remote suppliers. |
| | 195444 | 267 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 268 | | { |
| | 75308 | 269 | | if (!(coordinate == 0 ? normal.X : coordinate == 1 ? normal.Y : normal.Z).IsZero) continue; |
| | 52876 | 270 | | if (supplier.BoundsMax[coordinate] < _movingMesh.Bounds.Min[coordinate] |
| | 60936 | 271 | | || supplier.BoundsMin[coordinate] > _movingMesh.Bounds.Max[coordinate]) return false; |
| | | 272 | | } |
| | 22414 | 273 | | return true; |
| | | 274 | | } |
| | | 275 | | |
| | | 276 | | private void CompleteFaceRegions(int firstSample) |
| | | 277 | | { |
| | 54612 | 278 | | for (_member = 0; _member < _joined.Inputs.Count; _member++) |
| | | 279 | | { |
| | 17608 | 280 | | var member = _joined.Members[_member]; |
| | 17608 | 281 | | if (RegionCount(_member) == 0) continue; |
| | 118732 | 282 | | for (int span = 0; span < member.Boundary.Spans.Length; span++) |
| | | 283 | | { |
| | 48114 | 284 | | if (!member.JoinedSpans[span]) continue; |
| | 9422 | 285 | | var edge = member.Boundary.Spans[span]; |
| | 9422 | 286 | | int other = _joined.MemberBySurface[(_joined.Meshes[edge.OtherMember], edge.OtherSurface)]; |
| | 9422 | 287 | | if (other <= _member || RegionCount(other) == 0 |
| | 9422 | 288 | | || !IntersectsSupport(new FixedSegment(edge.Start, edge.End), edge.Interval)) continue; |
| | 1512 | 289 | | int first = SupportEdgeRegion(_member, edge.First, edge.Second); |
| | 1512 | 290 | | var neighbor = _joined.Members[other]; |
| | 3024 | 291 | | int saved = _member; _member = other; |
| | 15152 | 292 | | foreach (var reciprocal in neighbor.Boundary.Spans) |
| | | 293 | | { |
| | 6064 | 294 | | if (reciprocal.Kind != SpanKind.Joined |
| | 6064 | 295 | | || reciprocal.OtherMember != member.Boundary.SourceMember || reciprocal.OtherSurface != member.O |
| | 6064 | 296 | | || !IsReciprocalSpan(edge, member.Collider.Mesh, reciprocal, neighbor.Collider.Mesh)) continue; |
| | 1512 | 297 | | int second = SupportEdgeRegion(other, reciprocal.First, reciprocal.Second); |
| | | 298 | | System.Diagnostics.Debug.Assert(first >= 0 && second >= 0); |
| | 1512 | 299 | | _joined.Union(member.Offset + first, neighbor.Offset + second); |
| | | 300 | | } |
| | 1512 | 301 | | _member = saved; |
| | | 302 | | } |
| | | 303 | | } |
| | 9698 | 304 | | _joined.CompleteRegions(); |
| | 9698 | 305 | | ResponseGroup response = _groups[_group]; |
| | 19540 | 306 | | for (int region = 1; region < _joined.Roots.Count; region++) _groups.Add(response); |
| | 83374 | 307 | | for (int index = firstSample; index < _samples.Count; index++) |
| | | 308 | | { |
| | 31989 | 309 | | Sample sample = _samples[index]; |
| | 31989 | 310 | | _samples[index] = new(_group + _joined.RegionByRoot[_joined.Find(sample.Group)], sample.Member, sample.Point |
| | | 311 | | } |
| | 9698 | 312 | | } |
| | | 313 | | |
| | | 314 | | private static bool IsReciprocalSpan(in BoundarySpan3D source, PhysicsMesh sourceMesh, |
| | | 315 | | in BoundarySpan3D target, PhysicsMesh targetMesh) |
| | | 316 | | { |
| | | 317 | | // Trusted reciprocal spans are positive maximal runs after collinear |
| | | 318 | | // reduction and equal-class merging. Both exact ends identify the same |
| | | 319 | | // window; matching owners alone could cross-link separated U-shaped arms. |
| | 1516 | 320 | | Signed832 one = Signed832.ExtendValue(Signed192.One); |
| | 1516 | 321 | | Signed832 lowerN = source.Interval.IsClipped ? source.Interval.WideLowerNumerator : default; |
| | 1516 | 322 | | Signed832 lowerD = source.Interval.IsClipped ? source.Interval.WideLowerDenominator : one; |
| | 1516 | 323 | | Signed832 upperN = source.Interval.IsClipped ? source.Interval.WideUpperNumerator : one; |
| | 1516 | 324 | | Signed832 upperD = source.Interval.IsClipped ? source.Interval.WideUpperDenominator : one; |
| | 1516 | 325 | | var edge = new FixedSegment(source.Start, source.End); |
| | 1516 | 326 | | var other = new FixedSegment(target.Start, target.End); |
| | 1516 | 327 | | return WideTriangleRelations.LocateEdgePoint(edge, sourceMesh.GeometryFrame, lowerN, lowerD, |
| | 1516 | 328 | | other, targetMesh.GeometryFrame, target.Interval) >= 0 |
| | 1516 | 329 | | && WideTriangleRelations.LocateEdgePoint(edge, sourceMesh.GeometryFrame, upperN, upperD, |
| | 1516 | 330 | | other, targetMesh.GeometryFrame, target.Interval) >= 0; |
| | | 331 | | } |
| | | 332 | | |
| | 25303 | 333 | | private int RegionCount(int member) => (member + 1 < _joined.Inputs.Count |
| | 25303 | 334 | | ? _joined.Members[member + 1].Offset : _joined.Parents.Count) - _joined.Members[member].Offset; |
| | | 335 | | |
| | | 336 | | private int SupportEdgeRegion(int index, int first, int second) |
| | | 337 | | { |
| | 6040 | 338 | | if (RegionCount(index) == 1) return 0; |
| | 8 | 339 | | var member = _joined.Members[index]; |
| | 8 | 340 | | return MeshSurfaceBoundaryTopology.FindSupportEdgeRegion(member.Collider.Mesh, member.Owner, |
| | 8 | 341 | | first, second, member.Connectivity, this); |
| | | 342 | | } |
| | | 343 | | |
| | 169 | 344 | | internal bool IntersectsSupport(FixedSegment segment) => IntersectsSupport(segment, default); |
| | | 345 | | |
| | | 346 | | private bool IntersectsSupport(FixedSegment segment, in RationalSegmentInterval clip) |
| | | 347 | | { |
| | 1725 | 348 | | Span<Vector3d> points = stackalloc Vector3d[2] { segment.Start, segment.End }; |
| | 1725 | 349 | | var mesh = _joined.Members[_member].Collider.Mesh; |
| | 3450 | 350 | | _supportQuery = true; _supportClip[0] = clip; |
| | 1725 | 351 | | bool intersects = _supportPenetration.IsScaled |
| | 1725 | 352 | | ? WideOrientedBox.VisitConvexHullContacts(_movingMesh.GeometryFrame, _movingMesh.Origin, |
| | 1725 | 353 | | MovingPoints, MovingTriangles, MovingEdges, mesh.GeometryFrame, mesh.Origin, |
| | 1725 | 354 | | points, ReadOnlySpan<int>.Empty, TriangleEdges.AsSpan(0, 2), _supportPenetration.Scaled, |
| | 1725 | 355 | | this, static (owner, contact) => owner.AddSample(contact), this, admissionOnly: true) > 0 |
| | 1725 | 356 | | : WideOrientedBox.VisitConvexHullContacts(_movingMesh.Origin, _movingMesh.Rotation, |
| | 1725 | 357 | | MovingPoints, MovingTriangles, MovingEdges, |
| | 1725 | 358 | | mesh.Origin, mesh.Rotation, points, ReadOnlySpan<int>.Empty, TriangleEdges.AsSpan(0, 2), _supportPenetration |
| | 1725 | 359 | | this, static (owner, contact) => owner.AddSample(contact), this, admissionOnly: true) > 0; |
| | 1725 | 360 | | _supportQuery = false; |
| | 1725 | 361 | | return intersects; |
| | | 362 | | } |
| | | 363 | | |
| | | 364 | | private int VisitFacePatch(PhysicsMesh mesh, ReadOnlySpan<Vector3d> points, ReadOnlySpan<int> triangles, |
| | 23061 | 365 | | ReadOnlySpan<int> edges, in NativePenetration penetration, bool admissionOnly) => penetration.IsScaled |
| | 23061 | 366 | | ? WideOrientedBox.VisitConvexHullContacts(_movingMesh.GeometryFrame, _movingMesh.Origin, |
| | 23061 | 367 | | MovingPoints, MovingTriangles, MovingEdges, mesh.GeometryFrame, mesh.Origin, |
| | 23061 | 368 | | points, triangles, edges, penetration.Scaled, |
| | 23061 | 369 | | this, static (owner, contact) => owner.AddSample(contact), admissionOnly: admissionOnly) |
| | 23061 | 370 | | : WideOrientedBox.VisitConvexHullContacts(_movingMesh.Origin, _movingMesh.Rotation, |
| | 23061 | 371 | | MovingPoints, MovingTriangles, MovingEdges, |
| | 23061 | 372 | | mesh.Origin, mesh.Rotation, points, triangles, edges, penetration.Rigid, |
| | 23061 | 373 | | this, static (owner, contact) => owner.AddSample(contact), admissionOnly: admissionOnly); |
| | | 374 | | |
| | 2502 | 375 | | private int Visit(in NativePenetration penetration, bool admissionOnly) => penetration.IsScaled |
| | 2502 | 376 | | ? WideOrientedBox.VisitConvexHullContacts(_movingMesh.GeometryFrame, _movingMesh.Origin, |
| | 2502 | 377 | | MovingPoints, MovingTriangles, MovingEdges, _joined.Members[_member].Collider.Mesh.GeometryFrame, |
| | 2502 | 378 | | _joined.Members[_member].Collider.Mesh.Origin, _points.AsReadOnlySpan(), _triangles.AsReadOnlySpan(), |
| | 2502 | 379 | | _edges.AsReadOnlySpan(), penetration.Scaled, this, static (owner, contact) => owner.AddSample(contact), this |
| | 2502 | 380 | | : WideOrientedBox.VisitConvexHullContacts(_movingMesh.Origin, _movingMesh.Rotation, |
| | 2502 | 381 | | MovingPoints, MovingTriangles, MovingEdges, |
| | 2502 | 382 | | _joined.Members[_member].Collider.Mesh.Origin, _joined.Members[_member].Collider.Mesh.Rotation, |
| | 2502 | 383 | | _points.AsReadOnlySpan(), _triangles.AsReadOnlySpan(), _edges.AsReadOnlySpan(), penetration.Rigid, |
| | 2502 | 384 | | this, static (owner, contact) => owner.AddSample(contact), this, admissionOnly); |
| | | 385 | | |
| | | 386 | | private void BuildContacts(NativePenetration penetration) |
| | | 387 | | { |
| | | 388 | | // An already collected plane group is also its exact finite admission certificate. |
| | 12890 | 389 | | if ((_movingFeature < 0 || _movingPatch) && IsFaceNormal(penetration.Axis) && FindGroup(penetration) < _groups.C |
| | | 390 | | // Native triangle contacts historically orient by centroids. Response |
| | | 391 | | // admission must rank the actual signed pushes instead of that legacy orientation. |
| | 10028 | 392 | | if (_movingFeature >= 0 && !_movingPatch || !AdmitAxis(penetration)) |
| | | 393 | | { |
| | | 394 | | // Admission changes response ranking, never the already proved overlap. |
| | 429 | 395 | | bool overlaps = FindPenetration(out penetration, filtered: true); |
| | | 396 | | System.Diagnostics.Debug.Assert(overlaps); |
| | 433 | 397 | | if (!penetration.HasValue) return; |
| | | 398 | | } |
| | 10024 | 399 | | int component = _joined.FindResponseComponent(_member); |
| | 10024 | 400 | | WideDirection3d axis = penetration.Negate ? -penetration.Axis : penetration.Axis; |
| | 10024 | 401 | | _group = FindGroup(penetration); |
| | 10024 | 402 | | bool face = IsFaceNormal(axis); |
| | 19822 | 403 | | if (_group == _groups.Count) _groups.Add(new(component, axis, penetration, _movingFeature, _joined.GetResponseMe |
| | 442 | 404 | | else if (face) return; // The first accepted plane already collected every supplier. |
| | 19506 | 405 | | if (face) VisitFace(penetration, admissionOnly: false); |
| | 110 | 406 | | else Visit(penetration, admissionOnly: false); |
| | 110 | 407 | | } |
| | | 408 | | |
| | | 409 | | private int FindGroup(in NativePenetration penetration) |
| | | 410 | | { |
| | 30895 | 411 | | int component = _joined.FindResponseComponent(_member); |
| | 30895 | 412 | | WideDirection3d axis = penetration.Negate ? -penetration.Axis : penetration.Axis; |
| | 30895 | 413 | | int group = 0; |
| | 93565 | 414 | | for (; group < _groups.Count; group++) |
| | | 415 | | { |
| | 33208 | 416 | | ResponseGroup existing = _groups[group]; |
| | 33208 | 417 | | if (existing.Component == component && existing.Feature == _movingFeature && WideDirection3d.AreParallel(axi |
| | 33208 | 418 | | && WideDirection3d.Dot(axis, existing.Axis).Sign > 0 |
| | 33208 | 419 | | && !existing.Penetration.ShouldReplace(penetration) |
| | 33208 | 420 | | && !penetration.ShouldReplace(existing.Penetration)) break; |
| | | 421 | | } |
| | 30895 | 422 | | return group; |
| | | 423 | | } |
| | | 424 | | |
| | | 425 | | private void AddSample(FixedContactAnchors contact) |
| | | 426 | | { |
| | | 427 | | // Rank final points in one translated world chart without narrowing an absolute world position. |
| | 32138 | 428 | | bool represented = contact.SecondAnchor.TryGetPointInFrame(_movingCenter, out Vector3d point); |
| | | 429 | | System.Diagnostics.Debug.Assert(represented); |
| | 32138 | 430 | | _samples.Add(new(_sampleRegion < 0 ? _group : _sampleRegion, _member, point, contact)); |
| | 32138 | 431 | | } |
| | | 432 | | |
| | | 433 | | internal bool Append(LSMeshCollider collider, LSMeshCollider mover, ContactManifold manifold, bool reverse) |
| | | 434 | | { |
| | 5955 | 435 | | Span<int> chosen = stackalloc int[ContactManifold.MaxContactsPerGroup]; |
| | 37042 | 436 | | for (int group = 0; group < _groups.Count; group++) |
| | | 437 | | { |
| | 12566 | 438 | | ResponseGroup response = _groups[group]; |
| | 12566 | 439 | | if (_joined.Members[response.Carrier].Collider != collider) continue; |
| | 9862 | 440 | | _pool.FastClear(); |
| | 282728 | 441 | | foreach (Sample sample in _samples.AsReadOnlySpan()) if (sample.Group == group) _pool.Add(sample); |
| | 9862 | 442 | | _pool.SortInPlace(PointComparer); |
| | 9862 | 443 | | int unique = 0; |
| | 84158 | 444 | | foreach (Sample sample in _pool.AsReadOnlySpan()) |
| | | 445 | | { |
| | 32217 | 446 | | if (unique > 0 && ComparePoints(sample, _pool[unique - 1]) == 0) continue; |
| | 26765 | 447 | | _pool[unique++] = sample; |
| | | 448 | | } |
| | 9862 | 449 | | ReadOnlySpan<Sample> pool = _pool.AsReadOnlySpan()[..unique]; |
| | 9862 | 450 | | int count = Math.Min(pool.Length, chosen.Length); |
| | 9862 | 451 | | if (response.Penetration.IsScaled) |
| | | 452 | | { |
| | 298 | 453 | | for (int slot = 0; slot < count; slot++) |
| | 92 | 454 | | chosen[slot] = slot == 0 ? 0 : MeshPlaneSampleReduction.ChooseNext(pool, chosen[..slot], response.Ax |
| | 57 | 455 | | if (count > 2 && WidePlaneMetrics.GetAreaSign(response.Axis, |
| | 59 | 456 | | pool[chosen[0]].Point, pool[chosen[1]].Point, pool[chosen[2]].Point) == 0) count = 2; |
| | | 457 | | } |
| | | 458 | | else |
| | | 459 | | { |
| | 9805 | 460 | | var metrics = new WidePlaneMetrics(response.Penetration.Rigid.Axis); |
| | 70212 | 461 | | for (int slot = 0; slot < count; slot++) |
| | 25301 | 462 | | chosen[slot] = slot == 0 ? 0 : MeshPlaneSampleReduction.ChooseNext(pool, chosen[..slot], metrics, Po |
| | 10479 | 463 | | if (count > 2 && metrics.GetAreaSquared(pool[chosen[0]].Point, pool[chosen[1]].Point, pool[chosen[2]].Po |
| | | 464 | | } |
| | 69158 | 465 | | for (int slot = 0; slot < count; slot++) |
| | | 466 | | { |
| | 49434 | 467 | | Sample sample = pool[chosen[slot]]; var supplier = _joined.Members[sample.Member]; |
| | 24717 | 468 | | FixedContactAnchors contact = sample.Contact; |
| | 24717 | 469 | | int movingSurface = response.Feature < 0 ? 0 : _movingMesh.GetCanonicalSurfaceOrdinal(response.Feature) |
| | 24717 | 470 | | var key = new ContactGroupKey(0, 0, supplier.Collider.Mesh.GetCanonicalSurfaceOrdinal(supplier.Owner) + |
| | 24717 | 471 | | movingSurface, group, sourceIdA: supplier.Collider.Id + 1, |
| | 24717 | 472 | | sourceOrderA: ((IPhysicsColliderRegistryItem)supplier.Collider).ReplayOrder + 1); |
| | 24717 | 473 | | manifold.AddContact(new ContactAnchor(reverse ? contact.FirstAnchor : contact.SecondAnchor), |
| | 24717 | 474 | | new ContactAnchor(reverse ? contact.SecondAnchor : contact.FirstAnchor), contact.Depth, |
| | 24717 | 475 | | reverse ? contact.Normal : -contact.Normal, reverse ? mover.Material : supplier.Collider.Material, |
| | 24717 | 476 | | reverse ? supplier.Collider.Material : mover.Material, contact.DepthIsClamped, |
| | 24717 | 477 | | group: reverse ? key.Remap(0, 0, true) : key); |
| | | 478 | | } |
| | | 479 | | } |
| | 5955 | 480 | | return _overlappingMembers.Contains(collider.Id); |
| | | 481 | | } |
| | | 482 | | |
| | | 483 | | ReadOnlySpan<RationalSegmentInterval> IConvexHullContactDomain.GetEdgeIntervals(bool first, int start, int end, |
| | 14424 | 484 | | in WideAxis3 normal) => GetEdgeIntervals(first, start, end); |
| | | 485 | | |
| | | 486 | | public ReadOnlySpan<RationalSegmentInterval> GetEdgeIntervals(bool first, int start, int end, |
| | 775 | 487 | | in WideDirection3d normal) => GetEdgeIntervals(first, start, end); |
| | | 488 | | |
| | | 489 | | private ReadOnlySpan<RationalSegmentInterval> GetEdgeIntervals(bool first, int start, int end) |
| | | 490 | | { |
| | 26906 | 491 | | if (first) return FullInterval; |
| | 5145 | 492 | | if (_supportQuery) return _supportClip; |
| | 1839 | 493 | | var member = _joined.Members[_member]; |
| | | 494 | | // Face visitation uses the filled supplier directly; only nonface |
| | | 495 | | // candidates and the support-query branch above borrow this domain. |
| | 1839 | 496 | | _intervals.FastClear(); |
| | 20220 | 497 | | for (int span = 0; span < member.Boundary.Spans.Length; span++) |
| | | 498 | | { |
| | 8271 | 499 | | var edge = member.Boundary.Spans[span]; |
| | 8271 | 500 | | if (member.JoinedSpans[span]) continue; |
| | 6603 | 501 | | RationalSegmentInterval interval = member.GetRetainedInterval(ref span); |
| | 7330 | 502 | | if (MapBoundaryInterval(edge, interval, start, end, out var mapped)) _intervals.Add(mapped); |
| | | 503 | | } |
| | 1839 | 504 | | return _intervals.AsReadOnlySpan(); |
| | | 505 | | } |
| | | 506 | | |
| | | 507 | | private bool MapBoundaryInterval(in BoundarySpan3D edge, in RationalSegmentInterval interval, |
| | | 508 | | int start, int end, out RationalSegmentInterval mapped) |
| | | 509 | | { |
| | 15328 | 510 | | if (edge.First == _vertices[start] && edge.Second == _vertices[end]) |
| | 1032 | 511 | | { mapped = interval; return true; } |
| | | 512 | | // Canonical perimeters merge authored collinear subedges. Remap their |
| | | 513 | | // cuts before witness selection; matching only vertex IDs loses real rims. |
| | | 514 | | // Authored coordinates can be large even when scaled reach is small; |
| | | 515 | | // the interval owner retains full products before reducing these bounds. |
| | 14812 | 516 | | return interval.TryMapToSegment(new FixedSegment(edge.Start, edge.End), |
| | 14812 | 517 | | new FixedSegment(_points[start], _points[end]), out mapped); |
| | | 518 | | } |
| | | 519 | | |
| | | 520 | | bool IConvexHullContactDomain.ContainsPoint(bool first, int start, int end, in Signed832 numerator, |
| | 7559 | 521 | | in Signed832 denominator, in WideAxis3 normal) => ContainsPoint(first, start, end, numerator, denominator, new W |
| | | 522 | | |
| | | 523 | | public bool ContainsPoint(bool first, int start, int end, in Signed832 numerator, in Signed832 denominator, in WideD |
| | | 524 | | { |
| | 10480 | 525 | | if (first) return true; |
| | 4816 | 526 | | if (_supportQuery) |
| | | 527 | | { |
| | 1593 | 528 | | var clip = _supportClip[0]; |
| | 2741 | 529 | | if (!clip.IsClipped) return true; |
| | 445 | 530 | | Signed832 n = start == end ? (start == 0 ? default : denominator) : numerator; |
| | 445 | 531 | | return WideTriangleRelations.CompareCoreParameters(n, denominator, |
| | 445 | 532 | | clip.WideLowerNumerator, clip.WideLowerDenominator) >= 0 |
| | 445 | 533 | | && WideTriangleRelations.CompareCoreParameters(n, denominator, |
| | 445 | 534 | | clip.WideUpperNumerator, clip.WideUpperDenominator) <= 0; |
| | | 535 | | } |
| | 3223 | 536 | | var member = _joined.Members[_member]; |
| | 6446 | 537 | | int a = _vertices[start], b = _vertices[end]; |
| | 3223 | 538 | | if (start == end || numerator.IsZero || WideArithmetic.SubtractSigned832(denominator, numerator).IsZero) |
| | | 539 | | { |
| | 630 | 540 | | int vertex = start == end || numerator.IsZero ? a : b; |
| | 630 | 541 | | return SupportsJoinedFan(_joined.Fan.BuildAtPoint(_joined.Meshes.AsReadOnlySpan(), member.Boundary.SourceMem |
| | 630 | 542 | | member.Owner, member.Collider.Mesh.GeometryVertices[vertex], _joined.PreparedBoundaries.AsReadOnlySpan() |
| | | 543 | | } |
| | 25618 | 544 | | for (int span = 0; span < member.Boundary.Spans.Length; span++) |
| | | 545 | | { |
| | 10825 | 546 | | var edge = member.Boundary.Spans[span]; |
| | 10825 | 547 | | if (member.JoinedSpans[span] || !MapBoundaryInterval(edge, edge.Interval, start, end, out var mapped)) conti |
| | 2324 | 548 | | Signed832 lowerN = mapped.WideLowerNumerator, lowerD = mapped.WideLowerDenominator; |
| | 2324 | 549 | | Signed832 upperN = mapped.WideUpperNumerator, upperD = mapped.WideUpperDenominator; |
| | 2016 | 550 | | if (!mapped.IsClipped) { lowerN = default; lowerD = upperN = upperD = Signed832.ExtendValue(Signed192.Signed |
| | 1162 | 551 | | int lower = WideTriangleRelations.CompareCoreParameters(numerator, denominator, lowerN, lowerD); |
| | 1162 | 552 | | int upper = WideTriangleRelations.CompareCoreParameters(numerator, denominator, upperN, upperD); |
| | 1162 | 553 | | if (lower < 0 || upper > 0) continue; |
| | 1184 | 554 | | if (lower > 0 && upper < 0) return true; |
| | 34 | 555 | | int coordinate = edge.Start.X != edge.End.X ? 0 : edge.Start.Y != edge.End.Y ? 1 : 2; |
| | 34 | 556 | | bool reverse = (_points[start][coordinate] > _points[end][coordinate]) != (edge.Start[coordinate] > edge.End |
| | 34 | 557 | | return SupportsJoinedFan(_joined.Fan.BuildEndpoint(_joined.Meshes.AsReadOnlySpan(), member.Boundary, span, ( |
| | 34 | 558 | | _joined.PreparedBoundaries.AsReadOnlySpan()), normal); |
| | | 559 | | } |
| | 1984 | 560 | | return false; |
| | | 561 | | } |
| | | 562 | | |
| | | 563 | | public bool ContainsPoint(bool first, int start, int end, scoped ContactQuadratic numerator, |
| | | 564 | | scoped ContactQuadratic denominator, in WideDirection3d normal) |
| | | 565 | | { |
| | 360 | 566 | | if (first) return true; |
| | 120 | 567 | | if (_supportQuery) |
| | | 568 | | { |
| | 2 | 569 | | var clip = _supportClip[0]; |
| | 2 | 570 | | return !clip.IsClipped || clip.Contains(numerator, denominator, ReadOnlySpan<ulong>.Empty); |
| | | 571 | | } |
| | 118 | 572 | | var member = _joined.Members[_member]; |
| | 236 | 573 | | int a = _vertices[start], b = _vertices[end]; |
| | 118 | 574 | | bool atStart = numerator.Signs[0] == 0; |
| | 118 | 575 | | bool atEnd = CompareBorrowedParameter(numerator, denominator, Signed832.ExtendValue(Signed192.One), |
| | 118 | 576 | | Signed832.ExtendValue(Signed192.One)) == 0; |
| | | 577 | | // Borrowed ratios come only from the native edge/edge fallback; |
| | | 578 | | // point/face witnesses use the compact overload above. |
| | 118 | 579 | | if (atStart || atEnd) |
| | | 580 | | { |
| | 21 | 581 | | int vertex = atStart ? a : b; |
| | 21 | 582 | | return SupportsJoinedFan(_joined.Fan.BuildAtPoint(_joined.Meshes.AsReadOnlySpan(), member.Boundary.SourceMem |
| | 21 | 583 | | member.Owner, member.Collider.Mesh.GeometryVertices[vertex], _joined.PreparedBoundaries.AsReadOnlySpan() |
| | | 584 | | } |
| | 97 | 585 | | Signed832 one = Signed832.ExtendValue(Signed192.One); |
| | 916 | 586 | | for (int span = 0; span < member.Boundary.Spans.Length; span++) |
| | | 587 | | { |
| | 394 | 588 | | var edge = member.Boundary.Spans[span]; |
| | 394 | 589 | | if (member.JoinedSpans[span] || !MapBoundaryInterval(edge, edge.Interval, start, end, out var mapped)) conti |
| | 39 | 590 | | int lower = CompareBorrowedParameter(numerator, denominator, |
| | 39 | 591 | | mapped.IsClipped ? mapped.WideLowerNumerator : default, mapped.IsClipped ? mapped.WideLowerDenominator : |
| | 39 | 592 | | int upper = CompareBorrowedParameter(numerator, denominator, |
| | 39 | 593 | | mapped.IsClipped ? mapped.WideUpperNumerator : one, mapped.IsClipped ? mapped.WideUpperDenominator : one |
| | 39 | 594 | | if (lower < 0 || upper > 0) continue; |
| | 57 | 595 | | if (lower > 0 && upper < 0) return true; |
| | 9 | 596 | | int coordinate = edge.Start.X != edge.End.X ? 0 : edge.Start.Y != edge.End.Y ? 1 : 2; |
| | 9 | 597 | | bool reverse = (_points[start][coordinate] > _points[end][coordinate]) != (edge.Start[coordinate] > edge.End |
| | 9 | 598 | | return SupportsJoinedFan(_joined.Fan.BuildEndpoint(_joined.Meshes.AsReadOnlySpan(), member.Boundary, span, |
| | 9 | 599 | | (upper == 0) != reverse, _joined.PreparedBoundaries.AsReadOnlySpan()), normal); |
| | | 600 | | } |
| | 64 | 601 | | return false; |
| | | 602 | | } |
| | | 603 | | |
| | | 604 | | private static int CompareBorrowedParameter(scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 605 | | Signed832 otherN, Signed832 otherD) |
| | | 606 | | { |
| | 392 | 607 | | Span<ulong> values = stackalloc ulong[52]; Span<int> signs = stackalloc int[4]; |
| | 392 | 608 | | var n = ContactQuadratic.At(values, signs, 0, 13); var d = ContactQuadratic.At(values, signs, 1, 13); |
| | 392 | 609 | | n.Set(otherN); d.Set(otherD); |
| | 196 | 610 | | return ContactQuadratic.CompareRatios(numerator, denominator, ReadOnlySpan<ulong>.Empty, |
| | 196 | 611 | | n, d, ReadOnlySpan<ulong>.Empty); |
| | | 612 | | } |
| | | 613 | | |
| | | 614 | | private bool IsFaceNormal(in WideDirection3d normal) |
| | | 615 | | { |
| | 33784 | 616 | | return WideDirection3d.AreParallel(normal, _joined.Members[_member].Frame); |
| | | 617 | | } |
| | | 618 | | |
| | | 619 | | private bool SupportsJoinedFan(bool built, in WideDirection3d normal) |
| | | 620 | | { |
| | | 621 | | // Failed or ineligible neighbor certificates preserve the native feature. |
| | 703 | 622 | | if (!built) return true; |
| | 3568 | 623 | | foreach (var owner in _joined.Fan.Owners) |
| | 1102 | 624 | | if (!_joined.MemberBySurface.ContainsKey((_joined.Meshes[owner.Member], owner.Surface))) return true; |
| | 683 | 625 | | return Supports(_joined.Fan.Rays, normal); |
| | | 626 | | } |
| | | 627 | | |
| | | 628 | | private bool Supports(ReadOnlySpan<WideAxis3> rays, in WideDirection3d normal) |
| | | 629 | | { |
| | 683 | 630 | | WideDirection3d inward = -normal; |
| | 5021 | 631 | | foreach (WideAxis3 ray in rays) |
| | | 632 | | { |
| | 2444 | 633 | | if (WideDirection3d.Dot(inward, ray).Sign > 0) return false; |
| | | 634 | | } |
| | 272 | 635 | | return true; |
| | | 636 | | } |
| | | 637 | | |
| | | 638 | | private static int ComparePoints(Sample a, Sample b) |
| | | 639 | | { |
| | 63520 | 640 | | int order = a.Point.X.CompareTo(b.Point.X); |
| | 98599 | 641 | | if (order == 0) order = a.Point.Y.CompareTo(b.Point.Y); |
| | 98500 | 642 | | if (order == 0) order = a.Point.Z.CompareTo(b.Point.Z); |
| | 63520 | 643 | | return order != 0 ? order : a.Contact.FirstAnchor.CompareLocalFeature(b.Contact.FirstAnchor); |
| | | 644 | | } |
| | | 645 | | private static int CompareSamples(Sample a, Sample b) |
| | 82330 | 646 | | { int order = ComparePoints(a, b); return order != 0 ? order : a.Member.CompareTo(b.Member); } |
| | | 647 | | private readonly struct Sample : IPlaneContactSample |
| | | 648 | | { |
| | | 649 | | internal readonly int Group, Member; |
| | | 650 | | internal readonly FixedContactAnchors Contact; |
| | | 651 | | public Vector3d Point { get; } |
| | | 652 | | internal Sample(int group, int member, Vector3d point, FixedContactAnchors contact) |
| | 320635 | 653 | | { Group = group; Member = member; Point = point; Contact = contact; } |
| | | 654 | | } |
| | | 655 | | private readonly struct ResponseGroup |
| | | 656 | | { |
| | | 657 | | internal readonly int Component, Feature, Carrier; |
| | | 658 | | internal readonly WideDirection3d Axis; |
| | | 659 | | internal readonly NativePenetration Penetration; |
| | | 660 | | internal ResponseGroup(int component, WideDirection3d axis, NativePenetration penetration, int feature, int carr |
| | 58788 | 661 | | { Component = component; Axis = axis; Penetration = penetration; Feature = feature; Carrier = carrier; } |
| | | 662 | | } |
| | | 663 | | |
| | | 664 | | private readonly struct NativePenetration |
| | | 665 | | { |
| | | 666 | | internal readonly WidePointSpanPenetration Rigid; |
| | | 667 | | internal readonly WideScaledHullPenetration Scaled; |
| | | 668 | | internal readonly bool IsScaled; |
| | 79260 | 669 | | internal NativePenetration(WidePointSpanPenetration value) { this = default; Rigid = value; } |
| | 1176 | 670 | | internal NativePenetration(WideScaledHullPenetration value) { this = default; Scaled = value; IsScaled = true; } |
| | 429 | 671 | | internal bool HasValue => IsScaled ? Scaled.HasValue : Rigid.HasValue; |
| | 40919 | 672 | | internal bool Negate => IsScaled ? Scaled.Negate : Rigid.Negate; |
| | 95153 | 673 | | internal WideDirection3d Axis => IsScaled ? Scaled.Axis : new WideDirection3d(Rigid.Axis); |
| | 52 | 674 | | private Signed832 Overlap => IsScaled ? Scaled.Overlap : Signed832.ExtendValue(Rigid.ExactOverlap); |
| | 52 | 675 | | private Signed832 SquaredAxisLength => IsScaled ? Scaled.SquaredAxisLength : Signed832.ExtendValue(Rigid.ExactSq |
| | 52 | 676 | | private Signed576 Denominator => IsScaled ? Scaled.CommonDenominator : Signed576.ExtendValue(Rigid.ExactCommonDe |
| | | 677 | | // Group comparison sees only successful SAT results or constructed |
| | | 678 | | // admission candidates; a filtered empty result is rejected earlier. |
| | 3754 | 679 | | internal bool ShouldReplace(in NativePenetration candidate) => !IsScaled && !candidate.IsScaled |
| | 3754 | 680 | | ? Rigid.ShouldReplace(candidate.Rigid.ExactOverlap, candidate.Rigid.ExactSquaredAxisLength, candidate.Rigid. |
| | 3754 | 681 | | : WideArithmetic.CompareNonNegativeNormalizedDepths(candidate.Overlap, candidate.SquaredAxisLength, candidat |
| | 3754 | 682 | | Overlap, SquaredAxisLength, Denominator) < 0; |
| | | 683 | | } |
| | | 684 | | } |