| | | 1 | | //======================================================================= |
| | | 2 | | // StaticSurface2D.Mixed.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 Gravitas.CollisionHandling; |
| | | 14 | | using Gravitas.Support; |
| | | 15 | | using SwiftCollections; |
| | | 16 | | |
| | | 17 | | namespace Gravitas; |
| | | 18 | | |
| | | 19 | | /// <content>Admits finite exposed prism supports before native mixed response selection.</content> |
| | | 20 | | public sealed partial class StaticSurface2D : IScaledConvexHullContactDomain |
| | | 21 | | { |
| | 600 | 22 | | private readonly SwiftList<LSCollider2D> _mixedLocal = new(); |
| | 600 | 23 | | private readonly SwiftList<MixedPrism> _mixedPrisms = new(); |
| | 1200 | 24 | | private readonly SwiftList<MixedSample> _mixedSamples = new(), _mixedPool = new(); |
| | 600 | 25 | | private readonly SwiftList<MixedGroup> _mixedGroups = new(); |
| | 600 | 26 | | private readonly SwiftHashSet<int> _mixedOverlaps = new(); |
| | 600 | 27 | | private readonly SwiftList<RationalSegmentInterval> _mixedIntervals = new(); |
| | 600 | 28 | | private readonly SwiftList<int> _mixedFan = new(); |
| | 600 | 29 | | private readonly SwiftList<Fixed64> _mixedHeights = new(); |
| | | 30 | | private int _mixedEdgeStart, _mixedEdgeEnd; |
| | 600 | 31 | | private readonly RationalSegmentInterval[] _mixedEdgeClip = new RationalSegmentInterval[1]; |
| | 1 | 32 | | private static readonly RationalSegmentInterval[] MixedFullInterval = { default }; |
| | 600 | 33 | | private readonly Vector3d[] _mixedBox = new Vector3d[8]; |
| | 600 | 34 | | private Vector3d[] _mixedFace = new Vector3d[4]; |
| | 1800 | 35 | | private int[] _mixedFaceTriangles = new int[6], _mixedFaceEdges = new int[8], _mixedComponents = Array.Empty<int>(); |
| | | 36 | | private readonly ScaledHullPenetrationAdmission _mixedAdmit; |
| | | 37 | | private readonly PointSpanPenetrationAdmission _mixedAdmitRigid; |
| | | 38 | | private WidePointSpanPenetration _mixedRigidPenetration; |
| | | 39 | | private bool _mixedUsingRigid; |
| | | 40 | | private LSCollider? _mixedMoving; |
| | | 41 | | private ScaledRigidFrame3d _mixedMovingFrame; |
| | | 42 | | private uint _mixedMovingBroad, _mixedMovingDependency; |
| | | 43 | | private Vector3d _mixedMovingCenter; |
| | | 44 | | private FixedQuaternion _mixedMovingRotation; |
| | | 45 | | private bool _mixedCurrent, _mixedQuaternion; |
| | | 46 | | private Fixed64 _mixedReach, _mixedRequestedReach; |
| | 600 | 47 | | private int _mixedMember, _mixedGroup, _mixedSupplier, _mixedPreparedCount = -1; |
| | 1 | 48 | | private static readonly IComparer<MixedSample> MixedPointOrder = Comparer<MixedSample>.Create(static (a, b) => |
| | 1 | 49 | | { |
| | 1 | 50 | | int order = a.Contact.SecondAnchor.CompareLocalFeature(b.Contact.SecondAnchor); |
| | 1 | 51 | | return order != 0 ? order : a.Supplier.CompareTo(b.Supplier); |
| | 1 | 52 | | }); |
| | | 53 | | |
| | 9918 | 54 | | private ReadOnlySpan<Vector3d> MixedMovingPoints => _mixedMoving is LSMeshCollider mesh |
| | 9918 | 55 | | ? _mixedMovingFeature >= 0 ? _mixedPatchPoints.AsReadOnlySpan() : mesh.Mesh.GeometryVertices : _mixedBox; |
| | 8980 | 56 | | private ReadOnlySpan<int> MixedMovingTriangles => _mixedMoving is LSMeshCollider mesh |
| | 8980 | 57 | | ? _mixedMovingFeature >= 0 ? _mixedPatchTriangles.AsReadOnlySpan() : mesh.Mesh.Triangles : WideOrientedBox.BoxHu |
| | 9322 | 58 | | private ReadOnlySpan<int> MixedMovingEdges => _mixedMoving is LSMeshCollider mesh |
| | 9322 | 59 | | ? _mixedMovingFeature >= 0 ? _mixedPatchEdges.AsReadOnlySpan() : mesh.Mesh.ConvexSatEdgeVertexPairs : WideOrient |
| | | 60 | | |
| | | 61 | | internal bool AppendMixed(LSCollider mover, LSCollider2D overlap, ContactManifold destination, |
| | | 62 | | int namespace3D, int namespace2D) |
| | | 63 | | { |
| | 7424 | 64 | | if (_members.Length == 1) |
| | 14 | 65 | | return CollisionDetectionMixed.AppendManifold(mover, overlap, destination, namespace3D, namespace2D, false); |
| | 7410 | 66 | | bool current = _mixedCurrent && _mixedMoving == mover && _mixedMovingBroad == mover.BroadPhaseVersion |
| | 7410 | 67 | | && _mixedMovingDependency == (mover.CompoundOwner ?? mover).ContactDependencyVersion |
| | 7410 | 68 | | && _mixedMovingCenter == mover.Center && _mixedMovingRotation == mover.Rotation; |
| | 12392 | 69 | | if (!current) _mixedReach = mover.CanonicalCenteredProxyRadius * Fixed64.Two; |
| | | 70 | | while (true) |
| | | 71 | | { |
| | 7970 | 72 | | GatherMixed(mover, _mixedReach); |
| | 7970 | 73 | | if (current && _mixedCurrent) break; |
| | 5543 | 74 | | _mixedMoving = mover; |
| | 5543 | 75 | | _mixedMovingFrame = mover is LSMeshCollider mesh ? mesh.Mesh.GeometryFrame |
| | 5543 | 76 | | : ScaledRigidFrame3d.Rigid(mover.Center, mover.Rotation); |
| | 6449 | 77 | | if (mover is LSCuboidCollider box) WideOrientedBox.GetBoxHullVertices(box.OrientedBox.HalfExtents, _mixedBox |
| | 5543 | 78 | | _mixedRequestedReach = _mixedReach; |
| | 5543 | 79 | | _mixedMovingFeature = -1; |
| | 6654 | 80 | | if (mover is LSCapsuleCollider capsule && capsule.AxisLength > Fixed64.Zero) BuildMixedCapsule(); |
| | 6147 | 81 | | else if (mover is LSSphereCollider or LSCapsuleCollider) BuildMixedSphere(); |
| | 3349 | 82 | | else if (mover is LSCylinderCollider or LSConeCollider) BuildMixedCurved(); |
| | 2085 | 83 | | else BuildMixed(); |
| | 5543 | 84 | | if (_mixedRequestedReach > _mixedReach) |
| | | 85 | | { |
| | | 86 | | // A deep placement can project beyond the body's initial query. |
| | | 87 | | // Grow only to a considered native SAT exit, never a guessed iteration budget. |
| | 1680 | 88 | | _mixedReach = _mixedRequestedReach; current = false; continue; |
| | | 89 | | } |
| | 9966 | 90 | | _mixedMovingBroad = mover.BroadPhaseVersion; _mixedMovingDependency = (mover.CompoundOwner ?? mover).Contact |
| | 14949 | 91 | | _mixedMovingCenter = mover.Center; _mixedMovingRotation = mover.Rotation; _mixedCurrent = true; |
| | | 92 | | break; |
| | | 93 | | } |
| | 44238 | 94 | | for (int member = 0; member < _mixedLocal.Count; member++) |
| | | 95 | | // Keep a mesh's native patch inventory when it leaves the seam's |
| | | 96 | | // neighborhood; switching back to individual triangles changes response. |
| | 16030 | 97 | | if (_mixedLocal[member] == overlap && !_mixedPrisms[member].Joined && _mixedMovingFeature < 0) |
| | 1321 | 98 | | return CollisionDetectionMixed.AppendManifold(mover, overlap, destination, namespace3D, namespace2D, fal |
| | 6089 | 99 | | if (mover is LSCapsuleCollider supportedCapsule && supportedCapsule.AxisLength > Fixed64.Zero) |
| | 1777 | 100 | | return AppendMixedCapsule(overlap, destination, namespace3D, namespace2D); |
| | 4312 | 101 | | if (mover is LSSphereCollider or LSCapsuleCollider) |
| | 1195 | 102 | | return AppendMixedSphere(overlap, destination, namespace3D, namespace2D); |
| | 3117 | 103 | | if (mover is LSCylinderCollider or LSConeCollider) |
| | 1166 | 104 | | return AppendMixedCurved(overlap, destination, namespace3D, namespace2D); |
| | 1951 | 105 | | Span<int> chosen = stackalloc int[ContactManifold.MaxContactsPerGroup]; |
| | 8196 | 106 | | for (int group = 0; group < _mixedGroups.Count; group++) |
| | | 107 | | { |
| | 2147 | 108 | | MixedGroup response = _mixedGroups[group]; |
| | 2147 | 109 | | if (response.Carrier != overlap.Id) continue; |
| | 1425 | 110 | | _mixedPool.FastClear(); |
| | 26506 | 111 | | foreach (MixedSample sample in _mixedSamples.AsReadOnlySpan()) if (sample.Group == group) _mixedPool.Add(sam |
| | 1425 | 112 | | _mixedPool.SortInPlace(MixedPointOrder); |
| | 1425 | 113 | | int count = Math.Min(chosen.Length, _mixedPool.Count); |
| | 12518 | 114 | | for (int slot = 0; slot < count; slot++) |
| | 4834 | 115 | | chosen[slot] = slot == 0 ? 0 : MeshPlaneSampleReduction.ChooseNext(_mixedPool.AsReadOnlySpan(), |
| | 4834 | 116 | | chosen[..slot], response.Axis, MixedPointOrder); |
| | 12518 | 117 | | for (int slot = 0; slot < count; slot++) |
| | | 118 | | { |
| | 9668 | 119 | | MixedSample sample = _mixedPool[chosen[slot]]; LSCollider2D supplier = _mixedLocal[sample.Supplier]; |
| | 4834 | 120 | | FixedContactAnchors contact = sample.Contact; |
| | 4834 | 121 | | destination.AddContact(new ContactAnchor(contact.FirstAnchor), new ContactAnchor(contact.SecondAnchor), |
| | 4834 | 122 | | contact.Depth, contact.Normal, mover.Material, supplier.Material, contact.DepthIsClamped, |
| | 4834 | 123 | | featureNamespaceA: namespace3D, featureNamespaceB: namespace2D, |
| | 4834 | 124 | | group: new ContactGroupKey(namespace3D, namespace2D, |
| | 4834 | 125 | | surfaceA: response.Feature < 0 ? 0 : ((LSMeshCollider)mover).Mesh.GetCanonicalSurfaceOrdinal(res |
| | 4834 | 126 | | region: group, sourceIdB: supplier.Id + 1, |
| | 4834 | 127 | | sourceOrderB: ((IPhysicsColliderRegistryItem)supplier).ReplayOrder + 1)); |
| | | 128 | | } |
| | | 129 | | } |
| | 1951 | 130 | | return _mixedOverlaps.Contains(overlap.Id); |
| | | 131 | | } |
| | | 132 | | |
| | | 133 | | private void RequestMixedReach(Fixed64 depth) |
| | | 134 | | { |
| | 3898 | 135 | | Fixed64 reach = _mixedMoving!.CanonicalCenteredProxyRadius + depth + Fixed64.MinIncrement + Fixed64.MinIncrement |
| | 3923 | 136 | | if (reach > _mixedRequestedReach) _mixedRequestedReach = reach; |
| | 3898 | 137 | | } |
| | | 138 | | |
| | | 139 | | private void GatherMixed(LSCollider mover, Fixed64 reach) |
| | | 140 | | { |
| | 8003 | 141 | | FixedBoundBox bounds = FixedBoundBox.FromCenterAndScopeClippedToDomain(mover.Center, |
| | 8003 | 142 | | Vector3d.One * reach); |
| | 8003 | 143 | | int previous = _mixedLocal.Count; |
| | 8003 | 144 | | GatherMixedMembers(mover, bounds, _mixedLocal); |
| | 8569 | 145 | | if (previous != _mixedLocal.Count) _mixedCurrent = false; |
| | 8003 | 146 | | bool quaternion = mover is LSMeshCollider; |
| | 8003 | 147 | | bool prepared = _mixedQuaternion == quaternion && _mixedPreparedCount == _mixedLocal.Count; |
| | 10003 | 148 | | while (_mixedPrisms.Count < _mixedLocal.Count) _mixedPrisms.Add(new MixedPrism()); |
| | 50794 | 149 | | for (int member = 0; member < _mixedLocal.Count; member++) |
| | 17394 | 150 | | prepared &= _mixedPrisms[member].Matches(_mixedLocal[member]); |
| | 9716 | 151 | | if (!prepared) { _mixedQuaternion = quaternion; PrepareMixed(); _mixedCurrent = false; } |
| | | 152 | | |
| | 8003 | 153 | | } |
| | | 154 | | |
| | | 155 | | private void GatherMixedMembers(LSCollider mover, FixedBoundBox bounds, SwiftList<LSCollider2D> destination) |
| | | 156 | | { |
| | 8545 | 157 | | _context.MixedCollisions.Collect2DCandidatesInMixedBounds(bounds.Min, bounds.Max, PhysicsLayerMask.All, |
| | 8545 | 158 | | _candidates, staticStyleOnly: true, cachePartitionRefresh: true); |
| | 8545 | 159 | | destination.FastClear(); |
| | 54116 | 160 | | foreach (LSCollider2D candidate in _candidates.AsReadOnlySpan()) |
| | 18513 | 161 | | if (candidate.StaticSurface == this && !candidate.IsTrigger |
| | 36926 | 162 | | && _context.MixedCollisions.RequireCollisionPair(mover.CompoundOwner ?? mover, candidate)) destination.A |
| | 8545 | 163 | | destination.SortInPlace(SurfaceMemberOrder); |
| | 8545 | 164 | | } |
| | | 165 | | |
| | | 166 | | private void PrepareMixed() |
| | | 167 | | { |
| | 571 | 168 | | _mixedSphereConcavityCurrent = false; |
| | 571 | 169 | | _mixedBoundaryLinesCurrent = false; |
| | 571 | 170 | | _mixedCurvedDomainsCurrent = false; |
| | 571 | 171 | | _mixedPreparedCount = _mixedLocal.Count; |
| | 1015 | 172 | | if (_mixedComponents.Length < _mixedLocal.Count) Array.Resize(ref _mixedComponents, _mixedLocal.Count); |
| | 3814 | 173 | | for (int member = 0; member < _mixedLocal.Count; member++) |
| | | 174 | | { |
| | 1336 | 175 | | _mixedPrisms[member].Prepare(_mixedLocal[member], _mixedQuaternion); |
| | 1336 | 176 | | _mixedPrisms[member].Joined = false; |
| | 1336 | 177 | | PrepareMixedBands(member); |
| | 1336 | 178 | | _mixedComponents[member] = member; |
| | | 179 | | } |
| | 3814 | 180 | | for (int member = 0; member < _mixedLocal.Count; member++) |
| | 7032 | 181 | | for (int band = 0; band < _mixedPrisms[member].BandCount; band++) |
| | 22834 | 182 | | foreach (var span in _mixedPrisms[member].Bands[band].Boundary.Spans) |
| | 9237 | 183 | | if (span.Kind == SpanKind.Joined) |
| | | 184 | | { |
| | 2238 | 185 | | _mixedPrisms[member].Joined = true; |
| | 4476 | 186 | | int from = _mixedComponents[span.OtherMember], to = _mixedComponents[member]; |
| | 22532 | 187 | | for (int index = 0; index < _mixedLocal.Count; index++) |
| | 15313 | 188 | | if (_mixedComponents[index] == from) _mixedComponents[index] = to; |
| | | 189 | | } |
| | 571 | 190 | | } |
| | | 191 | | |
| | | 192 | | private void PrepareMixedBands(int member) |
| | | 193 | | { |
| | 1336 | 194 | | MixedPrism prism = _mixedPrisms[member]; |
| | 1336 | 195 | | Fixed64 half = _mixedLocal[member].MixedHalfThickness; |
| | 4008 | 196 | | _mixedHeights.FastClear(); _mixedHeights.Add(-half); _mixedHeights.Add(half); |
| | 10588 | 197 | | for (int other = 0; other < _mixedLocal.Count; other++) |
| | 3958 | 198 | | if (TryGetMixedHeights(member, other, out Fixed64 lower, out Fixed64 upper)) |
| | | 199 | | { |
| | 4427 | 200 | | if (lower > -half && lower < half) _mixedHeights.Add(lower); |
| | 4445 | 201 | | if (upper > -half && upper < half) _mixedHeights.Add(upper); |
| | | 202 | | } |
| | 1336 | 203 | | _mixedHeights.SortInPlace(); |
| | 1336 | 204 | | int count = 0; |
| | 7384 | 205 | | for (int height = 1; height < _mixedHeights.Count; height++) |
| | | 206 | | { |
| | 4712 | 207 | | Fixed64 lower = _mixedHeights[height - 1], upper = _mixedHeights[height]; |
| | 2356 | 208 | | if (lower == upper) continue; |
| | 3584 | 209 | | if (prism.Bands.Count == count) prism.Bands.Add(new MixedBand()); |
| | 2180 | 210 | | MixedBand band = prism.Bands[count++]; |
| | 4360 | 211 | | band.Lower = lower; band.Upper = upper; |
| | 2180 | 212 | | band.Boundary.Build(_mixedLocal.AsReadOnlySpan(), member, mixed: _mixedQuaternion, |
| | 2180 | 213 | | lowerHeight: lower, upperHeight: upper); |
| | | 214 | | } |
| | 1336 | 215 | | prism.BandCount = count; |
| | 1336 | 216 | | } |
| | | 217 | | |
| | | 218 | | private void BuildMixed() |
| | | 219 | | { |
| | 6255 | 220 | | _mixedSamples.FastClear(); _mixedGroups.FastClear(); _mixedOverlaps.Clear(); |
| | 4170 | 221 | | _mixedRegions.FastClear(); _mixedRegionParents.FastClear(); |
| | 2085 | 222 | | if (_mixedMoving is LSMeshCollider mesh && (mesh.Mode != MeshColliderMode.Convex || !mesh.Mesh.IsClosedSurface)) |
| | 322 | 223 | | BuildMixedMesh(mesh.Mesh); |
| | 1763 | 224 | | else BuildMixedPart(); |
| | 2085 | 225 | | CompleteMixedRegions(); |
| | 2085 | 226 | | } |
| | | 227 | | |
| | | 228 | | private void BuildMixedPart() |
| | | 229 | | { |
| | 11416 | 230 | | for (_mixedMember = 0; _mixedMember < _mixedLocal.Count; _mixedMember++) |
| | | 231 | | { |
| | 3595 | 232 | | MixedPrism prism = _mixedPrisms[_mixedMember]; |
| | 3595 | 233 | | if (!_mixedMoving!.Bounds.Intersects(_mixedLocal[_mixedMember].MixedBounds3D)) continue; |
| | 2846 | 234 | | bool hit = _mixedQuaternion ? FindMixedPenetration(prism.Frame, prism, out var penetration) |
| | 2846 | 235 | | : FindMixedPenetration(prism.NativeFrame, prism, out penetration); |
| | 2846 | 236 | | if (!hit) continue; |
| | 2619 | 237 | | _mixedOverlaps.Add(_mixedLocal[_mixedMember].Id); |
| | 3975 | 238 | | if (!prism.Joined && _mixedMovingFeature < 0) { HasMixedReach(penetration); continue; } |
| | 1941 | 239 | | if (!AdmitMixedAxis(penetration)) |
| | | 240 | | { |
| | 223 | 241 | | if (_mixedRequestedReach > _mixedReach) return; |
| | 329 | 242 | | if (_mixedQuaternion) FindMixedPenetration(prism.Frame, prism, out penetration, _mixedAdmit); |
| | 105 | 243 | | else FindMixedPenetration(prism.NativeFrame, prism, out penetration, _mixedAdmit); |
| | | 244 | | } |
| | 1989 | 245 | | if (_mixedRequestedReach > _mixedReach) return; |
| | 1887 | 246 | | if (!penetration.HasValue) continue; |
| | 1885 | 247 | | _mixedGroup = FindMixedGroup(penetration); |
| | 1885 | 248 | | if (IsMixedFaceAxis(penetration) && HasMixedFaceRegion(_mixedGroup)) continue; |
| | 1443 | 249 | | if (_mixedGroup == _mixedGroups.Count) |
| | 1414 | 250 | | _mixedGroups.Add(new(_mixedComponents[_mixedMember], _mixedLocal[_mixedMember].Id, penetration, _mixedMo |
| | 1443 | 251 | | VisitMixed(penetration, false); |
| | | 252 | | } |
| | 2113 | 253 | | } |
| | | 254 | | |
| | | 255 | | private bool FindMixedPenetration<TFrame>(in TFrame frame, MixedPrism prism, out WideScaledHullPenetration penetrati |
| | | 256 | | ScaledHullPenetrationAdmission? admission = null) where TFrame : struct, IForwardHullFrame3d |
| | | 257 | | { |
| | 3063 | 258 | | _mixedUsingRigid = _mixedMovingFrame.IsRigid && prism.Rigid; |
| | 3063 | 259 | | if (_mixedUsingRigid) |
| | | 260 | | { |
| | 2929 | 261 | | bool hit = WideOrientedBox.TryGetConvexHullPenetration(_mixedMovingFrame.RigidOrigin, _mixedMovingFrame.Rota |
| | 2929 | 262 | | MixedMovingPoints, MixedMovingTriangles, MixedMovingEdges, prism.Frame.RigidOrigin, prism.Frame.Rotation |
| | 2929 | 263 | | prism.Points, prism.Triangles, prism.Edges, out WidePointSpanPenetration selected, |
| | 2929 | 264 | | skipParallelDirections: true, admission: admission == null ? null : _mixedAdmitRigid); |
| | | 265 | | // Admission borrows the current candidate for rigid visitation; |
| | | 266 | | // it must not alias the SAT accumulator through this field. |
| | 2929 | 267 | | _mixedRigidPenetration = selected; |
| | 2929 | 268 | | penetration = new(_mixedRigidPenetration); |
| | 2929 | 269 | | return hit; |
| | | 270 | | } |
| | 134 | 271 | | return WideOrientedBox.TryGetConvexHullPenetration(_mixedMovingFrame, MixedMovingPoints, MixedMovingTriangles, |
| | 134 | 272 | | MixedMovingEdges, frame, prism.Points, prism.Triangles, prism.Edges, out penetration, |
| | 134 | 273 | | skipParallelDirections: true, admission: admission); |
| | | 274 | | } |
| | | 275 | | |
| | | 276 | | private bool AdmitMixedRigidAxis(in WidePointSpanPenetration penetration) |
| | | 277 | | { |
| | 1623 | 278 | | _mixedRigidPenetration = penetration; |
| | 1623 | 279 | | return AdmitMixedAxis(new WideScaledHullPenetration(penetration)); |
| | | 280 | | } |
| | | 281 | | |
| | | 282 | | private bool AdmitMixedAxis(in WideScaledHullPenetration penetration) => |
| | 3783 | 283 | | HasMixedReach(penetration) && (IsMixedFaceAxis(penetration) && HasMixedFaceRegion(FindMixedGroup(penetration)) |
| | 3783 | 284 | | || VisitMixed(penetration, true) > 0); |
| | | 285 | | |
| | | 286 | | private bool HasMixedReach(in WideScaledHullPenetration penetration) |
| | | 287 | | { |
| | | 288 | | // Every source support point is within its radius; projecting it to |
| | | 289 | | // this plane adds at most the exact depth. One raw unit raises the |
| | | 290 | | // nearest depth outward; another covers final anchor rounding. |
| | 4461 | 291 | | Fixed64 reach = _mixedMoving!.CanonicalCenteredProxyRadius |
| | 4461 | 292 | | + penetration.GetRoundedDepth(out _) + Fixed64.MinIncrement + Fixed64.MinIncrement; |
| | 8566 | 293 | | if (reach <= _mixedReach) return true; |
| | 457 | 294 | | if (reach > _mixedRequestedReach) _mixedRequestedReach = reach; |
| | 356 | 295 | | return false; |
| | | 296 | | } |
| | | 297 | | |
| | | 298 | | private int VisitMixed(in WideScaledHullPenetration penetration, bool admissionOnly) |
| | | 299 | | { |
| | | 300 | | // Select the original prism's actual feature. A remote coplanar face |
| | | 301 | | // must not suppress a disconnected vertex/edge supplier on this plane. |
| | 7919 | 302 | | if (IsMixedFaceAxis(penetration)) return VisitMixedFaces(penetration, admissionOnly); |
| | 909 | 303 | | MixedPrism prism = _mixedPrisms[_mixedMember]; |
| | 909 | 304 | | _mixedSupplier = _mixedMember; |
| | 909 | 305 | | return _mixedQuaternion ? VisitMixedEdges(prism.Frame, prism, penetration, admissionOnly) |
| | 909 | 306 | | : VisitMixedEdges(prism.NativeFrame, prism, penetration, admissionOnly); |
| | | 307 | | } |
| | | 308 | | |
| | | 309 | | private bool IsMixedFaceAxis(in WideScaledHullPenetration penetration) |
| | | 310 | | { |
| | 9728 | 311 | | MixedPrism prism = _mixedPrisms[_mixedMember]; |
| | 9728 | 312 | | WideDirection3d axis = penetration.Negate ? -penetration.Axis : penetration.Axis; |
| | 9728 | 313 | | return _mixedQuaternion ? SupportsFace(prism.Frame) : SupportsFace(prism.NativeFrame); |
| | | 314 | | bool SupportsFace<TFrame>(TFrame frame) where TFrame : struct, IForwardHullFrame3d |
| | | 315 | | { |
| | | 316 | | Signed832 support = WideOrientedBox.GetScaledHullSupport(frame, prism.Points, axis, false); |
| | | 317 | | for (int triangle = 0; triangle < prism.Triangles.Length; triangle += 3) |
| | | 318 | | if (WideOrientedBox.IsScaledHullSupport(frame.GetPoint(prism.Points[prism.Triangles[triangle]]), axis, s |
| | | 319 | | && WideOrientedBox.IsScaledHullSupport(frame.GetPoint(prism.Points[prism.Triangles[triangle + 1]]), |
| | | 320 | | && WideOrientedBox.IsScaledHullSupport(frame.GetPoint(prism.Points[prism.Triangles[triangle + 2]]), |
| | | 321 | | return false; |
| | | 322 | | } |
| | | 323 | | } |
| | | 324 | | |
| | | 325 | | |
| | | 326 | | private int VisitMixedEdges<TFrame>(in TFrame frame, MixedPrism prism, in WideScaledHullPenetration penetration, |
| | | 327 | | bool admissionOnly) where TFrame : struct, IForwardHullFrame3d |
| | | 328 | | { |
| | 909 | 329 | | WideDirection3d axis = penetration.Negate ? -penetration.Axis : penetration.Axis; |
| | 909 | 330 | | Signed832 support = WideOrientedBox.GetScaledHullSupport(frame, prism.Points, axis, false); |
| | 1818 | 331 | | int count = 0; bool hasEdge = false; |
| | 22170 | 332 | | for (int edge = 0; edge < prism.Edges.Length; edge += 2) |
| | | 333 | | { |
| | 20542 | 334 | | int start = prism.Edges[edge], end = prism.Edges[edge + 1]; |
| | 10271 | 335 | | if (!WideOrientedBox.IsScaledHullSupport(frame.GetPoint(prism.Points[start]), axis, support) |
| | 10271 | 336 | | || !WideOrientedBox.IsScaledHullSupport(frame.GetPoint(prism.Points[end]), axis, support)) continue; |
| | 598 | 337 | | hasEdge = true; |
| | 2869 | 338 | | foreach (var clip in GetMixedEdgeIntervals(start, end)) |
| | | 339 | | { |
| | 884 | 340 | | int firstSample = _mixedSamples.Count; |
| | 884 | 341 | | count += VisitMixedEdgeInterval(frame, prism, start, end, clip, penetration, admissionOnly); |
| | 1024 | 342 | | if (!admissionOnly) AddMixedRegion(_mixedMember, -1, default, default, default, firstSample, |
| | 140 | 343 | | edgeStart: start, edgeEnd: end, edgeInterval: clip); |
| | 979 | 344 | | if (admissionOnly && count > 0) return count; |
| | | 345 | | } |
| | | 346 | | } |
| | 1317 | 347 | | if (hasEdge) return count; |
| | 2874 | 348 | | for (int vertex = 0; vertex < prism.Points.Length; vertex++) |
| | | 349 | | { |
| | 1437 | 350 | | if (!WideOrientedBox.IsScaledHullSupport(frame.GetPoint(prism.Points[vertex]), axis, support)) continue; |
| | 311 | 351 | | int firstSample = _mixedSamples.Count; |
| | 311 | 352 | | count += VisitMixedEdgeInterval(frame, prism, vertex, vertex, default, penetration, admissionOnly); |
| | 312 | 353 | | if (!admissionOnly) AddMixedRegion(_mixedMember, -1, default, default, default, firstSample, |
| | 1 | 354 | | edgeStart: vertex, edgeEnd: vertex); |
| | 1 | 355 | | break; |
| | | 356 | | } |
| | 311 | 357 | | return count; |
| | | 358 | | } |
| | | 359 | | |
| | | 360 | | private int VisitMixedEdgeInterval<TFrame>(in TFrame frame, MixedPrism prism, int start, int end, |
| | | 361 | | in RationalSegmentInterval clip, in WideScaledHullPenetration penetration, bool admissionOnly) |
| | | 362 | | where TFrame : struct, IForwardHullFrame3d |
| | | 363 | | { |
| | 3699 | 364 | | _mixedEdgeStart = start; _mixedEdgeEnd = end; _mixedEdgeClip[0] = clip; |
| | 1233 | 365 | | Span<Vector3d> points = stackalloc Vector3d[2] { prism.Points[start], prism.Points[end] }; |
| | 1233 | 366 | | ReadOnlySpan<int> edges = stackalloc int[2] { 0, 1 }; |
| | 1233 | 367 | | int count = WideOrientedBox.VisitConvexHullContacts(_mixedMovingFrame, _mixedMoving!.Center, MixedMovingPoints, |
| | 1233 | 368 | | MixedMovingTriangles, MixedMovingEdges, frame, MixedEmbedded2DGeometry.GetCenter3D(_mixedLocal[_mixedMember] |
| | 1233 | 369 | | points, ReadOnlySpan<int>.Empty, edges, penetration, this, |
| | 1233 | 370 | | static (owner, contact) => owner.AddMixedSample(contact), this, admissionOnly); |
| | 1233 | 371 | | if (count == 0 && start != end) |
| | 715 | 372 | | count = VisitMixedEdgeInterior(frame, points, clip, penetration, admissionOnly); |
| | 1233 | 373 | | return count; |
| | | 374 | | } |
| | | 375 | | |
| | | 376 | | private int VisitMixedEdgeInterior<TFrame>(in TFrame frame, ReadOnlySpan<Vector3d> points, |
| | | 377 | | in RationalSegmentInterval clip, in WideScaledHullPenetration penetration, bool admissionOnly) |
| | | 378 | | where TFrame : struct, IForwardHullFrame3d |
| | | 379 | | { |
| | 715 | 380 | | Signed832 one = Signed832.ExtendValue(Signed192.Signed(1)); |
| | 1430 | 381 | | Signed832 ln = clip.IsClipped ? clip.WideLowerNumerator : default, ld = clip.IsClipped ? clip.WideLowerDenominat |
| | 1430 | 382 | | Signed832 un = clip.IsClipped ? clip.WideUpperNumerator : one, ud = clip.IsClipped ? clip.WideUpperDenominator : |
| | 721 | 383 | | if (RationalSegmentInterval.CompareParameters(ln, ld, un, ud) >= 0) return 0; |
| | 1418 | 384 | | Span<ulong> values = stackalloc ulong[6 * 14]; Span<int> signs = stackalloc int[6]; |
| | 1418 | 385 | | var n = ContactQuadratic.At(values, signs, 0, 14); var d = ContactQuadratic.At(values, signs, 1, 14); |
| | 709 | 386 | | var term = ContactQuadratic.At(values, signs, 2, 14); |
| | | 387 | | // The exact mediant is strictly inside a nonempty retained band. Its |
| | | 388 | | // <833-bit values fit the existing triangle witness's 2048-bit banks. |
| | 4254 | 389 | | n.Set(ln); term.Set(un); n.Add(term); d.Set(ld); term.Set(ud); d.Add(term); |
| | 709 | 390 | | WideDirection3d axis = penetration.Negate ? -penetration.Axis : penetration.Axis; |
| | 1360 | 391 | | if (!ContainsMixedPoint(0, 1, n, d, axis)) return 0; |
| | 58 | 392 | | Signed832 support = WideOrientedBox.GetScaledHullSupport(_mixedMovingFrame, MixedMovingPoints, axis, true); |
| | 116 | 393 | | RationalPoint3d a = frame.GetPoint(points[0]), b = frame.GetPoint(points[1]); |
| | 116 | 394 | | ReadOnlySpan<Vector3d> moving = MixedMovingPoints; ReadOnlySpan<int> triangles = MixedMovingTriangles; |
| | 852 | 395 | | for (int triangle = 0; triangle < triangles.Length; triangle += 3) |
| | | 396 | | { |
| | 1158 | 397 | | Vector3d x = moving[triangles[triangle]], y = moving[triangles[triangle + 1]], z = moving[triangles[triangle |
| | 386 | 398 | | if (!WideOrientedBox.IsScaledHullSupport(_mixedMovingFrame.GetPoint(x), axis, support) |
| | 386 | 399 | | || !WideOrientedBox.IsScaledHullSupport(_mixedMovingFrame.GetPoint(y), axis, support) |
| | 386 | 400 | | || !WideOrientedBox.IsScaledHullSupport(_mixedMovingFrame.GetPoint(z), axis, support)) continue; |
| | 42 | 401 | | if (!WideTriangleRelations.TryGetProjectedFrameSegmentFaceAnchors(new(x, y, z), _mixedMovingFrame, a, b, |
| | 42 | 402 | | default, default, MixedEmbedded2DGeometry.GetCenter3D(_mixedLocal[_mixedMember]), _mixedMoving!.Center, |
| | 42 | 403 | | 0, out FixedPointAnchor target, out FixedPointAnchor source, requirePhysicalTargetBoundary: false, |
| | 42 | 404 | | materialize: !admissionOnly, wideN: n, wideD: d)) continue; |
| | 26 | 405 | | if (!admissionOnly) AddMixedSample(new(source, target, axis.GetNormal(false), |
| | 8 | 406 | | penetration.GetRoundedDepth(out bool clamped), clamped)); |
| | 18 | 407 | | return 1; |
| | | 408 | | } |
| | 40 | 409 | | return 0; |
| | | 410 | | } |
| | | 411 | | |
| | | 412 | | private int VisitMixedFaces(in WideScaledHullPenetration penetration, bool admissionOnly) |
| | | 413 | | { |
| | 7010 | 414 | | int source = _mixedMember, count = 0; |
| | 3505 | 415 | | bool rigid = _mixedUsingRigid; |
| | 3505 | 416 | | MixedPrism original = _mixedPrisms[source]; |
| | 3505 | 417 | | WideDirection3d axis = penetration.Negate ? -penetration.Axis : penetration.Axis; |
| | 3505 | 418 | | RationalPoint3d reference = _mixedQuaternion ? MixedSupportPoint(original.Frame, original, axis) |
| | 3505 | 419 | | : MixedSupportPoint(original.NativeFrame, original, axis); |
| | 17956 | 420 | | for (int member = 0; member < _mixedLocal.Count; member++) |
| | | 421 | | { |
| | 6937 | 422 | | if (_mixedComponents[member] != _mixedComponents[source] || !MaySupplyMixedFace(member, axis)) continue; |
| | 5846 | 423 | | MixedPrism prism = _mixedPrisms[member]; |
| | 5846 | 424 | | RationalPoint3d support = _mixedQuaternion ? MixedSupportPoint(prism.Frame, prism, axis) |
| | 5846 | 425 | | : MixedSupportPoint(prism.NativeFrame, prism, axis); |
| | 5846 | 426 | | Signed192 denominator = _mixedQuaternion ? original.Frame.RotationDenominator : original.NativeFrame.Rotatio |
| | 5846 | 427 | | if (!WideTriangleRelations.DotScaledDirections(axis, |
| | 5846 | 428 | | WideTriangleRelations.GetScaledPointDifference(support, reference, denominator)).IsZero) continue; |
| | 4395 | 429 | | _mixedMember = member; |
| | 4395 | 430 | | _mixedUsingRigid = _mixedMovingFrame.IsRigid && prism.Rigid; |
| | 4395 | 431 | | count += _mixedQuaternion ? VisitMixedFaces(prism.Frame, prism, penetration, admissionOnly) |
| | 4395 | 432 | | : VisitMixedFaces(prism.NativeFrame, prism, penetration, admissionOnly); |
| | 4395 | 433 | | if (admissionOnly && count > 0) break; |
| | | 434 | | } |
| | 7010 | 435 | | _mixedMember = source; _mixedUsingRigid = rigid; |
| | 3505 | 436 | | return count; |
| | | 437 | | } |
| | | 438 | | |
| | | 439 | | private static RationalPoint3d MixedSupportPoint<TFrame>(in TFrame frame, MixedPrism prism, |
| | | 440 | | in WideDirection3d axis) where TFrame : struct, IForwardHullFrame3d |
| | | 441 | | { |
| | 13552 | 442 | | Signed832 support = WideOrientedBox.GetScaledHullSupport(frame, prism.Points, axis, maximize: false); |
| | 13552 | 443 | | int vertex = 0; |
| | | 444 | | // Admitted nonempty geometry has a vertex attaining its exact support. |
| | 95190 | 445 | | while (!WideOrientedBox.IsScaledHullSupport(frame.GetPoint(prism.Points[vertex]), axis, support)) vertex++; |
| | 13552 | 446 | | return frame.GetPoint(prism.Points[vertex]); |
| | | 447 | | } |
| | | 448 | | |
| | | 449 | | private bool MaySupplyMixedFace(int member, in WideDirection3d normal) |
| | | 450 | | { |
| | 6936 | 451 | | FixedBoundBox bounds = _mixedLocal[member].MixedBounds3D; |
| | 48968 | 452 | | for (int axis = 0; axis < 3; axis++) |
| | 18638 | 453 | | if ((axis == 0 ? normal.X : axis == 1 ? normal.Y : normal.Z).IsZero |
| | 18638 | 454 | | && (bounds.Max[axis] < _mixedMoving!.Bounds.Min[axis] |
| | 19728 | 455 | | || bounds.Min[axis] > _mixedMoving.Bounds.Max[axis])) return false; |
| | 5846 | 456 | | return true; |
| | | 457 | | } |
| | | 458 | | |
| | | 459 | | private int VisitMixedFaces<TFrame>(in TFrame frame, MixedPrism prism, in WideScaledHullPenetration penetration, |
| | | 460 | | bool admissionOnly) where TFrame : struct, IForwardHullFrame3d |
| | | 461 | | { |
| | 4395 | 462 | | WideDirection3d axis = penetration.Negate ? -penetration.Axis : penetration.Axis; |
| | 4395 | 463 | | Signed832 support = WideOrientedBox.GetScaledHullSupport(frame, prism.Points, axis, maximize: false); |
| | 4395 | 464 | | int emitted = 0; |
| | 4395 | 465 | | _mixedSupplier = _mixedMember; |
| | 55018 | 466 | | for (int face = 0; face < prism.Offsets.Length + 2; face++) |
| | | 467 | | { |
| | 24578 | 468 | | int count = face < 2 ? prism.Offsets.Length : 4; |
| | 24580 | 469 | | if (_mixedFace.Length < count) Array.Resize(ref _mixedFace, count); |
| | 24580 | 470 | | if (_mixedFaceTriangles.Length < 3 * (count - 2)) Array.Resize(ref _mixedFaceTriangles, 3 * (count - 2)); |
| | 24580 | 471 | | if (_mixedFaceEdges.Length < 2 * count) Array.Resize(ref _mixedFaceEdges, 2 * count); |
| | 245688 | 472 | | for (int vertex = 0; vertex < count; vertex++) |
| | | 473 | | { |
| | 196532 | 474 | | int edge = face - 2, next = (edge + 1) % prism.Offsets.Length; |
| | 98266 | 475 | | int index = face < 2 ? 2 * vertex + face |
| | 98266 | 476 | | : vertex == 0 ? 2 * edge : vertex == 1 ? 2 * next : vertex == 2 ? 2 * next + 1 : 2 * edge + 1; |
| | 98266 | 477 | | _mixedFace[vertex] = prism.Points[index]; |
| | 196532 | 478 | | _mixedFaceEdges[2 * vertex] = vertex; _mixedFaceEdges[2 * vertex + 1] = (vertex + 1) % count; |
| | 98266 | 479 | | if (vertex < 2) continue; |
| | 49110 | 480 | | int triangle = 3 * (vertex - 2); |
| | 147330 | 481 | | _mixedFaceTriangles[triangle] = 0; _mixedFaceTriangles[triangle + 1] = vertex - 1; _mixedFaceTriangles[t |
| | | 482 | | } |
| | 24578 | 483 | | bool supporting = true; |
| | 245688 | 484 | | for (int vertex = 0; vertex < count; vertex++) |
| | 98266 | 485 | | supporting &= WideArithmetic.SubtractSigned832(WideOrientedBox.ProjectScaledHullPoint( |
| | 98266 | 486 | | frame.GetPoint(_mixedFace[vertex]), axis, Signed192.One), support).IsZero; |
| | 24578 | 487 | | if (!supporting) continue; |
| | 4365 | 488 | | if (face >= 2) |
| | | 489 | | { |
| | 16476 | 490 | | for (int bandIndex = 0; bandIndex < prism.BandCount; bandIndex++) |
| | | 491 | | { |
| | 5512 | 492 | | MixedBand band = prism.Bands[bandIndex]; |
| | 51130 | 493 | | foreach (var span in band.Boundary.Spans) |
| | | 494 | | { |
| | 20740 | 495 | | if (span.First != face - 2) continue; |
| | 5524 | 496 | | if (span.Kind != SpanKind.Joined) |
| | 4305 | 497 | | emitted += VisitMixedSide(frame, span, band.Lower, band.Upper, penetration, admissionOnly, b |
| | 6898 | 498 | | if (admissionOnly && emitted > 0) return emitted; |
| | | 499 | | } |
| | | 500 | | } |
| | 2726 | 501 | | continue; |
| | | 502 | | } |
| | 265 | 503 | | int firstSample = _mixedSamples.Count; |
| | 265 | 504 | | emitted += _mixedUsingRigid |
| | 265 | 505 | | ? VisitMixedRigidFace(prism, count, admissionOnly) |
| | 265 | 506 | | : WideOrientedBox.VisitConvexHullContacts(_mixedMovingFrame, _mixedMoving!.Center, |
| | 265 | 507 | | MixedMovingPoints, MixedMovingTriangles, MixedMovingEdges, frame, |
| | 265 | 508 | | MixedEmbedded2DGeometry.GetCenter3D(_mixedLocal[_mixedMember]), _mixedFace.AsSpan(0, count), |
| | 265 | 509 | | _mixedFaceTriangles.AsSpan(0, 3 * (count - 2)), _mixedFaceEdges.AsSpan(0, 2 * count), penetration, |
| | 265 | 510 | | this, static (owner, contact) => owner.AddMixedSample(contact), admissionOnly: admissionOnly); |
| | 408 | 511 | | if (!admissionOnly) AddMixedRegion(_mixedMember, face, default, _mixedFace[0].Y, _mixedFace[0].Y, firstSampl |
| | 355 | 512 | | if (admissionOnly && emitted > 0) return emitted; |
| | | 513 | | } |
| | 2931 | 514 | | return emitted; |
| | | 515 | | } |
| | | 516 | | |
| | | 517 | | private int VisitMixedSide<TFrame>(in TFrame frame, in BoundarySpan2D span, |
| | | 518 | | Fixed64 lower, Fixed64 upper, in WideScaledHullPenetration penetration, bool admissionOnly, int band) |
| | | 519 | | where TFrame : struct, IForwardHullFrame3d |
| | | 520 | | { |
| | 4305 | 521 | | MixedPrism prism = _mixedPrisms[_mixedMember]; |
| | 4305 | 522 | | int firstSample = _mixedSamples.Count; |
| | | 523 | | int emitted; |
| | 4305 | 524 | | if (_mixedUsingRigid && !span.Interval.IsClipped |
| | 4305 | 525 | | && lower == -_mixedLocal[_mixedMember].MixedHalfThickness && upper == _mixedLocal[_mixedMember].MixedHalfThi |
| | 2387 | 526 | | emitted = VisitMixedRigidFace(prism, 4, admissionOnly); |
| | | 527 | | else |
| | | 528 | | { |
| | 1918 | 529 | | FixedTriangle rectangle = new(new Vector3d(span.Start.X, lower, span.Start.Y), |
| | 1918 | 530 | | new(span.End.X, lower, span.End.Y), new(span.Start.X, upper, span.Start.Y)); |
| | 1918 | 531 | | emitted = WideOrientedBox.VisitProjectedHullRectangleContacts(_mixedMovingFrame, _mixedMoving!.Center, |
| | 1918 | 532 | | MixedMovingPoints, MixedMovingTriangles, MixedMovingEdges, frame, |
| | 1918 | 533 | | MixedEmbedded2DGeometry.GetCenter3D(_mixedLocal[_mixedMember]), rectangle, span.Interval, |
| | 1918 | 534 | | penetration, this, static (owner, contact) => owner.AddMixedSample(contact), admissionOnly); |
| | | 535 | | } |
| | 6695 | 536 | | if (!admissionOnly) AddMixedRegion(_mixedMember, span.First + 2, span, lower, upper, firstSample, band); |
| | 4305 | 537 | | return emitted; |
| | | 538 | | } |
| | | 539 | | |
| | | 540 | | private int VisitMixedRigidFace(MixedPrism prism, int count, bool admissionOnly) => |
| | 2630 | 541 | | WideOrientedBox.VisitConvexHullContacts(_mixedMovingFrame.RigidOrigin, _mixedMovingFrame.Rotation, |
| | 2630 | 542 | | MixedMovingPoints, MixedMovingTriangles, MixedMovingEdges, prism.Frame.RigidOrigin, prism.Frame.Rotation, |
| | 2630 | 543 | | _mixedFace.AsSpan(0, count), _mixedFaceTriangles.AsSpan(0, 3 * (count - 2)), _mixedFaceEdges.AsSpan(0, 2 * c |
| | 2630 | 544 | | _mixedRigidPenetration, this, static (owner, contact) => owner.AddMixedSample(contact), admissionOnly: admis |
| | | 545 | | |
| | | 546 | | private int FindMixedGroup(in WideScaledHullPenetration penetration) |
| | | 547 | | { |
| | 4478 | 548 | | WideDirection3d axis = penetration.Negate ? -penetration.Axis : penetration.Axis; |
| | 10034 | 549 | | for (int group = 0; group < _mixedGroups.Count; group++) |
| | | 550 | | { |
| | 1470 | 551 | | MixedGroup existing = _mixedGroups[group]; |
| | 1470 | 552 | | if (existing.Component == _mixedComponents[_mixedMember] && existing.Feature == _mixedMovingFeature |
| | 1470 | 553 | | && WideDirection3d.AreParallel(axis, existing.Axis) |
| | 1470 | 554 | | && WideDirection3d.Dot(axis, existing.Axis).Sign > 0 |
| | 1470 | 555 | | && !existing.Penetration.ShouldReplace(penetration.Overlap, penetration.SquaredAxisLength, penetration.C |
| | 1470 | 556 | | && !penetration.ShouldReplace(existing.Penetration.Overlap, existing.Penetration.SquaredAxisLength, |
| | 2401 | 557 | | existing.Penetration.CommonDenominator)) return group; |
| | | 558 | | } |
| | 3547 | 559 | | return _mixedGroups.Count; |
| | | 560 | | } |
| | | 561 | | |
| | | 562 | | ReadOnlySpan<RationalSegmentInterval> IScaledConvexHullContactDomain.GetEdgeIntervals(bool first, int start, int end |
| | | 563 | | in WideDirection3d normal) |
| | 4235 | 564 | | => first ? MixedFullInterval : _mixedEdgeClip; |
| | | 565 | | |
| | | 566 | | private ReadOnlySpan<RationalSegmentInterval> GetMixedEdgeIntervals(int start, int end) |
| | | 567 | | { |
| | 3581 | 568 | | _mixedIntervals.FastClear(); |
| | 3581 | 569 | | MixedPrism prism = _mixedPrisms[_mixedMember]; |
| | 7162 | 570 | | Vector3d a = prism.Points[start], b = prism.Points[end]; |
| | 3581 | 571 | | if (a.Y == b.Y) |
| | | 572 | | { |
| | 6632 | 573 | | for (int bandIndex = 0; bandIndex < prism.BandCount; bandIndex++) |
| | 17848 | 574 | | foreach (var span in prism.Bands[bandIndex].Boundary.Spans) |
| | 7156 | 575 | | if (span.Interval.TryMapToSegment(new FixedSegment(new(span.Start.X, a.Y, span.Start.Y), |
| | 8948 | 576 | | new(span.End.X, a.Y, span.End.Y)), new FixedSegment(a, b), out var mapped)) _mixedIntervals.Add( |
| | | 577 | | } |
| | | 578 | | else |
| | | 579 | | { |
| | | 580 | | // Neighbor caps split an original vertical edge into exact bands. |
| | 9420 | 581 | | for (int bandIndex = 0; bandIndex < prism.BandCount; bandIndex++) |
| | | 582 | | { |
| | 2677 | 583 | | MixedBand band = prism.Bands[bandIndex]; |
| | 5354 | 584 | | Fixed64 lower = band.Lower - a.Y, upper = band.Upper - a.Y; |
| | | 585 | | // Original vertical prism edges are always bottom to top. |
| | 2677 | 586 | | Signed320 denominator = Signed320.ExtendValue(Signed192.Raw(b.Y - a.Y)); |
| | 2677 | 587 | | _mixedIntervals.Add(new(Signed320.ExtendValue(Signed192.Raw(lower)), denominator, |
| | 2677 | 588 | | Signed320.ExtendValue(Signed192.Raw(upper)), denominator)); |
| | | 589 | | } |
| | | 590 | | } |
| | 3581 | 591 | | return _mixedIntervals.AsReadOnlySpan(); |
| | | 592 | | } |
| | | 593 | | |
| | | 594 | | bool IScaledConvexHullContactDomain.ContainsPoint(bool first, int start, int end, in Signed832 numerator, |
| | | 595 | | in Signed832 denominator, in WideDirection3d normal) |
| | | 596 | | { |
| | | 597 | | System.Diagnostics.Debug.Assert(!first); |
| | 1716 | 598 | | Span<ulong> values = stackalloc ulong[52]; Span<int> signs = stackalloc int[4]; |
| | 1716 | 599 | | var n = ContactQuadratic.At(values, signs, 0, 13); var d = ContactQuadratic.At(values, signs, 1, 13); |
| | 1716 | 600 | | n.Set(numerator); d.Set(denominator); |
| | 858 | 601 | | return ContainsMixedPoint(start, end, n, d, normal); |
| | | 602 | | } |
| | | 603 | | |
| | | 604 | | bool IScaledConvexHullContactDomain.ContainsPoint(bool first, int start, int end, scoped ContactQuadratic numerator, |
| | | 605 | | scoped ContactQuadratic denominator, in WideDirection3d normal) => |
| | 776 | 606 | | first || ContainsMixedPoint(start, end, numerator, denominator, normal); |
| | | 607 | | |
| | | 608 | | private bool ContainsMixedPoint(int start, int end, scoped ContactQuadratic n, scoped ContactQuadratic d, |
| | | 609 | | in WideDirection3d normal) |
| | | 610 | | { |
| | | 611 | | // This domain belongs to the current original edge, represented by the |
| | | 612 | | // two-point visitor as indices 0/1. Preserve its exact parameter. |
| | 1955 | 613 | | if (start == end) |
| | | 614 | | { |
| | 662 | 615 | | Span<ulong> storage = stackalloc ulong[52]; Span<int> signs = stackalloc int[4]; |
| | 662 | 616 | | var endpoint = ContactQuadratic.At(storage, signs, 0, 13); var one = ContactQuadratic.At(storage, signs, 1, |
| | 662 | 617 | | endpoint.Set(Signed576.ExtendValue(Signed192.Signed(start))); one.Set(Signed576.ExtendValue(Signed192.Signed |
| | 419 | 618 | | if (_mixedEdgeClip[0].IsClipped && !_mixedEdgeClip[0].Contains(endpoint, one, ReadOnlySpan<ulong>.Empty)) re |
| | 243 | 619 | | start = end = start == 0 ? _mixedEdgeStart : _mixedEdgeEnd; |
| | | 620 | | } |
| | | 621 | | else |
| | | 622 | | { |
| | 1738 | 623 | | if (_mixedEdgeClip[0].IsClipped && !_mixedEdgeClip[0].Contains(n, d, ReadOnlySpan<ulong>.Empty)) return fals |
| | 3020 | 624 | | start = _mixedEdgeStart; end = _mixedEdgeEnd; |
| | | 625 | | } |
| | 1753 | 626 | | MixedPrism source = _mixedPrisms[_mixedMember]; |
| | 3506 | 627 | | Vector3d a = source.Points[start], b = source.Points[end]; |
| | 1753 | 628 | | FixedSegment footprint = new(new(a.X, Fixed64.Zero, a.Z), new(b.X, Fixed64.Zero, b.Z)); |
| | 3506 | 629 | | _mixedFan.FastClear(); _mixedFan.Add(_mixedMember); |
| | 7452 | 630 | | for (int cursor = 0; cursor < _mixedFan.Count; cursor++) |
| | | 631 | | { |
| | 6616 | 632 | | int member = _mixedFan[cursor]; MixedPrism prism = _mixedPrisms[member]; |
| | 3308 | 633 | | if (member != _mixedMember) |
| | | 634 | | { |
| | | 635 | | // Every queried target edge lies on the selected support plane; |
| | | 636 | | // its original endpoint is an exact projection reference too. |
| | 1555 | 637 | | bool supports = _mixedQuaternion ? SupportsMixedFan(prism.Frame, source.Frame, a, prism.Points, normal) |
| | 1555 | 638 | | : SupportsMixedFan(prism.NativeFrame, source.NativeFrame, a, prism.Points, normal); |
| | 2890 | 639 | | if (!supports) return false; |
| | | 640 | | } |
| | 11516 | 641 | | for (int bandIndex = 0; bandIndex < prism.BandCount; bandIndex++) |
| | | 642 | | { |
| | 3785 | 643 | | MixedBand band = prism.Bands[bandIndex]; |
| | | 644 | | // A reached fan member already contains the queried height. Each |
| | | 645 | | // prepared band is covered completely by every Joined neighbor. |
| | 3785 | 646 | | Fixed64 delta = _mixedLocal[member].MixedSlabCenterY - _mixedLocal[_mixedMember].MixedSlabCenterY; |
| | 3785 | 647 | | if (!MixedHeightContains(a.Y, b.Y, n, d, delta + band.Lower, delta + band.Upper)) continue; |
| | 20748 | 648 | | foreach (var span in band.Boundary.Spans) |
| | | 649 | | { |
| | 8191 | 650 | | if (span.Kind != SpanKind.Joined || _mixedFan.Contains(span.OtherMember)) continue; |
| | 2856 | 651 | | FixedSegment edge = new(new(span.Start.X, Fixed64.Zero, span.Start.Y), new(span.End.X, Fixed64.Zero, |
| | 2856 | 652 | | int location = _mixedQuaternion |
| | 2856 | 653 | | ? WideTriangleRelations.LocateFrameEdgePoint(footprint, source.FootFrame, n, d, edge, prism.Foot |
| | 2856 | 654 | | : WideTriangleRelations.LocateFrameEdgePoint(footprint, source.NativeFootFrame, n, d, edge, pris |
| | 4649 | 655 | | if (location >= 0) _mixedFan.Add(span.OtherMember); |
| | | 656 | | } |
| | | 657 | | } |
| | | 658 | | } |
| | 418 | 659 | | return true; |
| | | 660 | | } |
| | | 661 | | |
| | | 662 | | private static bool SupportsMixedFan<TFrame>(in TFrame frame, in TFrame source, Vector3d reference, |
| | | 663 | | ReadOnlySpan<Vector3d> points, in WideDirection3d normal) where TFrame : struct, IForwardHullFrame3d |
| | | 664 | | { |
| | 24849 | 665 | | RationalPoint3d origin = source.GetPoint(reference); |
| | 275121 | 666 | | foreach (Vector3d vertex in points) |
| | 118356 | 667 | | if (WideTriangleRelations.DotScaledDirections(normal, WideTriangleRelations.GetScaledPointDifference( |
| | 129645 | 668 | | frame.GetPoint(vertex), origin, source.RotationDenominator)).Sign < 0) return false; |
| | 13560 | 669 | | return true; |
| | | 670 | | } |
| | | 671 | | |
| | | 672 | | private bool TryGetMixedHeights(int source, int other, out Fixed64 lower, out Fixed64 upper) |
| | | 673 | | { |
| | 8832 | 674 | | LSCollider2D a = _mixedLocal[source], b = _mixedLocal[other]; |
| | 4416 | 675 | | lower = upper = default; |
| | 4454 | 676 | | if (a.MixedBounds3D.Max.Y < b.MixedBounds3D.Min.Y || a.MixedBounds3D.Min.Y > b.MixedBounds3D.Max.Y) return false |
| | | 677 | | // Intersecting bounded extrusions have a bounded center difference. |
| | | 678 | | // Absolute cap coordinates may exceed Fixed64 and must not be clamped. |
| | 4378 | 679 | | Fixed64 delta = b.MixedSlabCenterY - a.MixedSlabCenterY; |
| | 8756 | 680 | | lower = delta - b.MixedHalfThickness; upper = delta + b.MixedHalfThickness; |
| | 4378 | 681 | | return true; |
| | | 682 | | } |
| | | 683 | | |
| | | 684 | | private static bool MixedHeightContains(Fixed64 first, Fixed64 second, scoped ContactQuadratic n, |
| | | 685 | | scoped ContactQuadratic d, Fixed64 minimum, Fixed64 maximum) |
| | | 686 | | { |
| | 4375 | 687 | | if (second < minimum || first > maximum) return false; |
| | 3972 | 688 | | if (first == second) return true; |
| | 4836 | 689 | | Fixed64 lower = minimum > first ? minimum : first, upper = maximum < second ? maximum : second; |
| | 2418 | 690 | | Signed320 denominator = Signed320.ExtendValue(Signed192.Raw(second - first)); |
| | 2418 | 691 | | var interval = new RationalSegmentInterval(Signed320.ExtendValue(Signed192.Raw(lower - first)), denominator, |
| | 2418 | 692 | | Signed320.ExtendValue(Signed192.Raw(upper - first)), denominator); |
| | 2418 | 693 | | return interval.Contains(n, d, ReadOnlySpan<ulong>.Empty); |
| | | 694 | | } |
| | | 695 | | |
| | | 696 | | private void AddMixedSample(FixedContactAnchors contact) |
| | | 697 | | { |
| | 7930 | 698 | | bool represented = contact.SecondAnchor.TryGetPointInFrame(_mixedMoving!.Center, out Vector3d point); |
| | | 699 | | System.Diagnostics.Debug.Assert(represented); |
| | 7930 | 700 | | _mixedSamples.Add(new(_mixedGroup, _mixedSupplier, point, contact)); |
| | 7930 | 701 | | } |
| | | 702 | | |
| | | 703 | | private readonly struct MixedSample : IPlaneContactSample |
| | | 704 | | { |
| | | 705 | | internal readonly int Group, Supplier; |
| | | 706 | | internal readonly FixedContactAnchors Contact; |
| | | 707 | | public Vector3d Point { get; } |
| | | 708 | | internal MixedSample(int group, int supplier, Vector3d point, FixedContactAnchors contact) |
| | 139190 | 709 | | { Group = group; Supplier = supplier; Point = point; Contact = contact; } |
| | | 710 | | } |
| | | 711 | | |
| | | 712 | | private readonly struct MixedGroup |
| | | 713 | | { |
| | | 714 | | internal readonly int Component, Carrier, Feature; |
| | | 715 | | internal readonly WideScaledHullPenetration Penetration; |
| | 6316 | 716 | | internal WideDirection3d Axis => Penetration.Negate ? -Penetration.Axis : Penetration.Axis; |
| | | 717 | | internal MixedGroup(int component, int carrier, WideScaledHullPenetration penetration, int feature) |
| | 7070 | 718 | | { Component = component; Carrier = carrier; Penetration = penetration; Feature = feature; } |
| | | 719 | | } |
| | | 720 | | |
| | | 721 | | private sealed class MixedPrism |
| | | 722 | | { |
| | 1000 | 723 | | internal readonly SwiftList<MixedBand> Bands = new(); |
| | | 724 | | internal int BandCount; |
| | 1000 | 725 | | internal Vector2d[] Offsets = Array.Empty<Vector2d>(); |
| | 1000 | 726 | | internal Vector3d[] Points = Array.Empty<Vector3d>(); |
| | 2000 | 727 | | internal int[] Triangles = Array.Empty<int>(), Edges = Array.Empty<int>(); |
| | | 728 | | internal ScaledRigidFrame3d Frame; |
| | | 729 | | internal ScaledRigidFrame3d FootFrame; |
| | | 730 | | internal NativePrismYawFrame3d NativeFrame, NativeFootFrame; |
| | | 731 | | internal bool Rigid, Joined, SphereHasConcaveFace; |
| | | 732 | | private int _id; |
| | | 733 | | private long _lifetime; |
| | | 734 | | private uint _broad, _dependency; |
| | 17394 | 735 | | internal bool Matches(LSCollider2D member) => _id == member.Id && _lifetime == member.LifetimeVersion |
| | 17394 | 736 | | && _broad == member.BroadPhaseVersion && _dependency == member.ContactDependencyVersion; |
| | | 737 | | internal void Prepare(LSCollider2D member, bool quaternion) |
| | | 738 | | { |
| | 1336 | 739 | | int count = member.VertexCount; |
| | 1336 | 740 | | if (Offsets.Length != count) |
| | | 741 | | { |
| | 2000 | 742 | | Offsets = new Vector2d[count]; Points = new Vector3d[2 * count]; |
| | 2000 | 743 | | Triangles = new int[12 * count - 12]; Edges = new int[6 * count]; |
| | | 744 | | } |
| | 18608 | 745 | | for (int vertex = 0; vertex < count; vertex++) Offsets[vertex] = member.GetScaledLocalVertexUnchecked(vertex |
| | 1336 | 746 | | CollisionDetectionMixed.FillPrismGeometry(Offsets, member.MixedHalfThickness, Points, Triangles, Edges); |
| | 1336 | 747 | | Vector3d center = MixedEmbedded2DGeometry.GetCenter3D(member); |
| | 1336 | 748 | | Rigid = quaternion || member.ConvexRotation == Fixed64.Zero; |
| | 2656 | 749 | | if (Rigid) Frame = ScaledRigidFrame3d.Rigid(center, FixedQuaternion.FromAxisAngle(Vector3d.Up, -member.Conve |
| | 1336 | 750 | | Vector3d footprint = new(center.X, Fixed64.Zero, center.Z); |
| | 1555 | 751 | | if (quaternion) FootFrame = ScaledRigidFrame3d.Rigid(footprint, Frame.Rotation); |
| | 2234 | 752 | | else { NativeFrame = new(center, member.ConvexRotation); NativeFootFrame = new(footprint, member.ConvexRotat |
| | 4008 | 753 | | _id = member.Id; _lifetime = member.LifetimeVersion; _broad = member.BroadPhaseVersion; |
| | 1336 | 754 | | _dependency = member.ContactDependencyVersion; |
| | 1336 | 755 | | } |
| | | 756 | | } |
| | | 757 | | |
| | | 758 | | private sealed class MixedBand |
| | | 759 | | { |
| | 1404 | 760 | | internal readonly MeshSurfaceBoundaryDomain2D Boundary = new(); |
| | | 761 | | internal Fixed64 Lower, Upper; |
| | | 762 | | } |
| | | 763 | | } |