| | | 1 | | //======================================================================= |
| | | 2 | | // SolidBody2D.ContinuousCollision.Rotational.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 FixedMathSharp; |
| | | 10 | | using FixedMathSharp.Geometry; |
| | | 11 | | using Gravitas.Colliders; |
| | | 12 | | using Gravitas.CollisionHandling; |
| | | 13 | | using Gravitas.Support; |
| | | 14 | | using SwiftCollections; |
| | | 15 | | using SwiftCollections.Query; |
| | | 16 | | |
| | | 17 | | namespace Gravitas; |
| | | 18 | | |
| | | 19 | | /// <content>Searches rotating planar trajectories against spatial targets and resolves mixed contact manifolds.</conten |
| | | 20 | | public sealed partial class SolidBody2D |
| | | 21 | | { |
| | 6448 | 22 | | private readonly ContactManifold _rotationalMixedKnownManifold = new(); |
| | 6448 | 23 | | private readonly ContactManifold _rotationalMixedEarliestManifold = new(); |
| | | 24 | | private bool _rotationalMixedKnownContactIsClosing; |
| | | 25 | | private bool _rotationalMixedEarliestContactIsClosing; |
| | | 26 | | private ContactWarmStartCache _mixedContinuousCollisionImpulses; |
| | | 27 | | private ExactContactImpulseState?[]? _mixedContinuousCollisionExactImpulses; |
| | 6448 | 28 | | private readonly SwiftList<LSCollider> _rotationalMixedStatic3DCandidates = new(); |
| | 6448 | 29 | | private readonly SwiftList<int> _rotationalMixedMoving3DCandidateIds = new(); |
| | | 30 | | private SwiftHashSet<StaticSurface>? _rotationalMixedSurfaceCandidates; |
| | | 31 | | private SwiftList<(LSMeshCollider Supplier, int Owner)>? _resolvedMixedSurfaceFamilies; |
| | | 32 | | |
| | | 33 | | private ReadOnlySpan<(LSMeshCollider Supplier, int Owner)> ResolvedMixedSurfaceFamilies(StaticSurface surface) => |
| | 202 | 34 | | _resolvedMixedSurfaceFamilies is { } families |
| | 202 | 35 | | && _continuousCollisionHandoffIgnoredCollider3D is LSMeshCollider previous |
| | 202 | 36 | | && previous.StaticSurface == surface ? families.AsReadOnlySpan() : default; |
| | | 37 | | |
| | | 38 | | internal void SetMixedContinuousCollisionSamplePose( |
| | | 39 | | Vector2d position, |
| | | 40 | | Fixed64 rotation) |
| | | 41 | | { |
| | 5947 | 42 | | _position = position; |
| | 5947 | 43 | | _rotation = rotation; |
| | 5947 | 44 | | Collider.RebuildRuntimeShapeOnly(); |
| | 5947 | 45 | | } |
| | | 46 | | |
| | | 47 | | internal FixedBoundVolume ResolveMixedContinuousCollisionTrajectoryBounds(Fixed64 radius) |
| | | 48 | | { |
| | 6513 | 49 | | DynamicCcdPlanarBounds planarBounds = |
| | 6513 | 50 | | ResolveContinuousCollisionTrajectoryBounds(radius); |
| | 6513 | 51 | | return new FixedBoundVolume( |
| | 6513 | 52 | | new Vector3d( |
| | 6513 | 53 | | planarBounds.MinX, |
| | 6513 | 54 | | Collider.MixedSlabCenterY - Collider.MixedHalfThickness, |
| | 6513 | 55 | | planarBounds.MinZ), |
| | 6513 | 56 | | new Vector3d( |
| | 6513 | 57 | | planarBounds.MaxX, |
| | 6513 | 58 | | Collider.MixedSlabCenterY + Collider.MixedHalfThickness, |
| | 6513 | 59 | | planarBounds.MaxZ)); |
| | | 60 | | } |
| | | 61 | | |
| | | 62 | | private readonly struct RotationalMixed3DHit |
| | | 63 | | { |
| | | 64 | | public RotationalMixed3DHit( |
| | | 65 | | Fixed64 safeTime, |
| | | 66 | | MixedContact contact, |
| | | 67 | | bool hasContact, |
| | | 68 | | bool hasClosingContact, |
| | | 69 | | Fixed64 contactTime, |
| | | 70 | | LSCollider target, |
| | | 71 | | ContactManifold manifold) |
| | | 72 | | { |
| | 170 | 73 | | SafeTime = safeTime; |
| | 170 | 74 | | Contact = contact; |
| | 170 | 75 | | HasContact = hasContact; |
| | 170 | 76 | | HasClosingContact = hasClosingContact; |
| | 170 | 77 | | ContactTime = contactTime; |
| | 170 | 78 | | Target = target; |
| | 170 | 79 | | Manifold = manifold; |
| | 170 | 80 | | } |
| | | 81 | | |
| | | 82 | | public Fixed64 SafeTime { get; } |
| | | 83 | | |
| | | 84 | | public MixedContact Contact { get; } |
| | | 85 | | |
| | | 86 | | public bool HasContact { get; } |
| | | 87 | | public bool HasClosingContact { get; } |
| | | 88 | | |
| | | 89 | | public Fixed64 ContactTime { get; } |
| | | 90 | | |
| | | 91 | | public LSCollider Target { get; } |
| | | 92 | | public ContactManifold Manifold { get; } |
| | | 93 | | } |
| | | 94 | | |
| | | 95 | | private bool TryFindEarliestMixedRotationalContinuousCollision( |
| | | 96 | | Vector2d startPosition, |
| | | 97 | | Vector2d displacement, |
| | | 98 | | Fixed64 startRotation, |
| | | 99 | | Fixed64 angularDelta, |
| | | 100 | | Fixed64 angularDistance, |
| | | 101 | | Fixed64 pivotRadius, |
| | | 102 | | Fixed64 elapsedTime, |
| | | 103 | | Fixed64 remainingTime, |
| | | 104 | | bool reuseCandidates, |
| | | 105 | | out RotationalMixed3DHit hit) |
| | | 106 | | { |
| | 382 | 107 | | hit = default; |
| | 382 | 108 | | if (!Context.Settings.RuntimeMode.RunsMixedContacts()) |
| | 143 | 109 | | return false; |
| | | 110 | | |
| | 239 | 111 | | _rotationalMixedSurfaceCandidates?.Clear(); |
| | | 112 | | try |
| | | 113 | | { |
| | 239 | 114 | | if (!reuseCandidates) |
| | 194 | 115 | | GatherMixedRotationalContinuousCollisionCandidates( |
| | 194 | 116 | | startPosition, |
| | 194 | 117 | | displacement, |
| | 194 | 118 | | pivotRadius); |
| | | 119 | | |
| | 239 | 120 | | bool found = false; |
| | 239 | 121 | | Fixed64 earliestTime = Fixed64.One; |
| | 239 | 122 | | int earliestOrder = int.MaxValue; |
| | 239 | 123 | | MixedContact earliestContact = default; |
| | 239 | 124 | | bool earliestHasContact = false; |
| | 239 | 125 | | Fixed64 earliestContactTime = Fixed64.Zero; |
| | 239 | 126 | | LSCollider? earliestTarget = null; |
| | | 127 | | |
| | 762 | 128 | | for (int i = 0; i < _rotationalMixedStatic3DCandidates.Count; i++) |
| | | 129 | | { |
| | 142 | 130 | | LSCollider target = _rotationalMixedStatic3DCandidates[i]; |
| | 142 | 131 | | if (target is LSMeshCollider { StaticSurface: { } surface } |
| | 142 | 132 | | && (!IsValidMixedRotationalContinuousCollisionTarget(target) |
| | 142 | 133 | | || !(_rotationalMixedSurfaceCandidates ??= new()).Add(surface))) continue; |
| | 94 | 134 | | ConsiderMixedRotationalContinuousCollisionCandidate( |
| | 94 | 135 | | target, |
| | 94 | 136 | | startPosition, |
| | 94 | 137 | | displacement, |
| | 94 | 138 | | startRotation, |
| | 94 | 139 | | angularDelta, |
| | 94 | 140 | | angularDistance, |
| | 94 | 141 | | pivotRadius, |
| | 94 | 142 | | elapsedTime, |
| | 94 | 143 | | remainingTime, |
| | 94 | 144 | | ref found, |
| | 94 | 145 | | ref earliestTime, |
| | 94 | 146 | | ref earliestOrder, |
| | 94 | 147 | | ref earliestContact, |
| | 94 | 148 | | ref earliestHasContact, |
| | 94 | 149 | | ref earliestContactTime, |
| | 94 | 150 | | ref earliestTarget); |
| | | 151 | | } |
| | | 152 | | |
| | 802 | 153 | | for (int i = 0; i < _rotationalMixedMoving3DCandidateIds.Count; i++) |
| | | 154 | | { |
| | 162 | 155 | | SolidBody targetBody = Context.Physics.GetContinuousCollisionCandidate( |
| | 162 | 156 | | _rotationalMixedMoving3DCandidateIds[i]); |
| | 162 | 157 | | if (!IsMovingMixedRotationalContinuousCollisionTarget(targetBody)) |
| | | 158 | | { |
| | | 159 | | continue; |
| | | 160 | | } |
| | | 161 | | |
| | 112 | 162 | | ConsiderMixedRotationalContinuousCollisionCandidate( |
| | 112 | 163 | | targetBody.Collider, |
| | 112 | 164 | | startPosition, |
| | 112 | 165 | | displacement, |
| | 112 | 166 | | startRotation, |
| | 112 | 167 | | angularDelta, |
| | 112 | 168 | | angularDistance, |
| | 112 | 169 | | pivotRadius, |
| | 112 | 170 | | elapsedTime, |
| | 112 | 171 | | remainingTime, |
| | 112 | 172 | | ref found, |
| | 112 | 173 | | ref earliestTime, |
| | 112 | 174 | | ref earliestOrder, |
| | 112 | 175 | | ref earliestContact, |
| | 112 | 176 | | ref earliestHasContact, |
| | 112 | 177 | | ref earliestContactTime, |
| | 112 | 178 | | ref earliestTarget); |
| | | 179 | | } |
| | | 180 | | |
| | 239 | 181 | | if (!found) |
| | 69 | 182 | | return false; |
| | | 183 | | |
| | 170 | 184 | | hit = new RotationalMixed3DHit( |
| | 170 | 185 | | earliestTime, |
| | 170 | 186 | | earliestContact, |
| | 170 | 187 | | earliestHasContact, |
| | 170 | 188 | | _rotationalMixedEarliestContactIsClosing, |
| | 170 | 189 | | earliestContactTime, |
| | 170 | 190 | | earliestTarget!, |
| | 170 | 191 | | _rotationalMixedEarliestManifold); |
| | 170 | 192 | | return true; |
| | | 193 | | } |
| | 478 | 194 | | finally { _rotationalMixedSurfaceCandidates?.Clear(); } |
| | 239 | 195 | | } |
| | | 196 | | |
| | | 197 | | private void GatherMixedRotationalContinuousCollisionCandidates( |
| | | 198 | | Vector2d startPosition, |
| | | 199 | | Vector2d displacement, |
| | | 200 | | Fixed64 pivotRadius) |
| | | 201 | | { |
| | 194 | 202 | | _rotationalMixedStatic3DCandidates.FastClear(); |
| | 194 | 203 | | _rotationalMixedMoving3DCandidateIds.FastClear(); |
| | 194 | 204 | | Vector3d start3D = new( |
| | 194 | 205 | | startPosition.X, |
| | 194 | 206 | | Collider.MixedSlabCenterY, |
| | 194 | 207 | | startPosition.Y); |
| | 194 | 208 | | Vector3d end3D = new( |
| | 194 | 209 | | startPosition.X + displacement.X, |
| | 194 | 210 | | Collider.MixedSlabCenterY, |
| | 194 | 211 | | startPosition.Y + displacement.Y); |
| | 194 | 212 | | FixedBoundVolume bounds = DynamicCcdCandidateIndex.CreateBoundsBetween( |
| | 194 | 213 | | start3D, |
| | 194 | 214 | | end3D, |
| | 194 | 215 | | new Vector3d( |
| | 194 | 216 | | pivotRadius, |
| | 194 | 217 | | Collider.MixedHalfThickness, |
| | 194 | 218 | | pivotRadius)); |
| | 194 | 219 | | Context.MixedCollisions.Collect3DCandidatesInMixedBounds( |
| | 194 | 220 | | bounds.Min, |
| | 194 | 221 | | bounds.Max, |
| | 194 | 222 | | PhysicsLayerMask.All, |
| | 194 | 223 | | _rotationalMixedStatic3DCandidates, |
| | 194 | 224 | | staticStyleOnly: true, |
| | 194 | 225 | | cachePartitionRefresh: true); |
| | 194 | 226 | | _rotationalMixedStatic3DCandidates.SortInPlace(default(ReplayOrderComparer<LSCollider>)); |
| | | 227 | | |
| | 194 | 228 | | SwiftList<int> candidateIds = |
| | 194 | 229 | | Context.Physics.QueryContinuousCollisionCandidates(bounds); |
| | 194 | 230 | | _rotationalMixedMoving3DCandidateIds.EnsureCapacity(candidateIds.Count); |
| | 622 | 231 | | for (int i = 0; i < candidateIds.Count; i++) |
| | 117 | 232 | | _rotationalMixedMoving3DCandidateIds.Add(candidateIds[i]); |
| | 194 | 233 | | } |
| | | 234 | | |
| | | 235 | | private void ConsiderMixedRotationalContinuousCollisionCandidate( |
| | | 236 | | LSCollider target, |
| | | 237 | | Vector2d startPosition, |
| | | 238 | | Vector2d displacement, |
| | | 239 | | Fixed64 startRotation, |
| | | 240 | | Fixed64 angularDelta, |
| | | 241 | | Fixed64 angularDistance, |
| | | 242 | | Fixed64 pivotRadius, |
| | | 243 | | Fixed64 elapsedTime, |
| | | 244 | | Fixed64 remainingTime, |
| | | 245 | | ref bool found, |
| | | 246 | | ref Fixed64 earliestTime, |
| | | 247 | | ref int earliestOrder, |
| | | 248 | | ref MixedContact earliestContact, |
| | | 249 | | ref bool earliestHasContact, |
| | | 250 | | ref Fixed64 earliestContactTime, |
| | | 251 | | ref LSCollider? earliestTarget) |
| | | 252 | | { |
| | | 253 | | // Kinematic clamps against immovable targets consume geometry/time only; |
| | | 254 | | // dynamic stopping and movable-target response still require full contacts. |
| | 206 | 255 | | int candidateOrder = target is LSMeshCollider { StaticSurface: { } surface } |
| | 206 | 256 | | ? surface.ContinuousOrder : ((IPhysicsColliderRegistryItem)target).ReplayOrder; |
| | 206 | 257 | | if (!IsValidMixedRotationalContinuousCollisionTarget(target) |
| | 206 | 258 | | || !TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget( |
| | 206 | 259 | | target, |
| | 206 | 260 | | startPosition, |
| | 206 | 261 | | displacement, |
| | 206 | 262 | | startRotation, |
| | 206 | 263 | | angularDelta, |
| | 206 | 264 | | angularDistance, |
| | 206 | 265 | | pivotRadius, |
| | 206 | 266 | | elapsedTime, |
| | 206 | 267 | | remainingTime, |
| | 206 | 268 | | Fixed64.Zero, |
| | 206 | 269 | | Fixed64.One, |
| | 206 | 270 | | needsResponseContact: !IsKinematic || target.Body?.HasSolverMobility == true, |
| | 206 | 271 | | out Fixed64 candidateTime, |
| | 206 | 272 | | out MixedContact candidateContact, |
| | 206 | 273 | | out bool candidateHasContact, |
| | 206 | 274 | | out Fixed64 candidateContactTime) |
| | 206 | 275 | | || !ContinuousCollisionMath.ShouldReplaceContinuousCollisionHit( |
| | 206 | 276 | | candidateTime, |
| | 206 | 277 | | candidateOrder, |
| | 206 | 278 | | found, |
| | 206 | 279 | | earliestTime, |
| | 206 | 280 | | earliestOrder)) |
| | | 281 | | { |
| | 36 | 282 | | return; |
| | | 283 | | } |
| | | 284 | | |
| | 170 | 285 | | found = true; |
| | 170 | 286 | | earliestTime = candidateTime; |
| | 170 | 287 | | earliestOrder = candidateOrder; |
| | 170 | 288 | | earliestContact = candidateContact; |
| | 170 | 289 | | earliestHasContact = candidateHasContact; |
| | 170 | 290 | | earliestContactTime = candidateContactTime; |
| | 170 | 291 | | earliestTarget = target; |
| | 170 | 292 | | _rotationalMixedEarliestManifold.CopyFrom(_rotationalMixedKnownManifold, Context.FrameCount); |
| | 170 | 293 | | _rotationalMixedEarliestContactIsClosing = _rotationalMixedKnownContactIsClosing; |
| | 170 | 294 | | } |
| | | 295 | | |
| | | 296 | | private bool TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget( |
| | | 297 | | LSCollider target, |
| | | 298 | | Vector2d startPosition, |
| | | 299 | | Vector2d displacement, |
| | | 300 | | Fixed64 startRotation, |
| | | 301 | | Fixed64 angularDelta, |
| | | 302 | | Fixed64 angularDistance, |
| | | 303 | | Fixed64 pivotRadius, |
| | | 304 | | Fixed64 elapsedTime, |
| | | 305 | | Fixed64 remainingTime, |
| | | 306 | | Fixed64 searchStart, |
| | | 307 | | Fixed64 searchEnd, |
| | | 308 | | bool needsResponseContact, |
| | | 309 | | out Fixed64 safeTime, |
| | | 310 | | out MixedContact contact, |
| | | 311 | | out bool hasContact, |
| | | 312 | | out Fixed64 contactTime) |
| | | 313 | | { |
| | 240 | 314 | | safeTime = Fixed64.Zero; |
| | 240 | 315 | | contact = default; |
| | 240 | 316 | | hasContact = false; |
| | 240 | 317 | | contactTime = Fixed64.Zero; |
| | 240 | 318 | | _rotationalMixedKnownManifold.BeginUpdate(Context.FrameCount); |
| | 240 | 319 | | _rotationalMixedKnownContactIsClosing = false; |
| | 240 | 320 | | StaticSurface? surface = (target as LSMeshCollider)?.StaticSurface; |
| | 240 | 321 | | ReadOnlySpan<(LSMeshCollider Supplier, int Owner)> resolved = surface == null |
| | 240 | 322 | | ? default : ResolvedMixedSurfaceFamilies(surface); |
| | 240 | 323 | | SolidBody? targetBody = target.Body; |
| | 240 | 324 | | bool samplesTargetMotion = targetBody != null |
| | 240 | 325 | | && IsMovingMixedRotationalContinuousCollisionTarget(targetBody); |
| | 240 | 326 | | Vector3d originalTargetPosition = targetBody?.Position3d ?? Vector3d.Zero; |
| | 240 | 327 | | FixedQuaternion originalTargetRotation = targetBody?.Rotation |
| | 240 | 328 | | ?? FixedQuaternion.Identity; |
| | 240 | 329 | | if (samplesTargetMotion) |
| | 158 | 330 | | targetBody!.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken); |
| | | 331 | | |
| | | 332 | | try |
| | | 333 | | { |
| | 240 | 334 | | SampleMixedRotationalContinuousPairPose(startPosition, displacement, startRotation, angularDelta, |
| | 240 | 335 | | searchStart, elapsedTime, remainingTime, targetBody, samplesTargetMotion); |
| | | 336 | | // Resolve an actual closing start before midpoint refinement can |
| | | 337 | | // introduce penetration. Retain the complete native matched rows. |
| | 240 | 338 | | if (needsResponseContact && (surface?.IntersectsMixedContinuous(Collider, resolved: resolved) |
| | 240 | 339 | | ?? CollisionDetectionMixed.IntersectsForContinuousCollision(target, Collider))) |
| | | 340 | | { |
| | 20 | 341 | | BuildMixedRotationalContinuousContact(target, |
| | 20 | 342 | | ResolveRotationalFrameFraction(elapsedTime, remainingTime, searchStart), |
| | 20 | 343 | | out contact, out _rotationalMixedKnownContactIsClosing); |
| | 20 | 344 | | if (_rotationalMixedKnownContactIsClosing) |
| | | 345 | | { |
| | 14 | 346 | | _rotationalMixedKnownManifold.CopyFrom(Context.CollisionScratch.MixedManifold, Context.FrameCount); |
| | 14 | 347 | | safeTime = contactTime = searchStart; |
| | 14 | 348 | | hasContact = true; |
| | 14 | 349 | | return true; |
| | | 350 | | } |
| | | 351 | | } |
| | 226 | 352 | | Span<RotationalInterval> intervals = |
| | 226 | 353 | | stackalloc RotationalInterval[ |
| | 226 | 354 | | ContinuousCollisionMath.RotationalIntervalMaxDepth + 2]; |
| | 226 | 355 | | int intervalCount = 1; |
| | 226 | 356 | | int processedNodeCount = 0; |
| | 226 | 357 | | bool hasKnownContact = false; |
| | 226 | 358 | | Fixed64 knownContactTime = Fixed64.One; |
| | 226 | 359 | | intervals[0] = new RotationalInterval( |
| | 226 | 360 | | searchStart, |
| | 226 | 361 | | searchEnd, |
| | 226 | 362 | | depth: 0); |
| | | 363 | | |
| | 5771 | 364 | | while (intervalCount > 0) |
| | | 365 | | { |
| | 5749 | 366 | | RotationalInterval interval = intervals[--intervalCount]; |
| | 5749 | 367 | | Fixed64 midpoint = |
| | 5749 | 368 | | (interval.LowerTime + interval.UpperTime) * Fixed64.Half; |
| | 5749 | 369 | | Fixed64 intervalSpan = interval.UpperTime - interval.LowerTime; |
| | 5749 | 370 | | SampleMixedRotationalContinuousPairPose( |
| | 5749 | 371 | | startPosition, |
| | 5749 | 372 | | displacement, |
| | 5749 | 373 | | startRotation, |
| | 5749 | 374 | | angularDelta, |
| | 5749 | 375 | | midpoint, |
| | 5749 | 376 | | elapsedTime, |
| | 5749 | 377 | | remainingTime, |
| | 5749 | 378 | | targetBody, |
| | 5749 | 379 | | samplesTargetMotion); |
| | 5749 | 380 | | processedNodeCount++; |
| | | 381 | | |
| | 5749 | 382 | | bool hasMotionBound = |
| | 5749 | 383 | | ContinuousCollisionMath.TryResolveRotationalIntervalMotionBound( |
| | 5749 | 384 | | displacement, |
| | 5749 | 385 | | angularDistance, |
| | 5749 | 386 | | pivotRadius, |
| | 5749 | 387 | | intervalSpan, |
| | 5749 | 388 | | out Fixed64 motionBound); |
| | 5749 | 389 | | if (hasMotionBound && samplesTargetMotion) |
| | | 390 | | { |
| | 4785 | 391 | | Fixed64 lowerFraction = ResolveRotationalFrameFraction( |
| | 4785 | 392 | | elapsedTime, |
| | 4785 | 393 | | remainingTime, |
| | 4785 | 394 | | interval.LowerTime); |
| | 4785 | 395 | | Fixed64 upperFraction = ResolveRotationalFrameFraction( |
| | 4785 | 396 | | elapsedTime, |
| | 4785 | 397 | | remainingTime, |
| | 4785 | 398 | | interval.UpperTime); |
| | 4785 | 399 | | Fixed64 targetRadius = targetBody!.ResolveContinuousCollisionProxyRadius(); |
| | 4785 | 400 | | bool targetBoundResolved = targetBody.TryResolveContinuousCollisionMotionBound( |
| | 4785 | 401 | | lowerFraction, |
| | 4785 | 402 | | upperFraction, |
| | 4785 | 403 | | targetRadius, |
| | 4785 | 404 | | out Fixed64 targetMotionBound); |
| | 4785 | 405 | | bool combinedBoundResolved = Fixed64.TryAdd( |
| | 4785 | 406 | | motionBound, |
| | 4785 | 407 | | targetMotionBound, |
| | 4785 | 408 | | out motionBound); |
| | 4785 | 409 | | hasMotionBound = targetBoundResolved & combinedBoundResolved; |
| | | 410 | | } |
| | | 411 | | |
| | 5749 | 412 | | bool sampleHasContact = surface != null ? surface.IntersectsMixedContinuous(Collider, resolved: resolved |
| | 5749 | 413 | | : CollisionDetectionMixed.IntersectsForContinuousCollision(target, Collider); |
| | 5749 | 414 | | if (sampleHasContact |
| | 5749 | 415 | | && (!hasKnownContact || midpoint < knownContactTime)) |
| | | 416 | | { |
| | 1148 | 417 | | hasKnownContact = true; |
| | 1148 | 418 | | knownContactTime = midpoint; |
| | | 419 | | } |
| | | 420 | | |
| | 5749 | 421 | | Fixed64 shapeGap = default; |
| | 5749 | 422 | | bool shapeGapSupported = false; |
| | 5749 | 423 | | bool hasShapeGap = surface == null && CollisionDetectionMixed.TryGetRotationalSeparationGap( |
| | 5749 | 424 | | target, |
| | 5749 | 425 | | Collider, |
| | 5749 | 426 | | out shapeGap, |
| | 5749 | 427 | | out shapeGapSupported); |
| | 5749 | 428 | | Fixed64 characteristicScale = target is LSSphereCollider sphere |
| | 5749 | 429 | | ? FixedMath.Max(pivotRadius, sphere.ScaledRadius) |
| | 5749 | 430 | | : pivotRadius; |
| | 5749 | 431 | | bool shapeGapCertifiesSeparation = shapeGapSupported |
| | 5749 | 432 | | && hasShapeGap |
| | 5749 | 433 | | && ContinuousCollisionMath.TrySubtractClosestFeatureUncertainty( |
| | 5749 | 434 | | shapeGap, |
| | 5749 | 435 | | characteristicScale, |
| | 5749 | 436 | | out Fixed64 conservativeShapeGap) |
| | 5749 | 437 | | && conservativeShapeGap > motionBound; |
| | 5749 | 438 | | bool fallbackBoundsCertifySeparation = !sampleHasContact && hasMotionBound && !shapeGapSupported |
| | 5749 | 439 | | && (surface != null ? !surface.IntersectsMixedContinuous(Collider, motionBound, resolved) |
| | 5749 | 440 | | : ContinuousCollisionBounds.AreSeparated(target, Collider, motionBound) |
| | 5749 | 441 | | || !CollisionDetectionMixed.IntersectsForContinuousCollision(target, Collider, motionBound)) |
| | 5749 | 442 | | if (!sampleHasContact |
| | 5749 | 443 | | && hasMotionBound |
| | 5749 | 444 | | && (shapeGapCertifiesSeparation || fallbackBoundsCertifySeparation)) |
| | | 445 | | { |
| | | 446 | | continue; |
| | | 447 | | } |
| | | 448 | | |
| | 3854 | 449 | | if (ContinuousCollisionMath.TryResolveRotationalSearchLimit( |
| | 3854 | 450 | | interval, |
| | 3854 | 451 | | processedNodeCount, |
| | 3854 | 452 | | hasKnownContact, |
| | 3854 | 453 | | knownContactTime, |
| | 3854 | 454 | | out safeTime, |
| | 3854 | 455 | | out contactTime, |
| | 3854 | 456 | | out hasContact)) |
| | | 457 | | { |
| | | 458 | | // A later witness cannot authorize response at an earlier unresolved frontier. |
| | 204 | 459 | | if (hasContact && needsResponseContact && contactTime == safeTime) |
| | | 460 | | { |
| | | 461 | | // Reconstruct complete matched rows at the admitted response pose. |
| | 70 | 462 | | SampleMixedRotationalContinuousPairPose(startPosition, displacement, startRotation, angularDelta |
| | 70 | 463 | | elapsedTime, remainingTime, targetBody, samplesTargetMotion); |
| | 70 | 464 | | BuildMixedRotationalContinuousContact(target, |
| | 70 | 465 | | ResolveRotationalFrameFraction(elapsedTime, remainingTime, contactTime), |
| | 70 | 466 | | out contact, out _rotationalMixedKnownContactIsClosing); |
| | 70 | 467 | | _rotationalMixedKnownManifold.CopyFrom(Context.CollisionScratch.MixedManifold, Context.FrameCoun |
| | | 468 | | } |
| | 204 | 469 | | return true; |
| | | 470 | | } |
| | | 471 | | |
| | 3650 | 472 | | int childDepth = interval.Depth + 1; |
| | 3650 | 473 | | if (sampleHasContact) |
| | | 474 | | { |
| | 1131 | 475 | | intervals[intervalCount++] = |
| | 1131 | 476 | | new RotationalInterval( |
| | 1131 | 477 | | interval.LowerTime, |
| | 1131 | 478 | | midpoint, |
| | 1131 | 479 | | childDepth); |
| | 1131 | 480 | | continue; |
| | | 481 | | } |
| | | 482 | | |
| | 2519 | 483 | | intervals[intervalCount++] = |
| | 2519 | 484 | | new RotationalInterval( |
| | 2519 | 485 | | midpoint, |
| | 2519 | 486 | | interval.UpperTime, |
| | 2519 | 487 | | childDepth); |
| | 2519 | 488 | | intervals[intervalCount++] = |
| | 2519 | 489 | | new RotationalInterval( |
| | 2519 | 490 | | interval.LowerTime, |
| | 2519 | 491 | | midpoint, |
| | 2519 | 492 | | childDepth); |
| | | 493 | | } |
| | | 494 | | |
| | 22 | 495 | | return false; |
| | | 496 | | } |
| | | 497 | | finally |
| | | 498 | | { |
| | 240 | 499 | | if (targetBody != null) |
| | 167 | 500 | | targetBody.SetMixedContinuousCollisionSamplePose( |
| | 167 | 501 | | originalTargetPosition, |
| | 167 | 502 | | originalTargetRotation); |
| | 240 | 503 | | } |
| | 240 | 504 | | } |
| | | 505 | | |
| | | 506 | | private void SampleMixedRotationalContinuousPairPose( |
| | | 507 | | Vector2d startPosition, |
| | | 508 | | Vector2d displacement, |
| | | 509 | | Fixed64 startRotation, |
| | | 510 | | Fixed64 angularDelta, |
| | | 511 | | Fixed64 sampleTime, |
| | | 512 | | Fixed64 elapsedTime, |
| | | 513 | | Fixed64 remainingTime, |
| | | 514 | | SolidBody? targetBody, |
| | | 515 | | bool samplesTargetMotion) |
| | | 516 | | { |
| | 6059 | 517 | | _position = startPosition + displacement * sampleTime; |
| | 6059 | 518 | | _rotation = startRotation + angularDelta * sampleTime; |
| | 6059 | 519 | | Collider.RebuildRuntimeShapeOnly(); |
| | | 520 | | |
| | 6059 | 521 | | if (!samplesTargetMotion) |
| | 1069 | 522 | | return; |
| | | 523 | | |
| | 4990 | 524 | | Fixed64 frameFraction = ResolveRotationalFrameFraction( |
| | 4990 | 525 | | elapsedTime, |
| | 4990 | 526 | | remainingTime, |
| | 4990 | 527 | | sampleTime); |
| | 4990 | 528 | | targetBody!.SetMixedContinuousCollisionSamplePose( |
| | 4990 | 529 | | targetBody.SampleContinuousCollisionPosition(frameFraction), |
| | 4990 | 530 | | targetBody.SampleContinuousCollisionRotation(frameFraction)); |
| | 4990 | 531 | | } |
| | | 532 | | |
| | | 533 | | private bool TryGetExactSphereCircleTranslationalContact( |
| | | 534 | | SolidBody target, |
| | | 535 | | ContinuousCollisionMotionSegment3D targetSegment, |
| | | 536 | | FixedSegment targetPath, |
| | | 537 | | Vector3d targetDisplacement, |
| | | 538 | | Fixed64 targetTravelDistance, |
| | | 539 | | FixedSegment2d sourcePath, |
| | | 540 | | Vector2d sourceDisplacement, |
| | | 541 | | Fixed64 sourceTravelDistance, |
| | | 542 | | Fixed64 sourceStartRotation, |
| | | 543 | | Fixed64 sourceAngularDelta, |
| | | 544 | | out bool requiresRotationalFallback, |
| | | 545 | | out Fixed64 relativeContactDistance, |
| | | 546 | | out Fixed64 sourceContactDistance, |
| | | 547 | | out Fixed64 localClosingSpeed, |
| | | 548 | | out MixedContact contact) |
| | | 549 | | { |
| | 93 | 550 | | requiresRotationalFallback = true; |
| | 93 | 551 | | relativeContactDistance = default; |
| | 93 | 552 | | sourceContactDistance = default; |
| | 93 | 553 | | localClosingSpeed = default; |
| | 93 | 554 | | contact = default; |
| | 93 | 555 | | if (Collider is not LSCircleCollider2D circle |
| | 93 | 556 | | || target.Collider is not LSSphereCollider sphere |
| | 93 | 557 | | || (targetSegment.AngularDistance != Fixed64.Zero |
| | 93 | 558 | | && sphere.ScaledOffset != Vector3d.Zero) |
| | 93 | 559 | | || (sourceAngularDelta != Fixed64.Zero |
| | 93 | 560 | | && circle.ScaledLocalOffset != Vector2d.Zero)) |
| | | 561 | | { |
| | 47 | 562 | | return false; |
| | | 563 | | } |
| | | 564 | | |
| | 46 | 565 | | Vector2d originalSourcePosition = _position; |
| | 46 | 566 | | Fixed64 originalSourceRotation = _rotation; |
| | 46 | 567 | | Vector3d originalTargetPosition = target.Position3d; |
| | 46 | 568 | | FixedQuaternion originalTargetRotation = target.Rotation; |
| | | 569 | | try |
| | | 570 | | { |
| | 46 | 571 | | _position = sourcePath.Start; |
| | 46 | 572 | | _rotation = sourceStartRotation; |
| | 46 | 573 | | Collider.RebuildRuntimeShapeOnly(); |
| | 46 | 574 | | target.SetMixedContinuousCollisionSamplePose( |
| | 46 | 575 | | targetPath.Start, |
| | 46 | 576 | | targetSegment.StartRotation); |
| | 46 | 577 | | Vector3d cylinderStart = new( |
| | 46 | 578 | | circle.Center.X, |
| | 46 | 579 | | circle.MixedSlabCenterY, |
| | 46 | 580 | | circle.Center.Y); |
| | 46 | 581 | | Vector3d sphereStart = sphere.Center; |
| | 46 | 582 | | Vector3d cylinderDisplacement = sourceDisplacement.ToVector3d(Fixed64.Zero); |
| | 46 | 583 | | Vector3d relativeDisplacement = |
| | 46 | 584 | | ContinuousCollisionSweepRange.ValidateRelativeDisplacement( |
| | 46 | 585 | | targetDisplacement, |
| | 46 | 586 | | cylinderDisplacement, |
| | 46 | 587 | | out Fixed64 totalDistance); |
| | 46 | 588 | | if (!WideFiniteAxisIntersection |
| | 46 | 589 | | .TryGetSphericallyExpandedFiniteCylinderDirectionFirstDistanceFromHalfAxisLength( |
| | 46 | 590 | | sphereStart, |
| | 46 | 591 | | relativeDisplacement, |
| | 46 | 592 | | cylinderStart, |
| | 46 | 593 | | Vector3d.Up, |
| | 46 | 594 | | circle.MixedHalfThickness, |
| | 46 | 595 | | circle.ScaledRadius, |
| | 46 | 596 | | sphere.ScaledRadius, |
| | 46 | 597 | | totalDistance, |
| | 46 | 598 | | out Fixed64 contactDistance)) |
| | | 599 | | { |
| | 1 | 600 | | requiresRotationalFallback = false; |
| | 1 | 601 | | return false; |
| | | 602 | | } |
| | | 603 | | |
| | 45 | 604 | | relativeContactDistance = contactDistance; |
| | 45 | 605 | | _ = Fixed64.TryMultiplyDivide( |
| | 45 | 606 | | sourceTravelDistance, |
| | 45 | 607 | | contactDistance, |
| | 45 | 608 | | totalDistance, |
| | 45 | 609 | | out sourceContactDistance); |
| | 45 | 610 | | Fixed64 candidateDistance = contactDistance; |
| | 92 | 611 | | for (int attempt = 0; attempt < 2; attempt++) |
| | | 612 | | { |
| | 46 | 613 | | _ = Fixed64.TryMultiplyDivide( |
| | 46 | 614 | | sourceTravelDistance, |
| | 46 | 615 | | candidateDistance, |
| | 46 | 616 | | totalDistance, |
| | 46 | 617 | | out Fixed64 sampleSourceDistance); |
| | 46 | 618 | | _ = Fixed64.TryMultiplyDivide( |
| | 46 | 619 | | targetTravelDistance, |
| | 46 | 620 | | candidateDistance, |
| | 46 | 621 | | totalDistance, |
| | 46 | 622 | | out Fixed64 targetContactDistance); |
| | 46 | 623 | | _position = sourcePath.GetPointAtDistance( |
| | 46 | 624 | | sampleSourceDistance, |
| | 46 | 625 | | sourceTravelDistance); |
| | 46 | 626 | | _rotation = sourceStartRotation; |
| | 46 | 627 | | Collider.RebuildRuntimeShapeOnly(); |
| | 46 | 628 | | target.SetMixedContinuousCollisionSamplePose( |
| | 46 | 629 | | targetPath.GetPointAtDistance( |
| | 46 | 630 | | targetContactDistance, |
| | 46 | 631 | | targetTravelDistance), |
| | 46 | 632 | | targetSegment.StartRotation); |
| | 46 | 633 | | ContactManifold manifold = Context.CollisionScratch.MixedManifold; |
| | 46 | 634 | | if (CollisionDetectionMixed.TryBuildManifold(target.Collider, Collider, manifold)) |
| | | 635 | | { |
| | | 636 | | // Relative displacement is sphere-minus-circle even when the source is 2D. |
| | 45 | 637 | | contact = MixedContact.FromManifoldContact(manifold.PrimaryContact); |
| | 45 | 638 | | bool closingResolved = Vector3d.TryDot( |
| | 45 | 639 | | relativeDisplacement, |
| | 45 | 640 | | contact.Normal3DTo2D, |
| | 45 | 641 | | out localClosingSpeed); |
| | 45 | 642 | | if (closingResolved |
| | 45 | 643 | | && localClosingSpeed > Fixed64.Epsilon) |
| | | 644 | | { |
| | 42 | 645 | | return true; |
| | | 646 | | } |
| | | 647 | | |
| | 3 | 648 | | if (closingResolved) |
| | 3 | 649 | | requiresRotationalFallback = false; |
| | | 650 | | |
| | 3 | 651 | | break; |
| | | 652 | | } |
| | | 653 | | |
| | 1 | 654 | | if (candidateDistance >= totalDistance) |
| | | 655 | | break; |
| | | 656 | | |
| | 1 | 657 | | candidateDistance = FixedMath.Min( |
| | 1 | 658 | | totalDistance, |
| | 1 | 659 | | candidateDistance + Fixed64.MinIncrement); |
| | | 660 | | } |
| | | 661 | | |
| | 3 | 662 | | relativeContactDistance = default; |
| | 3 | 663 | | sourceContactDistance = default; |
| | 3 | 664 | | localClosingSpeed = default; |
| | 3 | 665 | | contact = default; |
| | 3 | 666 | | return false; |
| | | 667 | | } |
| | | 668 | | finally |
| | | 669 | | { |
| | 46 | 670 | | _position = originalSourcePosition; |
| | 46 | 671 | | _rotation = originalSourceRotation; |
| | 46 | 672 | | Collider.RebuildRuntimeShapeOnly(); |
| | 46 | 673 | | target.SetMixedContinuousCollisionSamplePose( |
| | 46 | 674 | | originalTargetPosition, |
| | 46 | 675 | | originalTargetRotation); |
| | 46 | 676 | | } |
| | 46 | 677 | | } |
| | | 678 | | |
| | | 679 | | private void BuildMixedRotationalContinuousContact(LSCollider target, |
| | | 680 | | Fixed64 frameFraction, out MixedContact contact, out bool contactIsClosing) |
| | | 681 | | { |
| | 90 | 682 | | ContactManifold manifold = Context.CollisionScratch.MixedManifold; |
| | 90 | 683 | | contact = default; |
| | 90 | 684 | | contactIsClosing = false; |
| | | 685 | | // Admission already retained this geometric witness; closing rows only authorize response. |
| | 90 | 686 | | if (target is LSMeshCollider { StaticSurface: { } surface }) |
| | | 687 | | { |
| | 25 | 688 | | manifold.BeginUpdate(Context.FrameCount); |
| | 25 | 689 | | surface.AppendMixedContinuousContacts(Collider, manifold, ResolvedMixedSurfaceFamilies(surface)); |
| | | 690 | | } |
| | 65 | 691 | | else CollisionDetectionMixed.TryBuildDiscreteManifold(target, Collider, manifold); |
| | 90 | 692 | | SolidBody? other = target.Body; |
| | 90 | 693 | | ContactAnchor center3D = other?.GetCenterOfMassAnchor() ?? ContactAnchor.FromWorldPoint(target.Center); |
| | 90 | 694 | | var center2D = new ContactAnchor(new Vector3d(_position.X, Collider.MixedSlabCenterY, _position.Y), |
| | 90 | 695 | | FixedQuaternion.FromAxisAngle(Vector3d.Up, -_rotation), |
| | 90 | 696 | | ExactContactLever2D.ToSpatial(LocalCenterOfMassOffset)); |
| | 90 | 697 | | contactIsClosing = MixedContact.TrySelectClosingContact(manifold, center3D, |
| | 90 | 698 | | other?.SampleContinuousCollisionLinearVelocity(frameFraction) ?? Vector3d.Zero, |
| | 90 | 699 | | other?.SampleContinuousCollisionAngularVelocity(frameFraction) ?? Vector3d.Zero, |
| | 90 | 700 | | center2D, IsKinematic ? SampleContinuousCollisionLinearVelocity(frameFraction) : _linearVelocity, |
| | 90 | 701 | | IsKinematic ? SampleContinuousCollisionAngularVelocity(frameFraction) : _angularVelocity, out contact, Colli |
| | 90 | 702 | | } |
| | | 703 | | |
| | | 704 | | private bool IsValidMixedRotationalContinuousCollisionTarget(LSCollider target) |
| | | 705 | | { |
| | 684 | 706 | | bool resolvedDomain = target is LSMeshCollider { StaticSurface: { } surface } |
| | 684 | 707 | | && !ResolvedMixedSurfaceFamilies(surface).IsEmpty; |
| | 684 | 708 | | return target.IsActive |
| | 684 | 709 | | && !target.IsTrigger |
| | 684 | 710 | | && (resolvedDomain || !ContinuousCollisionCandidateOrdering.IsIgnoredTarget( |
| | 684 | 711 | | target, |
| | 684 | 712 | | _continuousCollisionHandoffIgnoredCollider3D)) |
| | 684 | 713 | | && Context.MixedCollisions.RequireCollisionPair(target, Collider); |
| | | 714 | | } |
| | | 715 | | |
| | | 716 | | private bool IsMovingMixedRotationalContinuousCollisionTarget(SolidBody target) |
| | | 717 | | { |
| | 438 | 718 | | if (target.IsKinematic) |
| | 159 | 719 | | target.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken); |
| | | 720 | | |
| | 438 | 721 | | return (IsKinematic || !target.ShouldOwnContinuousCollisionMovingPair( |
| | 438 | 722 | | HasContinuousCollisionRotationalMotion)) |
| | 438 | 723 | | && IsValidMixedRotationalContinuousCollisionTarget(target.Collider) |
| | 438 | 724 | | && (target.IsKinematic |
| | 438 | 725 | | ? target.HasContinuousCollisionMotion |
| | 438 | 726 | | : target.IsDynamic); |
| | | 727 | | } |
| | | 728 | | |
| | | 729 | | private bool TryApplyMixedTranslationalManifoldResponse(SolidBody target, |
| | | 730 | | Vector2d sourcePosition, Vector3d targetPosition, Fixed64 elapsedTime, Fixed64 remainingTime) |
| | | 731 | | { |
| | 6 | 732 | | Fixed64 fraction = ResolveContinuousCollisionFrameFraction(elapsedTime); |
| | 6 | 733 | | Fixed64 sourceRotation = SampleContinuousCollisionRotation(fraction); |
| | 6 | 734 | | FixedQuaternion targetRotation = target.SampleContinuousCollisionRotation(fraction); |
| | 6 | 735 | | Vector2d originalSourcePosition = Position; |
| | 6 | 736 | | Fixed64 originalSourceRotation = Rotation; |
| | 6 | 737 | | Vector3d originalTargetPosition = target.Position3d; |
| | 6 | 738 | | FixedQuaternion originalTargetRotation = target.Rotation; |
| | | 739 | | try |
| | | 740 | | { |
| | 6 | 741 | | SetMixedContinuousCollisionSamplePose(sourcePosition, sourceRotation); |
| | 6 | 742 | | target.SetMixedContinuousCollisionSamplePose(targetPosition, targetRotation); |
| | 6 | 743 | | if (!CollisionDetectionMixed.TryBuildManifold(target.Collider, Collider, _rotationalMixedKnownManifold)) |
| | 1 | 744 | | return false; |
| | 5 | 745 | | } |
| | | 746 | | finally |
| | | 747 | | { |
| | 6 | 748 | | SetMixedContinuousCollisionSamplePose(originalSourcePosition, originalSourceRotation); |
| | 6 | 749 | | target.SetMixedContinuousCollisionSamplePose(originalTargetPosition, originalTargetRotation); |
| | 6 | 750 | | } |
| | 5 | 751 | | if (!TryApplyMixedRotationalContinuousCollisionResponse(target.Collider, |
| | 5 | 752 | | MixedContact.FromManifoldContact(_rotationalMixedKnownManifold.PrimaryContact), Fixed64.Zero, |
| | 5 | 753 | | sourcePosition, Vector2d.Zero, sourceRotation, Fixed64.Zero, elapsedTime, remainingTime, |
| | 7 | 754 | | _rotationalMixedKnownManifold)) return false; |
| | | 755 | | // The rotational outer loop appends this segment itself; the translational owner needs it here. |
| | 3 | 756 | | if (HasSolverMobility) |
| | | 757 | | { |
| | 2 | 758 | | UpdateContinuousCollisionFrameTrajectory(sourcePosition, _linearVelocity, elapsedTime); |
| | 2 | 759 | | Context.Physics2D.RefreshContinuousCollisionCandidate(this); |
| | | 760 | | } |
| | 3 | 761 | | return true; |
| | 1 | 762 | | } |
| | | 763 | | |
| | | 764 | | internal bool TryApplyMixedRotationalContinuousCollisionResponse( |
| | | 765 | | LSCollider targetCollider, |
| | | 766 | | MixedContact contact, |
| | | 767 | | Fixed64 localContactTime, |
| | | 768 | | Vector2d sourceSegmentStart, |
| | | 769 | | Vector2d sourceDisplacement, |
| | | 770 | | Fixed64 sourceSegmentStartRotation, |
| | | 771 | | Fixed64 sourceAngularDelta, |
| | | 772 | | Fixed64 elapsedTime, |
| | | 773 | | Fixed64 remainingTime, |
| | | 774 | | ContactManifold? manifold = null) |
| | | 775 | | { |
| | 256 | 776 | | Fixed64 frameFraction = ResolveRotationalFrameFraction( |
| | 256 | 777 | | elapsedTime, |
| | 256 | 778 | | remainingTime, |
| | 256 | 779 | | localContactTime); |
| | 256 | 780 | | Fixed64 consumedTime = remainingTime * localContactTime; |
| | 256 | 781 | | Fixed64 impactElapsedTime = elapsedTime + consumedTime; |
| | 256 | 782 | | Fixed64 remainingAfterImpact = remainingTime - consumedTime; |
| | 256 | 783 | | Vector2d sourcePosition = sourceSegmentStart |
| | 256 | 784 | | + sourceDisplacement * localContactTime; |
| | 256 | 785 | | Fixed64 sourceRotation = CanonicalizeRotation( |
| | 256 | 786 | | sourceSegmentStartRotation + sourceAngularDelta * localContactTime); |
| | 256 | 787 | | Vector2d sourceLinearVelocity = IsKinematic |
| | 256 | 788 | | ? SampleContinuousCollisionLinearVelocity(frameFraction) |
| | 256 | 789 | | : _linearVelocity; |
| | 256 | 790 | | Fixed64 sourceAngularVelocity = IsKinematic |
| | 256 | 791 | | ? SampleContinuousCollisionAngularVelocity(frameFraction) |
| | 256 | 792 | | : _angularVelocity; |
| | | 793 | | |
| | 256 | 794 | | SolidBody? target = targetCollider.Body; |
| | 256 | 795 | | Vector3d targetPosition = target?.SampleContinuousCollisionPosition(frameFraction) |
| | 256 | 796 | | ?? targetCollider.Center; |
| | 256 | 797 | | FixedQuaternion targetRotation = target?.SampleContinuousCollisionRotation(frameFraction) |
| | 256 | 798 | | ?? FixedQuaternion.Identity; |
| | 256 | 799 | | Vector3d targetLinearVelocity = target?.SampleContinuousCollisionLinearVelocity(frameFraction) |
| | 256 | 800 | | ?? Vector3d.Zero; |
| | 256 | 801 | | Vector3d targetAngularVelocity = target?.SampleContinuousCollisionAngularVelocity(frameFraction) |
| | 256 | 802 | | ?? Vector3d.Zero; |
| | 256 | 803 | | SolidBody? targetResponseBody = target?.HasSolverMobility == true ? target : null; |
| | 256 | 804 | | SolidBody2D? sourceResponseBody = HasSolverMobility ? this : null; |
| | 256 | 805 | | ContactAnchor targetCenter = new(targetPosition, targetRotation, |
| | 256 | 806 | | target?.LocalCenterOfMassOffset ?? Vector3d.Zero); |
| | 256 | 807 | | Vector3d sourcePosition3D = new(sourcePosition.X, Collider.MixedSlabCenterY, sourcePosition.Y); |
| | 256 | 808 | | var sourceCenter = new ContactAnchor(sourcePosition3D, |
| | 256 | 809 | | FixedQuaternion.FromAxisAngle(Vector3d.Up, -sourceRotation), |
| | 256 | 810 | | ExactContactLever2D.ToSpatial(LocalCenterOfMassOffset)); |
| | 256 | 811 | | if (manifold == null) |
| | | 812 | | { |
| | 4 | 813 | | _rotationalMixedKnownManifold.BeginUpdate(Context.FrameCount); |
| | 4 | 814 | | _rotationalMixedKnownManifold.AddContact(default, contact.ToManifoldContact()); |
| | 4 | 815 | | manifold = _rotationalMixedKnownManifold; |
| | | 816 | | } |
| | 256 | 817 | | if (!MixedContact.TrySelectClosingContact(manifold, targetCenter, targetLinearVelocity, |
| | 256 | 818 | | targetAngularVelocity, sourceCenter, sourceLinearVelocity, sourceAngularVelocity, out _, Collider)) |
| | 2 | 819 | | return false; |
| | 254 | 820 | | var impact = new ContactResponseSnapshot(targetPosition, sourcePosition3D, targetCenter, sourceCenter, |
| | 254 | 821 | | targetLinearVelocity, targetAngularVelocity, ExactContactLever2D.ToSpatial(sourceLinearVelocity), |
| | 254 | 822 | | new Vector3d(Fixed64.Zero, -sourceAngularVelocity, Fixed64.Zero), sourceAngularVelocity); |
| | 254 | 823 | | if (!CollisionResponseMixed.TrySolveManifoldVelocity(targetCollider, Collider, manifold, impact, |
| | 254 | 824 | | ref _mixedContinuousCollisionImpulses, ref _mixedContinuousCollisionExactImpulses, |
| | 255 | 825 | | out ContactResponseSnapshot solved)) return false; |
| | 253 | 826 | | Vector3d postTargetLinearVelocity = solved.LinearA; |
| | 253 | 827 | | Vector3d postTargetAngularVelocity = solved.AngularA; |
| | 253 | 828 | | Vector2d postSourceLinearVelocity = solved.LinearB.ToVector2d(); |
| | 253 | 829 | | Fixed64 postSourceAngularVelocity = solved.PlanarAngularB; |
| | 253 | 830 | | Vector3d targetResolvedPosition = default; |
| | 253 | 831 | | bool sourceStateAvailable = sourceResponseBody == null |
| | 253 | 832 | | || CanAppendContinuousCollisionFrameSegment(impactElapsedTime) |
| | 253 | 833 | | && Context.Physics2D.CanAdmitContinuousCollisionCandidateRefresh(this); |
| | 253 | 834 | | bool targetStateAvailable = targetResponseBody == null |
| | 253 | 835 | | || targetResponseBody.CanApplyContinuousCollisionHandoff(targetPosition, targetRotation, |
| | 253 | 836 | | remainingAfterImpact, out targetResolvedPosition); |
| | 257 | 837 | | if (!(sourceStateAvailable & targetStateAvailable)) return false; |
| | 249 | 838 | | if (sourceResponseBody != null) |
| | 208 | 839 | | ValidateContinuousCollisionPosition(sourcePosition, postSourceLinearVelocity, Context.DeltaTime - impactElap |
| | 249 | 840 | | if (targetResponseBody != null) |
| | 42 | 841 | | targetResponseBody.ValidateContinuousCollisionHandoffPosition(targetResolvedPosition, postTargetLinearVeloci |
| | | 842 | | |
| | 249 | 843 | | if (sourceResponseBody != null) |
| | 208 | 844 | | _ = Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(this); |
| | | 845 | | |
| | 249 | 846 | | if (targetResponseBody != null) |
| | | 847 | | { |
| | | 848 | | // Dirty overlay capacity is the registered-body count, so a valid |
| | | 849 | | // registered target cannot exhaust this reservation. |
| | 42 | 850 | | _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh( |
| | 42 | 851 | | targetResponseBody); |
| | 42 | 852 | | targetResponseBody.ApplyContinuousCollisionHandoffReserved( |
| | 42 | 853 | | targetResolvedPosition, |
| | 42 | 854 | | targetRotation, |
| | 42 | 855 | | postTargetLinearVelocity, |
| | 42 | 856 | | postTargetAngularVelocity, |
| | 42 | 857 | | remainingAfterImpact, |
| | 42 | 858 | | ignoredCollider2D: Collider); |
| | | 859 | | } |
| | | 860 | | |
| | 249 | 861 | | if (sourceResponseBody != null) |
| | | 862 | | { |
| | 208 | 863 | | WakeFromCollision(); |
| | 208 | 864 | | _linearVelocity = postSourceLinearVelocity; |
| | 208 | 865 | | _angularVelocity = postSourceAngularVelocity; |
| | 208 | 866 | | RefreshLinearSpeed(); |
| | 208 | 867 | | RefreshAngularSpeed(); |
| | | 868 | | } |
| | | 869 | | |
| | | 870 | | // This static-target path has no host callbacks between response and |
| | | 871 | | // the next search. Retain only closing, producer-certified face families; |
| | | 872 | | // the prepared local mapping keeps unknown provenance conservative. |
| | 249 | 873 | | _resolvedMixedSurfaceFamilies?.Clear(); |
| | 249 | 874 | | if (targetCollider is LSMeshCollider { StaticSurface: { } surface }) |
| | 104 | 875 | | surface.CaptureMixedContinuousFamilies(manifold, impact, Collider, _resolvedMixedSurfaceFamilies ??= new()); |
| | | 876 | | |
| | 249 | 877 | | return true; |
| | | 878 | | } |
| | | 879 | | } |