| | | 1 | | //======================================================================= |
| | | 2 | | // MeshCapsuleFaceRegions.Joined.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 | | using JoinedMember = Gravitas.CollisionHandling.Member<FixedMathSharp.Geometry.CapsulePlaneRayFrame>; |
| | | 16 | | |
| | | 17 | | namespace Gravitas.CollisionHandling; |
| | | 18 | | |
| | | 19 | | /// <content>Finite capsule sections across explicitly supplied static surface members.</content> |
| | | 20 | | internal sealed partial class MeshCapsuleFaceRegions |
| | | 21 | | { |
| | | 22 | | private JoinedState? _joined; |
| | | 23 | | |
| | | 24 | | internal void ReleaseJoined() |
| | | 25 | | { |
| | 1002 | 26 | | _joined = null; |
| | 1002 | 27 | | RegionCount = 0; |
| | 1002 | 28 | | } |
| | | 29 | | |
| | | 30 | | /// <summary>Prepares unique committed surface owners from bounded input; membership/filter admission belongs to the |
| | | 31 | | internal void BuildJoined(ReadOnlySpan<(LSMeshCollider Collider, int Triangle)> surfaces, |
| | | 32 | | Vector3d center, FixedQuaternion rotation, Fixed64 coreLength, Fixed64 radius, |
| | | 33 | | ReadOnlySpan<PhysicsMesh> preparedMeshes = default, |
| | | 34 | | SwiftDictionary<(PhysicsMesh, int), MeshSurfaceBoundaryDomain>? prepared = null) |
| | | 35 | | { |
| | 6425 | 36 | | JoinedState state = _joined ??= new JoinedState(this); |
| | 6425 | 37 | | state.Prepare(surfaces, preparedMeshes, prepared); |
| | 12850 | 38 | | _center = center; _rotation = coreLength == Fixed64.Zero ? FixedQuaternion.Identity : rotation; |
| | 6425 | 39 | | FixedBoundBox box = FixedBoundBox.FromCenteredCapsuleClippedToDomain(center, _rotation, Vector3d.Up, coreLength, |
| | 6425 | 40 | | var bounds = new FixedBoundVolume(box.Min, box.Max); |
| | 38758 | 41 | | for (int index = 0; index < state.Inputs.Count; index++) |
| | | 42 | | { |
| | 12954 | 43 | | JoinedMember member = state.Members[index]; |
| | 12954 | 44 | | PhysicsMesh mesh = member.Collider.Mesh; |
| | 12954 | 45 | | var finite = new CapsuleFiniteSectionFrame(mesh.GeometryFrame, center, _rotation, coreLength, radius); |
| | 12954 | 46 | | mesh.GetGeometryTriangleVertices(member.Triangle, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 12954 | 47 | | member.Frame = new CapsulePlaneRayFrame(new FixedTriangle(a, b, c), mesh.GeometryFrame, finite); |
| | 12954 | 48 | | member.Admitted.FastClear(); |
| | 12954 | 49 | | member.Collider.GetTrianglesInBounds(bounds, state.Candidates); |
| | 86118 | 50 | | foreach (int triangle in state.Candidates.AsReadOnlySpan()) |
| | | 51 | | { |
| | 30105 | 52 | | if (mesh.GetManifoldSurfaceOwner(triangle) != member.Owner) continue; |
| | 30043 | 53 | | mesh.GetGeometryTriangleVertices(triangle, out a, out b, out c); |
| | 39244 | 54 | | if (CapsulePlaneRayEvents.IntersectsTriangle(finite, new FixedTriangle(a, b, c))) member.Admitted.Add(tr |
| | | 55 | | } |
| | 12954 | 56 | | member.Connectivity.BuildCapsule(mesh, member.Triangle, finite, member.Admitted.AsReadOnlySpan()); |
| | 12954 | 57 | | state.AddRegions(index); |
| | | 58 | | } |
| | 6425 | 59 | | bool wideBanks = false; |
| | 38758 | 60 | | foreach (JoinedMember member in state.Members.AsReadOnlySpan()[..state.Inputs.Count]) |
| | | 61 | | { |
| | 12954 | 62 | | wideBanks |= member.Frame.UsesWideBanks; |
| | 185994 | 63 | | foreach (var span in member.Boundary.Spans) wideBanks |= span.Interval.HasWideBounds; |
| | | 64 | | } |
| | 6425 | 65 | | if (wideBanks) |
| | 142 | 66 | | foreach (JoinedMember member in state.Members.AsReadOnlySpan()[..state.Inputs.Count]) |
| | 48 | 67 | | member.Frame = member.Frame.WithWideParameters(); |
| | 6425 | 68 | | SelectBanks(state.Members[0].Frame); |
| | 6425 | 69 | | JoinFiniteSpans(state); |
| | 6425 | 70 | | state.CompleteRegions(); |
| | 12850 | 71 | | RegionCount = state.Roots.Count; HasBoundaryIntersection = false; |
| | 6425 | 72 | | Reserve(RegionCount); |
| | 6554 | 73 | | if (state.BoundaryHits.Length < RegionCount) Array.Resize(ref state.BoundaryHits, RegionCount); |
| | 6554 | 74 | | if (state.Trusted.Length < RegionCount) Array.Resize(ref state.Trusted, RegionCount); |
| | 12850 | 75 | | Array.Clear(state.BoundaryHits, 0, RegionCount); Array.Fill(state.Trusted, true, 0, RegionCount); |
| | 38758 | 76 | | foreach (JoinedMember member in state.Members.AsReadOnlySpan()[..state.Inputs.Count]) |
| | | 77 | | { |
| | 12954 | 78 | | if (member.BoundaryTrusted) continue; |
| | 140 | 79 | | for (int local = 0; local < member.Connectivity.RegionCount; local++) |
| | 27 | 80 | | state.Trusted[state.RegionByRoot[state.Find(member.Offset + local)]] = false; |
| | | 81 | | } |
| | 6554 | 82 | | if (state.Maximum.Length < 2 * RegionCount) Array.Resize(ref state.Maximum, 2 * RegionCount); |
| | 6554 | 83 | | if (state.MaximumPoint.Length < 2 * RegionCount) Array.Resize(ref state.MaximumPoint, 2 * RegionCount); |
| | 6425 | 84 | | Array.Clear(_winners, 0, 2 * RegionCount); |
| | 25700 | 85 | | _events.FastClear(); _points.FastClear(); _pointWordCount = 0; state.PointSources.FastClear(); |
| | 6425 | 86 | | if (!TryGatherContainedJoinedSupport(state)) |
| | | 87 | | { |
| | | 88 | | // The intrinsic plane is shared by the whole joined component; |
| | | 89 | | // finite admission below still selects each actual supplying owner. |
| | 952 | 90 | | for (int region = 0; region < RegionCount; region++) |
| | | 91 | | { |
| | 520 | 92 | | state.CurrentRegion = region; state.CurrentMember = state.NodeMembers[state.Roots[region]]; |
| | 260 | 93 | | _frame = state.Members[state.CurrentMember].Frame; |
| | 260 | 94 | | VisitJoinedSource(state, CapsulePlaneRayEventSource.Plane, default); |
| | | 95 | | } |
| | 216 | 96 | | state.CurrentRegion = -1; |
| | 1176 | 97 | | for (int member = 0; member < state.Inputs.Count; member++) |
| | | 98 | | { |
| | 372 | 99 | | if (state.Members[member].Connectivity.RegionCount == 0) continue; |
| | 341 | 100 | | state.CurrentMember = member; |
| | 682 | 101 | | JoinedMember current = state.Members[member]; _frame = current.Frame; |
| | 341 | 102 | | ReadOnlySpan<BoundarySpan3D> spans = current.Boundary.Spans; |
| | 3642 | 103 | | for (int span = 0; span < spans.Length; span++) |
| | | 104 | | { |
| | 1480 | 105 | | if (current.JoinedSpans[span] || !_frame.Finite.IntersectsSegment(new FixedSegment(spans[span].Start |
| | 383 | 106 | | VisitJoinedSource(state, new CapsulePlaneRayEventSource(new FixedSegment(spans[span].Start, spans[sp |
| | | 107 | | } |
| | | 108 | | } |
| | | 109 | | } |
| | 32257 | 110 | | for (int region = 0; region < RegionCount; region++) ReduceJoinedSamples(state, region); |
| | 6425 | 111 | | state.Commit(); |
| | 6425 | 112 | | } |
| | | 113 | | |
| | | 114 | | internal CapsulePlaneRayFrame GetBoundaryFrame(LSMeshCollider collider, int triangle, out Vector3d normal) |
| | | 115 | | { |
| | 1188 | 116 | | if (_joined?.Active != true) { normal = _normal; return _frame; } |
| | 66 | 117 | | var frame = _joined.Members[_joined.MemberBySurface[(collider.Mesh, collider.Mesh.GetManifoldSurfaceOwner(triang |
| | 66 | 118 | | normal = frame.GetWorldNormal(collider.Mesh.Rotation); |
| | 66 | 119 | | return frame; |
| | | 120 | | } |
| | | 121 | | |
| | | 122 | | internal int GetBoundaryOrientation(LSMeshCollider collider, int triangle, int localRegion) |
| | | 123 | | { |
| | 15245 | 124 | | if (_joined?.Active != true) return GetOrientation(localRegion); |
| | 287 | 125 | | JoinedState state = _joined; |
| | 287 | 126 | | JoinedMember member = state.Members[state.MemberBySurface[(collider.Mesh, collider.Mesh.GetManifoldSurfaceOwner( |
| | 287 | 127 | | int region = state.RegionByRoot[state.Find(member.Offset + localRegion)]; |
| | 287 | 128 | | return WideDirection3d.Dot(member.Frame.Normal, GetJoinedRegionFrame(state, region).Normal).Sign * _orientations |
| | | 129 | | } |
| | | 130 | | |
| | | 131 | | internal MeshSurfaceBoundaryDomain? GetJoinedBoundary(LSMeshCollider collider, int triangle) => |
| | 440 | 132 | | _joined?.Active == true |
| | 440 | 133 | | ? _joined.Members[_joined.MemberBySurface[(collider.Mesh, collider.Mesh.GetManifoldSurfaceOwner(triangle))]] |
| | | 134 | | |
| | | 135 | | internal bool IsJoinedSpan(LSMeshCollider collider, int triangle, int span) => |
| | 145 | 136 | | _joined!.Members[_joined.MemberBySurface[(collider.Mesh, collider.Mesh.GetManifoldSurfaceOwner(triangle))]].Join |
| | | 137 | | |
| | | 138 | | internal ReadOnlySpan<WideAxis3> GetJoinedEndpointFan(LSMeshCollider collider, int triangle, int span, bool upper, o |
| | | 139 | | { |
| | 76 | 140 | | return _joined!.GetEndpointFan(collider, triangle, span, upper, out suppress); |
| | | 141 | | } |
| | | 142 | | |
| | | 143 | | internal bool AppendJoinedBoundaries(LSMeshCollider collider, Gravitas.Materials.PhysicsMaterial material, |
| | | 144 | | Vector3d center, FixedQuaternion rotation, Fixed64 coreLength, Fixed64 radius, |
| | | 145 | | MeshCapsuleSurfaceBoundary boundary, ContactManifold manifold) |
| | | 146 | | { |
| | 13192 | 147 | | bool overlaps = false; |
| | 13192 | 148 | | JoinedState state = _joined!; |
| | 80182 | 149 | | foreach (JoinedMember member in state.Members.AsReadOnlySpan()[..state.Inputs.Count]) |
| | | 150 | | { |
| | 26899 | 151 | | if (member.Collider != collider || member.Connectivity.RegionCount == 0) continue; |
| | 7883 | 152 | | overlaps = true; |
| | 7883 | 153 | | if (HasBoundaryIntersection) |
| | 66 | 154 | | boundary.BuildSurfaceContacts(collider, material, center, rotation, coreLength, radius, |
| | 66 | 155 | | member.Triangle, member.Connectivity, this, manifold); |
| | | 156 | | } |
| | 13192 | 157 | | return overlaps; |
| | | 158 | | } |
| | | 159 | | |
| | | 160 | | private bool TryGatherContainedJoinedSupport(JoinedState state) |
| | | 161 | | { |
| | 6509 | 162 | | if (RegionCount != 1 || !state.Trusted[0]) return false; |
| | 6341 | 163 | | ref readonly CapsulePlaneRayFrame common = ref GetJoinedRegionFrame(state, 0); |
| | 6341 | 164 | | if (common.Finite.ShapeFrame.Radius.Sign <= 0 || common.Finite.Length != Fixed64.Zero |
| | 6351 | 165 | | && (!common.Normal.X.IsZero || !common.Normal.Z.IsZero)) return false; |
| | | 166 | | // Only the complete retained perimeter certifies containment. Testing |
| | | 167 | | // full original edges is conservative when a retained span is clipped. |
| | 37952 | 168 | | foreach (JoinedMember member in state.Members.AsReadOnlySpan()[..state.Inputs.Count]) |
| | | 169 | | { |
| | 12706 | 170 | | if (member.Connectivity.RegionCount == 0) continue; |
| | 7596 | 171 | | ReadOnlySpan<BoundarySpan3D> spans = member.Boundary.Spans; |
| | 77586 | 172 | | for (int span = 0; span < spans.Length; span++) |
| | 31319 | 173 | | if (!member.JoinedSpans[span] && member.Frame.Finite.IntersectsSegment(new FixedSegment(spans[span].Star |
| | 122 | 174 | | return false; |
| | | 175 | | } |
| | 6209 | 176 | | Span<ulong> values = stackalloc ulong[SelectionWords]; |
| | 6209 | 177 | | Span<int> signs = stackalloc int[CapsulePlaneRaySelection.SignCount]; |
| | 6209 | 178 | | Span<sbyte> valueSigns = stackalloc sbyte[CapsulePlaneRaySelection.ValueSignCount]; |
| | 18627 | 179 | | state.CurrentRegion = 0; state.CurrentMember = state.NodeMembers[state.Roots[0]]; state.CurrentClip = default; |
| | 6209 | 180 | | _frame = state.Members[state.CurrentMember].Frame; |
| | 37254 | 181 | | for (int orientation = 1; orientation >= -1; orientation -= 2) |
| | | 182 | | { |
| | 12418 | 183 | | var support = new CapsulePlaneRaySelection(values, signs, valueSigns, _fieldWords, _rootWords, _valueWords); |
| | 12418 | 184 | | int endpoint = _frame.Finite.Length == Fixed64.Zero ? 0 : orientation * _frame.Normal.Y.Sign > 0 ? 1 : 0; |
| | 12418 | 185 | | var descriptor = new CapsulePlaneRayEvent(CapsulePlaneRayEventKind.Support, endpoint); |
| | 12418 | 186 | | bool found = CapsulePlaneRayEvents.Reconstruct(CapsulePlaneRayEventSource.Plane, _frame, descriptor, orienta |
| | | 187 | | System.Diagnostics.Debug.Assert(found); |
| | 12418 | 188 | | AdmitJoinedPoint(CapsulePlaneRayEventSource.Plane, _frame, descriptor, orientation, support); |
| | | 189 | | } |
| | 6209 | 190 | | return true; |
| | | 191 | | } |
| | | 192 | | |
| | | 193 | | private void JoinFiniteSpans(JoinedState state) |
| | | 194 | | { |
| | 38758 | 195 | | for (int member = 0; member < state.Inputs.Count; member++) |
| | | 196 | | { |
| | 12954 | 197 | | JoinedMember source = state.Members[member]; |
| | 12954 | 198 | | ReadOnlySpan<BoundarySpan3D> spans = source.Boundary.Spans; |
| | 132632 | 199 | | for (int span = 0; span < spans.Length; span++) |
| | | 200 | | { |
| | 53362 | 201 | | var edge = spans[span]; |
| | 53362 | 202 | | if (edge.Kind != SpanKind.Joined) continue; |
| | 18130 | 203 | | if (!state.MemberBySurface.TryGetValue((state.Meshes[edge.OtherMember], edge.OtherSurface), out int othe |
| | 12924 | 204 | | JoinedMember target = state.Members[other]; |
| | 12924 | 205 | | if (!edge.Interval.IsClipped && source.Connectivity.RegionCount == 1 && target.Connectivity.RegionCount |
| | | 206 | | { |
| | | 207 | | // A complete certified shared edge meets the convex capsule |
| | | 208 | | // in one closed interval belonging to both unique regions. |
| | 2503 | 209 | | if (source.Frame.Finite.IntersectsSegment(new FixedSegment(edge.Start, edge.End))) |
| | | 210 | | { |
| | 2501 | 211 | | state.Union(source.Offset, target.Offset); |
| | 2501 | 212 | | source.JoinedSpans[span] = true; |
| | | 213 | | } |
| | 2501 | 214 | | continue; |
| | | 215 | | } |
| | 31263 | 216 | | state.CurrentMember = member; state.OtherMember = other; state.CurrentSpan = span; |
| | 10421 | 217 | | CapsulePlaneRayEvents.VisitEvents(new CapsulePlaneRayEventSource(new FixedSegment(edge.Start, edge.End)) |
| | 10421 | 218 | | source.Frame, 1, edge.Interval, state.JoinVisitor); |
| | | 219 | | } |
| | | 220 | | } |
| | 6425 | 221 | | } |
| | | 222 | | |
| | | 223 | | private void JoinAtPoint(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 224 | | CapsulePlaneRayEvent descriptor, int orientation, scoped in CapsulePlaneRaySelection point) |
| | | 225 | | { |
| | 582 | 226 | | JoinedState state = _joined!; |
| | 1164 | 227 | | JoinedMember first = state.Members[state.CurrentMember], second = state.Members[state.OtherMember]; |
| | 2484 | 228 | | foreach (int a in first.Connectivity.TriangleIndices) |
| | | 229 | | { |
| | 660 | 230 | | first.Collider.Mesh.GetGeometryTriangleVertices(a, out Vector3d aa, out Vector3d ab, out Vector3d ac); |
| | 660 | 231 | | if (!point.ContainsTrianglePoint(frame, new FixedTriangle(aa, ab, ac))) continue; |
| | 2604 | 232 | | foreach (int b in second.Connectivity.TriangleIndices) |
| | | 233 | | { |
| | 715 | 234 | | second.Collider.Mesh.GetGeometryTriangleVertices(b, out Vector3d ba, out Vector3d bb, out Vector3d bc); |
| | 715 | 235 | | if (!point.ContainsTrianglePoint(second.Frame, new FixedTriangle(ba, bb, bc), frame)) continue; |
| | 605 | 236 | | state.Union(first.Offset + first.Connectivity.GetRegionOrdinal(a), |
| | 605 | 237 | | second.Offset + second.Connectivity.GetRegionOrdinal(b)); |
| | 605 | 238 | | first.JoinedSpans[state.CurrentSpan] = true; |
| | | 239 | | } |
| | | 240 | | } |
| | 582 | 241 | | } |
| | | 242 | | |
| | | 243 | | private void VisitJoinedSource(JoinedState state, in CapsulePlaneRayEventSource source, in RationalSegmentInterval c |
| | | 244 | | { |
| | 643 | 245 | | state.CurrentClip = clip; |
| | 643 | 246 | | CapsulePlaneRayEvents.VisitEvents(source, _frame, 1, clip, state.Visitor); |
| | 643 | 247 | | CapsulePlaneRayEvents.VisitEvents(source, _frame, -1, clip, state.Visitor); |
| | 643 | 248 | | } |
| | | 249 | | |
| | | 250 | | private void AdmitJoinedPoint(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 251 | | CapsulePlaneRayEvent descriptor, int orientation, scoped in CapsulePlaneRaySelection candidate) |
| | | 252 | | { |
| | 16976 | 253 | | JoinedState state = _joined!; |
| | 16976 | 254 | | state.VisitedRegions.Clear(); |
| | 16976 | 255 | | int first = state.CurrentRegion < 0 ? state.CurrentMember : 0; |
| | 16976 | 256 | | int end = state.CurrentRegion < 0 ? first + 1 : state.Inputs.Count; |
| | 98608 | 257 | | for (int memberIndex = first; memberIndex < end; memberIndex++) |
| | | 258 | | { |
| | 32328 | 259 | | JoinedMember member = state.Members[memberIndex]; |
| | 120456 | 260 | | foreach (int triangle in member.Connectivity.TriangleIndices) |
| | | 261 | | { |
| | 27900 | 262 | | int region = state.RegionByRoot[state.Find(member.Offset + member.Connectivity.GetRegionOrdinal(triangle |
| | 27900 | 263 | | if (state.CurrentRegion >= 0 && region != state.CurrentRegion) continue; |
| | 25450 | 264 | | member.Collider.Mesh.GetGeometryTriangleVertices(triangle, out Vector3d a, out Vector3d b, out Vector3d |
| | 25450 | 265 | | if (!candidate.ContainsTrianglePoint(member.Frame, new FixedTriangle(a, b, c), frame) || !state.VisitedR |
| | 16142 | 266 | | KeepJoinedPoint(state, memberIndex, region, source, frame, descriptor, orientation, candidate); |
| | | 267 | | } |
| | | 268 | | } |
| | 16976 | 269 | | } |
| | | 270 | | |
| | | 271 | | private void KeepJoinedPoint(JoinedState state, int member, int region, in CapsulePlaneRayEventSource source, |
| | | 272 | | in CapsulePlaneRayFrame frame, CapsulePlaneRayEvent descriptor, int orientation, scoped in CapsulePlaneRaySelect |
| | | 273 | | { |
| | 16142 | 274 | | bool represented = candidate.TryGetPointInRigidFrame(frame, _center, _rotation, _center, _rotation, out Vector3d |
| | | 275 | | System.Diagnostics.Debug.Assert(represented); |
| | 16142 | 276 | | int point = RetainJoinedPoint(candidate, frame); |
| | 16142 | 277 | | var provenance = new JoinedWitness(member, state.CurrentMember, source, descriptor, state.CurrentClip); |
| | 16142 | 278 | | state.PointSources.Add(provenance); |
| | 63033 | 279 | | foreach (Sample old in _events.AsReadOnlySpan()) |
| | | 280 | | { |
| | 19707 | 281 | | if (old.Region != region || old.Point != p |
| | 19707 | 282 | | || FiniteSectionPoint.CompareLexicographic(BorrowPoint(old.ExactPoint), BorrowPoint(point)) != 0) contin |
| | 8665 | 283 | | _pointWordCount = _points[point].Offset; |
| | 25995 | 284 | | _points.RemoveAt(point); state.PointSources.RemoveAt(point); point = old.ExactPoint; |
| | 8665 | 285 | | break; |
| | | 286 | | } |
| | 16142 | 287 | | bool retained = false; |
| | 63033 | 288 | | foreach (Sample old in _events.AsReadOnlySpan()) |
| | 37037 | 289 | | if (old.Region == region && old.ExactPoint == point) { retained = true; break; } |
| | 23619 | 290 | | if (!retained) _events.Add(new Sample(source, descriptor, region, p, point)); |
| | 19846 | 291 | | if (source.IsSegment) { HasBoundaryIntersection = true; state.BoundaryHits[region] = true; } |
| | 16142 | 292 | | ref readonly CapsulePlaneRayFrame common = ref GetJoinedRegionFrame(state, region); |
| | 16142 | 293 | | int aligned = WideDirection3d.Dot(frame.Normal, common.Normal).Sign * orientation; |
| | 16142 | 294 | | int slot = 2 * region + (aligned > 0 ? 0 : 1); |
| | 16142 | 295 | | var winner = BorrowWinner(slot); |
| | 16142 | 296 | | int comparison = !winner.HasValue ? 1 : candidate.CompareCoplanarDepth(winner, frame, |
| | 16142 | 297 | | state.Members[state.Maximum[slot].FrameMember].Frame); |
| | 18604 | 298 | | if (comparison < 0) return; |
| | 13680 | 299 | | if (comparison == 0) |
| | | 300 | | { |
| | 610 | 301 | | int order = FiniteSectionPoint.CompareLexicographic(BorrowPoint(point), BorrowPoint(state.MaximumPoint[slot] |
| | 1112 | 302 | | if (order > 0 || order == 0 && CompareJoinedWitness(state, provenance, state.Maximum[slot]) >= 0) return; |
| | | 303 | | } |
| | 39534 | 304 | | candidate.Values.CopyTo(winner.Values); candidate.Signs.CopyTo(winner.Signs); candidate.ValueSigns.CopyTo(winner |
| | 26356 | 305 | | winner.Restore(candidate.Metadata); _winners[slot] = winner.Metadata; |
| | 26356 | 306 | | state.Maximum[slot] = provenance; state.MaximumPoint[slot] = point; |
| | 13178 | 307 | | } |
| | | 308 | | |
| | | 309 | | private static ref readonly CapsulePlaneRayFrame GetJoinedRegionFrame(JoinedState state, int region) => |
| | 43537 | 310 | | ref state.Members[state.NodeMembers[state.Roots[region]]].Frame; |
| | | 311 | | |
| | | 312 | | private int RetainJoinedPoint(scoped in CapsulePlaneRaySelection point, in CapsulePlaneRayFrame frame) |
| | | 313 | | { |
| | 16142 | 314 | | int words = Math.Max(Math.Max(ContactQuadratic.ActiveWords(point.X), ContactQuadratic.ActiveWords(point.Y)), |
| | 16142 | 315 | | Math.Max(ContactQuadratic.ActiveWords(point.Z), ContactQuadratic.ActiveWords(point.Denominator))) + 6; |
| | 16142 | 316 | | int rootWords = WideArithmetic.GetActiveMagnitudeLength(point.Root); |
| | 32284 | 317 | | int index = _points.Count, needed = _pointWordCount + 8 * words + rootWords; |
| | 16558 | 318 | | if (_pointValues.Length < needed) Array.Resize(ref _pointValues, Math.Max(needed, 2 * _pointValues.Length)); |
| | 16142 | 319 | | if (_pointSigns.Length < (index + 1) * FiniteSectionPoint.SignCount) |
| | 160 | 320 | | Array.Resize(ref _pointSigns, Math.Max((index + 1) * FiniteSectionPoint.SignCount, Math.Max(64, 2 * _pointSi |
| | 32284 | 321 | | _points.Add(new PointStorage(_pointWordCount, words, rootWords)); _pointWordCount = needed; |
| | 16142 | 322 | | var retained = BorrowPoint(index); |
| | 48426 | 323 | | point.X.CopyTo(retained.Coordinate(0)); point.Y.CopyTo(retained.Coordinate(1)); point.Z.CopyTo(retained.Coordina |
| | 16142 | 324 | | Signed320 d = frame.Finite.ShapeFrame.CoordinateDenominator; |
| | 16142 | 325 | | ContactQuadratic.Scale(point.Denominator, Signed576.ExtendValue(WideArithmetic.AddSigned320(d, d)), retained.Den |
| | 16142 | 326 | | point.Root[..rootWords].CopyTo(retained.Root); |
| | 16142 | 327 | | return index; |
| | | 328 | | } |
| | | 329 | | |
| | | 330 | | private static int CompareJoinedWitness(JoinedState state, JoinedWitness first, JoinedWitness second) |
| | | 331 | | { |
| | 354 | 332 | | int order = ((IPhysicsColliderRegistryItem)state.Members[first.Member].Collider).ReplayOrder |
| | 354 | 333 | | .CompareTo(((IPhysicsColliderRegistryItem)state.Members[second.Member].Collider).ReplayOrder); |
| | 382 | 334 | | if (order != 0) return order; |
| | 326 | 335 | | return state.Members[first.Member].Owner.CompareTo(state.Members[second.Member].Owner); |
| | | 336 | | } |
| | | 337 | | |
| | | 338 | | private int CompareJoinedPoints(Sample first, Sample second) |
| | | 339 | | { |
| | 699 | 340 | | return FiniteSectionPoint.CompareLexicographic(BorrowPoint(first.ExactPoint), BorrowPoint(second.ExactPoint)); |
| | | 341 | | } |
| | | 342 | | |
| | | 343 | | private void ReduceJoinedSamples(JoinedState state, int region) |
| | | 344 | | { |
| | 12938 | 345 | | var positive = BorrowWinner(2 * region); var negative = BorrowWinner(2 * region + 1); |
| | 6469 | 346 | | var positiveFrame = state.Members[state.Maximum[2 * region].FrameMember].Frame; |
| | 6469 | 347 | | var negativeFrame = state.Members[state.Maximum[2 * region + 1].FrameMember].Frame; |
| | 6469 | 348 | | int orientation = positive.CompareCoplanarDepth(negative, positiveFrame, negativeFrame) <= 0 ? 1 : -1; |
| | 6469 | 349 | | _orientations[region] = orientation; |
| | 12938 | 350 | | int slot = 2 * region + (orientation > 0 ? 0 : 1), point = state.MaximumPoint[slot]; |
| | 6469 | 351 | | _pool.FastClear(); |
| | 20427 | 352 | | foreach (Sample sample in _events.AsReadOnlySpan()) |
| | 19917 | 353 | | if (sample.Region == region && sample.ExactPoint == point) { _pool.Add(sample); break; } |
| | 29150 | 354 | | foreach (Sample sample in _events.AsReadOnlySpan()) |
| | 9114 | 355 | | if (sample.Region == region && sample.ExactPoint != point) _pool.Add(sample); |
| | 6469 | 356 | | ref readonly CapsulePlaneRayFrame common = ref GetJoinedRegionFrame(state, region); |
| | 6469 | 357 | | WidePlaneMetrics metrics = common.Finite.ShapeFrame.HasExactScale ? default : new WidePlaneMetrics(new WideAxis3 |
| | 6469 | 358 | | Signed320.NarrowValue(common.Normal.X), Signed320.NarrowValue(common.Normal.Y), Signed320.NarrowValue(common |
| | | 359 | | |
| | | 360 | | // A nonempty connected section cannot leave an embedded filled union |
| | | 361 | | // without crossing its complete retained boundary. Only that global |
| | | 362 | | // certificate permits dropping off-peak samples for a strictly curved support. |
| | 6469 | 363 | | bool contained = state.Trusted[region] && !state.BoundaryHits[region]; |
| | 6469 | 364 | | bool uniqueSupport = common.Finite.ShapeFrame.Radius.Sign > 0 |
| | 6469 | 365 | | && (common.Finite.Length == Fixed64.Zero || common.Normal.X.IsZero && common.Normal.Z.IsZero); |
| | 6469 | 366 | | int count = contained && uniqueSupport ? 1 : Math.Min(ContactManifold.MaxContactsPerGroup, _pool.Count); |
| | 12938 | 367 | | Span<int> chosen = stackalloc int[ContactManifold.MaxContactsPerGroup]; chosen[0] = 0; |
| | 27404 | 368 | | for (int index = 0; index < count; index++) |
| | | 369 | | { |
| | 7997 | 370 | | if (index > 0) chosen[index] = common.Finite.ShapeFrame.HasExactScale |
| | 764 | 371 | | ? MeshPlaneSampleReduction.ChooseNext(_pool.AsReadOnlySpan(), chosen[..index], common.Normal, state.Poin |
| | 764 | 372 | | : MeshPlaneSampleReduction.ChooseNext(_pool.AsReadOnlySpan(), chosen[..index], metrics, state.PointCompa |
| | 7233 | 373 | | _samples[region * ContactManifold.MaxContactsPerGroup + index] = _pool[chosen[index]]; |
| | | 374 | | } |
| | 6469 | 375 | | _counts[region] = count; |
| | 6469 | 376 | | } |
| | | 377 | | |
| | | 378 | | internal bool TryGetJoinedSample(int region, int index, out FixedContactAnchors contact, out LSMeshCollider? supplie |
| | | 379 | | { |
| | 28610 | 380 | | contact = default; supplier = null; |
| | 14312 | 381 | | if (_joined == null || !_joined.Active || (uint)region >= (uint)RegionCount || (uint)index >= (uint)_counts[regi |
| | 14298 | 382 | | Sample sample = _samples[region * ContactManifold.MaxContactsPerGroup + index]; |
| | 14298 | 383 | | int slot = 2 * region + (_orientations[region] > 0 ? 0 : 1); |
| | 14298 | 384 | | JoinedWitness witness = index == 0 ? _joined.Maximum[slot] : _joined.PointSources[sample.ExactPoint]; |
| | 28596 | 385 | | JoinedMember member = _joined.Members[witness.Member]; supplier = member.Collider; |
| | 14298 | 386 | | JoinedMember producer = _joined.Members[witness.FrameMember]; |
| | 14298 | 387 | | ref readonly CapsulePlaneRayFrame common = ref GetJoinedRegionFrame(_joined, region); |
| | 14298 | 388 | | int orientation = WideDirection3d.Dot(producer.Frame.Normal, common.Normal).Sign * _orientations[region]; |
| | 14298 | 389 | | Span<ulong> values = stackalloc ulong[SelectionWords]; |
| | 14298 | 390 | | Span<int> signs = stackalloc int[CapsulePlaneRaySelection.SignCount]; |
| | 14298 | 391 | | Span<sbyte> valueSigns = stackalloc sbyte[CapsulePlaneRaySelection.ValueSignCount]; |
| | 14298 | 392 | | var selection = index == 0 ? BorrowWinner(slot) : new CapsulePlaneRaySelection(values, signs, valueSigns, _field |
| | 14298 | 393 | | if (index != 0) |
| | | 394 | | { |
| | 841 | 395 | | bool found = CapsulePlaneRayEvents.Reconstruct(witness.Source, producer.Frame, witness.Event, orientation, w |
| | | 396 | | System.Diagnostics.Debug.Assert(found); |
| | | 397 | | } |
| | 14298 | 398 | | PhysicsMesh mesh = member.Collider.Mesh; |
| | 14298 | 399 | | bool represented = selection.TryGetPointInRigidFrame(producer.Frame, _center, _rotation, _center, _rotation, out |
| | 14298 | 400 | | & selection.TryGetExitAnchor(producer.Frame, orientation, _center, _rotation, out FixedPointAnchor exit, out |
| | 14298 | 401 | | contact = new FixedContactAnchors(new FixedPointAnchor(_center, _rotation, authored), exit, |
| | 14298 | 402 | | -producer.Frame.GetWorldNormal(producer.Collider.Mesh.Rotation) * orientation, depth, clamped); |
| | 14298 | 403 | | return represented; |
| | | 404 | | } |
| | | 405 | | |
| | | 406 | | private readonly struct JoinedWitness |
| | | 407 | | { |
| | | 408 | | internal readonly int Member, FrameMember; |
| | | 409 | | internal readonly CapsulePlaneRayEventSource Source; |
| | | 410 | | internal readonly CapsulePlaneRayEvent Event; |
| | | 411 | | internal readonly RationalSegmentInterval Clip; |
| | | 412 | | internal JoinedWitness(int member, int frameMember, CapsulePlaneRayEventSource source, CapsulePlaneRayEvent desc |
| | | 413 | | RationalSegmentInterval clip) |
| | 96852 | 414 | | { Member = member; FrameMember = frameMember; Source = source; Event = descriptor; Clip = clip; } |
| | | 415 | | } |
| | | 416 | | |
| | | 417 | | private sealed class JoinedState : MeshJoinedSurfaceRegions<CapsulePlaneRayFrame> |
| | | 418 | | { |
| | 127 | 419 | | internal readonly SwiftList<JoinedWitness> PointSources = new(); |
| | 127 | 420 | | internal readonly SwiftHashSet<int> VisitedRegions = new(); |
| | 127 | 421 | | internal JoinedWitness[] Maximum = Array.Empty<JoinedWitness>(); |
| | 127 | 422 | | internal int[] MaximumPoint = Array.Empty<int>(); |
| | 254 | 423 | | internal bool[] BoundaryHits = Array.Empty<bool>(), Trusted = Array.Empty<bool>(); |
| | | 424 | | internal readonly IComparer<Sample> PointComparer; |
| | | 425 | | internal readonly CapsulePlaneRayEventVisitor Visitor, JoinVisitor; |
| | | 426 | | internal int CurrentMember, OtherMember, CurrentSpan, CurrentRegion; |
| | | 427 | | internal RationalSegmentInterval CurrentClip; |
| | 127 | 428 | | internal JoinedState(MeshCapsuleFaceRegions owner) |
| | | 429 | | { |
| | 127 | 430 | | PointComparer = Comparer<Sample>.Create(owner.CompareJoinedPoints); |
| | 254 | 431 | | Visitor = owner.AdmitJoinedPoint; JoinVisitor = owner.JoinAtPoint; |
| | 127 | 432 | | } |
| | | 433 | | } |
| | | 434 | | } |