| | | 1 | | //======================================================================= |
| | | 2 | | // MeshConeSurfaceBoundary.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>Retains finite exposed-feature certificates for one prepared mesh surface.</summary> |
| | | 17 | | internal sealed class MeshConeSurfaceBoundary |
| | | 18 | | { |
| | 10212 | 19 | | private readonly SwiftList<BoundaryCandidate> _isolated = new(); |
| | 10212 | 20 | | private readonly SwiftList<FamilyCandidate> _families = new(); |
| | 10212 | 21 | | private readonly SwiftList<FamilySample> _familySamples = new(); |
| | 10212 | 22 | | private readonly SwiftList<WideAxis3> _halfspaces = new(); |
| | | 23 | | private RationalSegmentInterval _clip; |
| | 10212 | 24 | | private WideAxis3[] _joinedFan = Array.Empty<WideAxis3>(); |
| | 10212 | 25 | | private ConeSurfaceFamilyEvent[] _familyStorage = Array.Empty<ConeSurfaceFamilyEvent>(); |
| | 10212 | 26 | | private readonly SwiftHashSet<int> _visitedCorners = new(); |
| | 10212 | 27 | | private readonly SwiftList<SurfaceSample> _surfacePool = new(); |
| | | 28 | | |
| | 788 | 29 | | internal int IsolatedCount => _isolated.Count; |
| | 6 | 30 | | internal int FamilyCount => _families.Count; |
| | 194 | 31 | | internal int FamilySampleCount => _familySamples.Count; |
| | | 32 | | |
| | | 33 | | internal void Build(PhysicsMesh mesh, int surfaceTriangle, Vector3d coneCenter, |
| | | 34 | | FixedQuaternion coneRotation, Fixed64 height, Fixed64 radius) |
| | | 35 | | { |
| | 164 | 36 | | _isolated.FastClear(); _families.FastClear(); _familySamples.FastClear(); _halfspaces.FastClear(); |
| | 41 | 37 | | ReadOnlySpan<int> boundary = mesh.GetCanonicalSurfaceBoundaryVertexPairs(surfaceTriangle); |
| | 41 | 38 | | ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices; |
| | 41 | 39 | | int owner = mesh.GetManifoldSurfaceOwner(surfaceTriangle); |
| | 41 | 40 | | Span<SegmentConeSurfaceCandidate> storage = stackalloc SegmentConeSurfaceCandidate[SegmentConeSurfaceCandidates. |
| | 362 | 41 | | for (int edge = 0; edge < boundary.Length; edge += 2) |
| | 140 | 42 | | Accumulate(mesh, owner, boundary[edge], boundary[edge + 1], vertices, |
| | 140 | 43 | | coneCenter, coneRotation, height, radius, storage); |
| | 41 | 44 | | } |
| | | 45 | | |
| | | 46 | | /// <summary>Adds independently reduced true-edge and corner constraints to the unpublished manifold.</summary> |
| | | 47 | | internal void BuildSurfaceContacts(LSMeshCollider meshCollider, LSConeCollider cone, int surfaceTriangle, |
| | | 48 | | MeshSurfaceConnectivity connectivity, MeshConeFaceRegions faces, ContactManifold manifold) |
| | | 49 | | { |
| | 372 | 50 | | PhysicsMesh mesh = meshCollider.Mesh; |
| | 372 | 51 | | ConePlaneRayFrame frame = faces.GetBoundaryFrame(meshCollider, surfaceTriangle); |
| | 372 | 52 | | WideDirection3d worldNormal = new WideDirection3d(frame.AuthoredNormal).Transform(mesh.GeometryFrame.GeometryBas |
| | 372 | 53 | | WideAxis3 normal = new(Signed320.NarrowValue(worldNormal.X), Signed320.NarrowValue(worldNormal.Y), Signed320.Nar |
| | 744 | 54 | | int owner = mesh.GetManifoldSurfaceOwner(surfaceTriangle), surface = mesh.GetCanonicalSurfaceOrdinal(surfaceTria |
| | 372 | 55 | | ReadOnlySpan<int> boundary = mesh.GetCanonicalSurfaceBoundaryVertexPairs(surfaceTriangle); |
| | 372 | 56 | | ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices; |
| | 372 | 57 | | _visitedCorners.Clear(); |
| | 372 | 58 | | MeshSurfaceBoundaryDomain? domain = faces.GetJoinedBoundary(meshCollider, surfaceTriangle); |
| | 372 | 59 | | if (domain != null) |
| | | 60 | | { |
| | 426 | 61 | | for (int span = 0; span < domain.Spans.Length; span++) |
| | | 62 | | { |
| | 174 | 63 | | var piece = domain.Spans[span]; |
| | 174 | 64 | | if (faces.IsJoinedSpan(meshCollider, surfaceTriangle, span) || !frame.IntersectsSegment(new FixedSegment |
| | 58 | 65 | | int region = connectivity.RegionCount == 1 ? 0 |
| | 58 | 66 | | : MeshSurfaceBoundaryTopology.FindEdgeRegion(mesh, owner, piece.First, piece.Second, connectivity, f |
| | 58 | 67 | | int feature = checked(3 * span); |
| | 58 | 68 | | RationalSegmentInterval clip = faces.GetRetainedBoundaryInterval(meshCollider, surfaceTriangle, ref span |
| | | 69 | | // An intersecting canonical perimeter belongs to an admitted incident triangle. |
| | | 70 | | System.Diagnostics.Debug.Assert(region >= 0); |
| | 58 | 71 | | KeepSurfaceFeature(piece.First, piece.Second, feature, region, false, clip); |
| | | 72 | | } |
| | | 73 | | // A certified exposed/joined transition lies at an authored |
| | | 74 | | // canonical vertex of an incident member, even across rigid frames. |
| | | 75 | | // Visit those actual vertices; never round a rational cut into one. |
| | 414 | 76 | | for (int edge = 0; edge < boundary.Length; edge += 2) |
| | | 77 | | { |
| | 336 | 78 | | int first = boundary[edge], second = boundary[edge + 1]; |
| | 168 | 79 | | int region = MeshSurfaceBoundaryTopology.FindEdgeRegion(mesh, owner, first, second, connectivity, frame) |
| | 168 | 80 | | if (region < 0) continue; |
| | 84 | 81 | | if (_visitedCorners.Add(first) && frame.ContainsPoint(vertices[first])) |
| | 10 | 82 | | KeepSurfaceFeature(first, second, checked(3 * (domain.Spans.Length + edge / 2) + 1), region, true); |
| | 84 | 83 | | if (_visitedCorners.Add(second) && frame.ContainsPoint(vertices[second])) |
| | 25 | 84 | | KeepSurfaceFeature(second, first, checked(3 * (domain.Spans.Length + edge / 2) + 2), region, true); |
| | | 85 | | } |
| | 39 | 86 | | return; |
| | | 87 | | } |
| | 3272 | 88 | | for (int edge = 0; edge < boundary.Length; edge += 2) |
| | | 89 | | { |
| | 2606 | 90 | | int first = boundary[edge], second = boundary[edge + 1]; |
| | 1303 | 91 | | int feature = checked(3 * (edge / 2)); |
| | 1303 | 92 | | int region = MeshSurfaceBoundaryTopology.FindEdgeRegion(mesh, owner, first, second, connectivity, frame); |
| | 2167 | 93 | | if (region >= 0) KeepSurfaceFeature(first, second, feature, region, pointOnly: false); |
| | 1303 | 94 | | if (_visitedCorners.Add(first) && frame.ContainsPoint(vertices[first])) |
| | | 95 | | { |
| | | 96 | | // A convex cone cuts this straight edge in one connected |
| | | 97 | | // interval. Its admitted authored subedges join through their |
| | | 98 | | // welded endpoints, so this corner shares the edge's region. |
| | | 99 | | System.Diagnostics.Debug.Assert(region >= 0); |
| | 160 | 100 | | KeepSurfaceFeature(first, second, feature + 1, region, pointOnly: true); |
| | | 101 | | } |
| | 1303 | 102 | | if (_visitedCorners.Add(second) && frame.ContainsPoint(vertices[second])) |
| | | 103 | | { |
| | | 104 | | System.Diagnostics.Debug.Assert(region >= 0); |
| | 403 | 105 | | KeepSurfaceFeature(second, first, feature + 2, region, pointOnly: true); |
| | | 106 | | } |
| | | 107 | | } |
| | | 108 | | |
| | | 109 | | void KeepSurfaceFeature(int first, int second, int feature, int region, bool pointOnly, |
| | | 110 | | in RationalSegmentInterval clip = default) |
| | | 111 | | { |
| | | 112 | | bool suppress = false; |
| | | 113 | | ReadOnlySpan<WideAxis3> fan = domain != null && pointOnly |
| | | 114 | | ? faces.GetJoinedCornerFan(meshCollider, surfaceTriangle, first, out suppress) : default; |
| | | 115 | | if (suppress) return; |
| | | 116 | | _isolated.FastClear(); _families.FastClear(); _familySamples.FastClear(); _halfspaces.FastClear(); |
| | | 117 | | WideAxis3 selectedExit = faces.GetBoundaryOrientation(meshCollider, surfaceTriangle, region) > 0 ? normal : |
| | | 118 | | // Use a point segment for each corner exactly once. Adjacent edges |
| | | 119 | | // must not duplicate its support branches or multiply its pressure. |
| | | 120 | | Span<SegmentConeSurfaceCandidate> featureStorage = stackalloc SegmentConeSurfaceCandidate[SegmentConeSurface |
| | | 121 | | Accumulate(mesh, owner, first, second, mesh.GeometryVertices, cone.Center, cone.Rotation, |
| | | 122 | | cone.Height, cone.ScaledRadius, featureStorage, interiorOnly: !pointOnly, pointOnly, selectedExit, clip, |
| | | 123 | | ReduceSurfaceFeature(meshCollider, cone, surface, feature, faces.SamplingOrigin, manifold); |
| | | 124 | | } |
| | 333 | 125 | | } |
| | | 126 | | |
| | | 127 | | private void ReduceSurfaceFeature(LSMeshCollider mesh, LSConeCollider cone, int surface, int feature, |
| | | 128 | | Vector3d samplingOrigin, ContactManifold manifold) |
| | | 129 | | { |
| | 1506 | 130 | | _surfacePool.FastClear(); |
| | 2182 | 131 | | if (_isolated.Count == 0 && _families.Count == 0) return; |
| | 830 | 132 | | SegmentConeSurfaceCandidate minimum = _isolated.Count > 0 |
| | 830 | 133 | | ? _isolated[0].Certificate : _families[0].Candidate.Certificate; |
| | 3140 | 134 | | foreach (BoundaryCandidate candidate in _isolated.AsReadOnlySpan()) |
| | 883 | 135 | | if (SegmentConeSurfaceCandidates.CompareDepths(candidate.Certificate, minimum) < 0) minimum = candidate.Cert |
| | 2678 | 136 | | foreach (FamilyCandidate family in _families.AsReadOnlySpan()) |
| | 746 | 137 | | if (SegmentConeSurfaceCandidates.CompareDepths(family.Candidate.Certificate, minimum) < 0) minimum = family. |
| | | 138 | | // A feature's support strata are alternative exits, not simultaneous |
| | | 139 | | // constraints. Retain its exact lower envelope before rounding; a far |
| | | 140 | | // apex exit cannot depenetrate a touching or barely penetrating side. |
| | | 141 | | // Clipped families retain their source certificate's constant depth. |
| | 830 | 142 | | Span<bool> coMinimalFamilies = stackalloc bool[SegmentConeSurfaceCandidates.MaximumCandidates]; |
| | | 143 | | System.Diagnostics.Debug.Assert(_families.Count <= coMinimalFamilies.Length); |
| | 2678 | 144 | | for (int index = 0; index < _families.Count; index++) |
| | 509 | 145 | | coMinimalFamilies[index] = SegmentConeSurfaceCandidates.CompareDepths(_families[index].Candidate.Certificate |
| | 3140 | 146 | | for (int index = 0; index < _isolated.Count; index++) |
| | 1093 | 147 | | if (SegmentConeSurfaceCandidates.CompareDepths(_isolated[index].Certificate, minimum) == 0) Keep(GetIsolated |
| | 3118 | 148 | | for (int index = 0; index < _familySamples.Count; index++) |
| | 1412 | 149 | | if (coMinimalFamilies[_familySamples[index].Family]) Keep(GetFamilySampleContact(index)); |
| | | 150 | | // Each retained family emitted at least one admitted sample. A minimum |
| | | 151 | | // family or isolated certificate therefore seeds this pool before any |
| | | 152 | | // duplicate can be discarded. |
| | | 153 | | System.Diagnostics.Debug.Assert(_surfacePool.Count > 0); |
| | 830 | 154 | | Span<int> chosen = stackalloc int[ContactManifold.MaxContactsPerGroup]; |
| | | 155 | | // All survivors have the same exact depth. Their canonical construction |
| | | 156 | | // order supplies the first sample; coverage chooses the remaining three. |
| | 830 | 157 | | chosen[0] = 0; |
| | 830 | 158 | | int count = Math.Min(ContactManifold.MaxContactsPerGroup, _surfacePool.Count); |
| | 1892 | 159 | | for (int slot = 1; slot < count; slot++) |
| | | 160 | | { |
| | 232 | 161 | | int best = -1, bestNearest = 0; |
| | 824 | 162 | | for (int candidate = 0; candidate < _surfacePool.Count; candidate++) |
| | | 163 | | { |
| | 296 | 164 | | if (chosen[..slot].IndexOf(candidate) >= 0) continue; |
| | 151 | 165 | | int nearest = chosen[0]; |
| | 376 | 166 | | for (int prior = 1; prior < slot; prior++) |
| | 41 | 167 | | if (CompareCoverage(candidate, chosen[prior], candidate, nearest) < 0) nearest = chosen[prior]; |
| | 151 | 168 | | if (best < 0 || CompareCoverage(candidate, nearest, best, bestNearest) > 0) |
| | 286 | 169 | | { best = candidate; bestNearest = nearest; } |
| | | 170 | | } |
| | 116 | 171 | | chosen[slot] = best; |
| | | 172 | | } |
| | | 173 | | // Negative feature ordinals are disjoint from nonnegative face regions. |
| | | 174 | | // Four samples approximate this feature's distributed response. Final |
| | | 175 | | // rounded coverage never feeds back into exact admission or depth selection. |
| | | 176 | | // Coverage ranks only the admitted finite pool: normal spread first |
| | | 177 | | // (a touch family can share both anchors), then mesh/cone anchor spread. |
| | 830 | 178 | | bool joined = mesh.StaticSurface != null; |
| | 830 | 179 | | var key = new ContactGroupKey(0, 0, surface, 0, checked(-feature - 1), |
| | 830 | 180 | | sourceIdA: joined ? mesh.Id + 1 : 0, |
| | 830 | 181 | | sourceOrderA: joined ? ((IPhysicsColliderRegistryItem)mesh).ReplayOrder + 1 : 0); |
| | 3552 | 182 | | for (int slot = 0; slot < count; slot++) |
| | | 183 | | { |
| | 946 | 184 | | FixedContactAnchors contact = _surfacePool[chosen[slot]].Contact; |
| | 946 | 185 | | manifold.AddContact(new ContactAnchor(contact.FirstAnchor), new ContactAnchor(contact.SecondAnchor), |
| | 946 | 186 | | contact.Depth, contact.Normal, mesh.Material, cone.Material, contact.DepthIsClamped, |
| | 946 | 187 | | group: key, sampleIdentity: slot + 1); |
| | | 188 | | } |
| | | 189 | | |
| | | 190 | | void Keep(FixedContactAnchors contact) |
| | | 191 | | { |
| | | 192 | | if (!contact.FirstAnchor.TryGetPointInFrame(samplingOrigin, out Vector3d p) |
| | | 193 | | || !contact.SecondAnchor.TryGetPointInFrame(samplingOrigin, out Vector3d q)) |
| | | 194 | | throw new OverflowException("An admitted boundary sample is outside the Fixed64 sampling range."); |
| | | 195 | | // Distinct exact constructions can round to the same solver row. |
| | | 196 | | // Collapse that final redundancy after admission; different normals |
| | | 197 | | // at coincident touch anchors remain independent samples. |
| | | 198 | | for (int index = 0; index < _surfacePool.Count; index++) |
| | | 199 | | { |
| | | 200 | | SurfaceSample existing = _surfacePool[index]; |
| | | 201 | | if (existing.MeshPoint != p || existing.ConePoint != q || existing.Contact.Normal != contact.Normal) con |
| | | 202 | | return; |
| | | 203 | | } |
| | | 204 | | _surfacePool.Add(new SurfaceSample(contact, p, q)); |
| | | 205 | | } |
| | 830 | 206 | | } |
| | | 207 | | |
| | | 208 | | private int CompareCoverage(int first, int firstReference, int second, int secondReference) |
| | | 209 | | { |
| | 144 | 210 | | SurfaceSample a = _surfacePool[first], ar = _surfacePool[firstReference]; |
| | 144 | 211 | | SurfaceSample b = _surfacePool[second], br = _surfacePool[secondReference]; |
| | 72 | 212 | | int comparison = Vector3d.CompareDistanceSquared(a.Contact.Normal, ar.Contact.Normal, b.Contact.Normal, br.Conta |
| | 114 | 213 | | if (comparison == 0) comparison = Vector3d.CompareDistanceSquared(a.MeshPoint, ar.MeshPoint, b.MeshPoint, br.Mes |
| | 72 | 214 | | return comparison == 0 ? Vector3d.CompareDistanceSquared(a.ConePoint, ar.ConePoint, b.ConePoint, br.ConePoint) : |
| | | 215 | | } |
| | | 216 | | |
| | 1069 | 217 | | internal FixedContactAnchors GetIsolatedContact(int index) => _isolated[index].Certificate.GetContact(); |
| | | 218 | | |
| | | 219 | | internal int GetIsolatedVertex(int index) |
| | | 220 | | { |
| | 94 | 221 | | BoundaryCandidate candidate = _isolated[index]; |
| | 94 | 222 | | return candidate.Certificate.PointLocation == ConeSurfacePointLocation.Start ? candidate.First |
| | 94 | 223 | | : candidate.Certificate.PointLocation == ConeSurfacePointLocation.End ? candidate.Second : -1; |
| | | 224 | | } |
| | | 225 | | |
| | | 226 | | private void Accumulate(PhysicsMesh mesh, int owner, int first, int second, ReadOnlySpan<Vector3d> vertices, |
| | | 227 | | Vector3d coneCenter, FixedQuaternion coneRotation, Fixed64 height, Fixed64 radius, |
| | | 228 | | Span<SegmentConeSurfaceCandidate> storage, bool interiorOnly = false, bool pointOnly = false, WideAxis3? selecte |
| | | 229 | | in RationalSegmentInterval clip = default, ReadOnlySpan<WideAxis3> joinedFan = default) |
| | | 230 | | { |
| | | 231 | | // The boundary owner reduces one clipped feature before visiting the next. |
| | 1646 | 232 | | _clip = clip; |
| | 1646 | 233 | | var selection = new ConeSurfaceSelection(storage); |
| | 1646 | 234 | | SegmentConeSurfaceCandidates.AccumulateSegmentConeSurfaceCandidates( |
| | 1646 | 235 | | new FixedSegment(vertices[first], vertices[pointOnly ? first : second]), mesh.GeometryFrame, |
| | 1646 | 236 | | coneCenter, coneRotation, height, radius, ref selection); |
| | 23928 | 237 | | for (int index = 0; index < selection.Count; index++) |
| | | 238 | | { |
| | 10318 | 239 | | SegmentConeSurfaceCandidate certificate = selection[index]; |
| | | 240 | | // Both endpoints of a point segment are the same corner. Retain |
| | | 241 | | // its construction, but use Start for the real incident-edge fan. |
| | 10318 | 242 | | if (pointOnly && certificate.PointLocation != ConeSurfacePointLocation.Start) |
| | 584 | 243 | | certificate = new SegmentConeSurfaceCandidate(certificate.Input, certificate.Feature, certificate.Family |
| | 584 | 244 | | certificate.Chart, certificate.RootOrdinal, ConeSurfacePointLocation.Start); |
| | 10318 | 245 | | var candidate = new BoundaryCandidate(first, second, certificate); |
| | 10318 | 246 | | if (clip.IsClipped && certificate.Family != ConeSurfaceFamily.SegmentInterval |
| | 10318 | 247 | | && !certificate.IsInInterval(clip, interiorOnly)) continue; |
| | 10298 | 248 | | if (interiorOnly && candidate.Certificate.Family != ConeSurfaceFamily.SegmentInterval |
| | 10298 | 249 | | && candidate.Certificate.PointLocation != ConeSurfacePointLocation.Interior) continue; |
| | 7128 | 250 | | if (candidate.Certificate.Family == ConeSurfaceFamily.None) |
| | | 251 | | { |
| | 7366 | 252 | | if (SupportsIncidentFan(mesh, owner, candidate, vertices, selectedExit, joinedFan)) _isolated.Add(candid |
| | | 253 | | } |
| | 1403 | 254 | | else if (candidate.Certificate.Family == ConeSurfaceFamily.SegmentInterval) |
| | | 255 | | { |
| | 2032 | 256 | | if (!interiorOnly) KeepFamily(mesh, owner, candidate, vertices, ConeSurfacePointLocation.Start, selected |
| | 2066 | 257 | | if (!pointOnly) KeepFamily(mesh, owner, candidate, vertices, ConeSurfacePointLocation.Interior, selected |
| | 1448 | 258 | | if (!interiorOnly && !pointOnly) KeepFamily(mesh, owner, candidate, vertices, ConeSurfacePointLocation.E |
| | | 259 | | } |
| | 78 | 260 | | else KeepFamily(mesh, owner, candidate, vertices, candidate.Certificate.PointLocation, selectedExit, clip, j |
| | | 261 | | } |
| | 1646 | 262 | | } |
| | | 263 | | |
| | | 264 | | internal FixedContactAnchors GetFamilySampleContact(int index) |
| | | 265 | | { |
| | 774 | 266 | | FamilySample sample = _familySamples[index]; |
| | 772 | 267 | | FamilyCandidate family = _families[sample.Family]; |
| | 772 | 268 | | return sample.Event.GetContact(family.Candidate.Certificate, |
| | 772 | 269 | | _halfspaces.AsReadOnlySpan().Slice(family.Offset, family.Count), _clip); |
| | | 270 | | } |
| | | 271 | | |
| | | 272 | | internal bool TryGetFamilySampleWorldAnchors(int index, out Vector3d first, out Vector3d second) |
| | | 273 | | { |
| | 117 | 274 | | first = second = default; |
| | 121 | 275 | | if ((uint)index >= (uint)_familySamples.Count) return false; |
| | 113 | 276 | | FamilySample sample = _familySamples[index]; |
| | 113 | 277 | | FamilyCandidate family = _families[sample.Family]; |
| | 113 | 278 | | return sample.Event.TryGetWorldAnchors(family.Candidate.Certificate, |
| | 113 | 279 | | _halfspaces.AsReadOnlySpan().Slice(family.Offset, family.Count), _clip, out first, out second); |
| | | 280 | | } |
| | | 281 | | |
| | | 282 | | private void KeepFamily(PhysicsMesh mesh, int owner, BoundaryCandidate candidate, |
| | | 283 | | ReadOnlySpan<Vector3d> vertices, ConeSurfacePointLocation location, WideAxis3? selectedExit = null, |
| | | 284 | | in RationalSegmentInterval clip = default, ReadOnlySpan<WideAxis3> joinedFan = default) |
| | | 285 | | { |
| | 1662 | 286 | | if (!joinedFan.IsEmpty && _joinedFan.Length < joinedFan.Length + 1) Array.Resize(ref _joinedFan, joinedFan.Lengt |
| | 1649 | 287 | | Span<WideAxis3> fan = joinedFan.IsEmpty ? stackalloc WideAxis3[5] : _joinedFan; |
| | | 288 | | int count; |
| | 3255 | 289 | | if (joinedFan.IsEmpty) count = MeshSurfaceBoundaryTopology.GetIncidentFan(mesh, owner, |
| | 1606 | 290 | | location == ConeSurfacePointLocation.End ? candidate.Second : candidate.First, |
| | 1606 | 291 | | location == ConeSurfacePointLocation.End ? candidate.First : candidate.Second, |
| | 1606 | 292 | | location == ConeSurfacePointLocation.Interior, fan); |
| | 86 | 293 | | else { joinedFan.CopyTo(fan); count = joinedFan.Length; } |
| | 1649 | 294 | | if (joinedFan.IsEmpty) |
| | 15353 | 295 | | for (int index = 0; index < count; index++) fan[index] = mesh.GeometryFrame.GetWorldDirectionNumerators(fan[ |
| | 2909 | 296 | | if (selectedExit.HasValue) fan[count++] = selectedExit.Value; |
| | 1649 | 297 | | int capacity = candidate.Certificate.GetMaximumFamilyEventCount(count); |
| | 1752 | 298 | | if (_familyStorage.Length < capacity) Array.Resize(ref _familyStorage, capacity); |
| | 1649 | 299 | | var selection = new ConeSurfaceFamilySelection(_familyStorage); |
| | 2444 | 300 | | if (!candidate.Certificate.AccumulateFamilyEvents(fan[..count], location, clip, ref selection)) return; |
| | 854 | 301 | | int family = _families.Count; |
| | 854 | 302 | | _families.Add(new FamilyCandidate(candidate, _halfspaces.Count, count)); |
| | 854 | 303 | | _halfspaces.AddRange(fan[..count]); |
| | 3992 | 304 | | for (int index = 0; index < selection.Count; index++) |
| | 1142 | 305 | | _familySamples.Add(new FamilySample(family, selection[index])); |
| | 854 | 306 | | } |
| | | 307 | | |
| | | 308 | | private static bool SupportsIncidentFan(PhysicsMesh mesh, int owner, BoundaryCandidate candidate, |
| | | 309 | | ReadOnlySpan<Vector3d> vertices, WideAxis3? selectedExit = null, ReadOnlySpan<WideAxis3> joinedFan = default) |
| | | 310 | | { |
| | 5725 | 311 | | Span<WideAxis3> localFan = stackalloc WideAxis3[5]; |
| | 5725 | 312 | | int count = joinedFan.IsEmpty ? MeshSurfaceBoundaryTopology.GetIncidentFan(mesh, owner, |
| | 5725 | 313 | | candidate.Certificate.PointLocation == ConeSurfacePointLocation.End ? candidate.Second : candidate.First, |
| | 5725 | 314 | | candidate.Certificate.PointLocation == ConeSurfacePointLocation.End ? candidate.First : candidate.Second, |
| | 5725 | 315 | | candidate.Certificate.PointLocation == ConeSurfacePointLocation.Interior, localFan) : joinedFan.Length; |
| | 5725 | 316 | | if (joinedFan.IsEmpty) |
| | 52739 | 317 | | for (int index = 0; index < count; index++) localFan[index] = mesh.GeometryFrame.GetWorldDirectionNumerators |
| | 5725 | 318 | | if (selectedExit.HasValue) |
| | | 319 | | { |
| | 4580 | 320 | | bool hasExit = candidate.Certificate.TryGetNormalDotSign(selectedExit.Value, out int exitSign); |
| | | 321 | | System.Diagnostics.Debug.Assert(hasExit); |
| | 7002 | 322 | | if (exitSign > 0) return false; |
| | | 323 | | } |
| | 3303 | 324 | | ReadOnlySpan<WideAxis3> fan = joinedFan.IsEmpty ? localFan[..count] : joinedFan; |
| | 19614 | 325 | | foreach (WideAxis3 ray in fan) |
| | | 326 | | { |
| | 7335 | 327 | | bool hasNormal = candidate.Certificate.TryGetNormalDotSign(ray, out int sign); |
| | | 328 | | System.Diagnostics.Debug.Assert(hasNormal); |
| | 8997 | 329 | | if (sign > 0) return false; |
| | | 330 | | } |
| | 1641 | 331 | | return true; |
| | | 332 | | } |
| | | 333 | | |
| | 489 | 334 | | private static WideAxis3 Negate(WideAxis3 value) => new( |
| | 489 | 335 | | WideArithmetic.Negate(value.X), WideArithmetic.Negate(value.Y), WideArithmetic.Negate(value.Z)); |
| | | 336 | | |
| | | 337 | | private readonly struct FamilyCandidate |
| | | 338 | | { |
| | | 339 | | internal readonly BoundaryCandidate Candidate; |
| | | 340 | | internal readonly int Offset, Count; |
| | | 341 | | internal FamilyCandidate(BoundaryCandidate candidate, int offset, int count) |
| | 3416 | 342 | | { Candidate = candidate; Offset = offset; Count = count; } |
| | | 343 | | } |
| | | 344 | | |
| | | 345 | | private readonly struct FamilySample |
| | | 346 | | { |
| | | 347 | | internal readonly int Family; |
| | | 348 | | internal readonly ConeSurfaceFamilyEvent Event; |
| | 3426 | 349 | | internal FamilySample(int family, ConeSurfaceFamilyEvent descriptor) { Family = family; Event = descriptor; } |
| | | 350 | | } |
| | | 351 | | |
| | | 352 | | private readonly struct BoundaryCandidate |
| | | 353 | | { |
| | | 354 | | internal readonly int First, Second; |
| | | 355 | | internal readonly SegmentConeSurfaceCandidate Certificate; |
| | | 356 | | internal BoundaryCandidate(int first, int second, SegmentConeSurfaceCandidate certificate) |
| | | 357 | | { |
| | 30954 | 358 | | First = first; Second = second; Certificate = certificate; |
| | 10318 | 359 | | } |
| | | 360 | | } |
| | | 361 | | |
| | | 362 | | private readonly struct SurfaceSample |
| | | 363 | | { |
| | | 364 | | internal readonly FixedContactAnchors Contact; |
| | | 365 | | internal readonly Vector3d MeshPoint, ConePoint; |
| | | 366 | | internal SurfaceSample(FixedContactAnchors contact, Vector3d meshPoint, Vector3d conePoint) |
| | 3792 | 367 | | { Contact = contact; MeshPoint = meshPoint; ConePoint = conePoint; } |
| | | 368 | | } |
| | | 369 | | } |