| | | 1 | | //======================================================================= |
| | | 2 | | // CollisionPair.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.Runtime.CompilerServices; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | using Gravitas.Colliders; |
| | | 12 | | using GridForge.Grids; |
| | | 13 | | using GridForge.Grids.Topology; |
| | | 14 | | using SwiftCollections; |
| | | 15 | | |
| | | 16 | | namespace Gravitas.CollisionHandling; |
| | | 17 | | |
| | | 18 | | |
| | | 19 | | |
| | | 20 | | /// <summary> |
| | | 21 | | /// Owns deterministic 3D pair lifecycle, culling, contact, and notification state. |
| | | 22 | | /// </summary> |
| | | 23 | | /// <remarks>The service releases the collider endpoints when it returns a retired pair to the pool.</remarks> |
| | | 24 | | /// <content>Owns contact groups, cached impulses and discrete response lifecycle.</content> |
| | | 25 | | public partial class CollisionPair |
| | | 26 | | { |
| | | 27 | | /// <summary>Stores the host-controlled pair diagnostic flag.</summary> |
| | 3104 | 28 | | public bool Debug = true; |
| | | 29 | | |
| | | 30 | | /// <summary>Gets whether this pooled pair currently represents two registered colliders.</summary> |
| | | 31 | | public bool Active { get; private set; } |
| | | 32 | | |
| | | 33 | | private bool _isPooledForDeactivation; |
| | | 34 | | |
| | | 35 | | /// <summary>Gets the world context that owns this pair.</summary> |
| | | 36 | | public GravitasWorldContext Context { get; private set; } = null!; |
| | | 37 | | |
| | | 38 | | /// <summary>Gets the GridForge world owned by <see cref="Context"/>.</summary> |
| | 1866 | 39 | | public GridWorld World => Context.World; |
| | | 40 | | |
| | | 41 | | // stores order in which they come in |
| | | 42 | | /// <summary>Gets the first collider ID used to identify the pair.</summary> |
| | | 43 | | public int Id1 { get; private set; } |
| | | 44 | | /// <summary>Gets the second collider ID used to identify the pair.</summary> |
| | | 45 | | public int Id2 { get; private set; } |
| | | 46 | | |
| | | 47 | | /// <summary>Gets the collider selected to lead narrow-phase dispatch.</summary> |
| | | 48 | | public LSCollider ColliderA { get; private set; } = null!; |
| | | 49 | | /// <summary>Gets the other collider in the pair.</summary> |
| | | 50 | | public LSCollider ColliderB { get; private set; } = null!; |
| | | 51 | | |
| | | 52 | | /// <summary>Tracks the collision-service partition pass that last processed this pair.</summary> |
| | | 53 | | public uint PartitionVersion; |
| | | 54 | | /// <summary>Tracks reuse of this pooled pair instance.</summary> |
| | 3104 | 55 | | public ushort PairVersion = 1; |
| | | 56 | | |
| | | 57 | | /// <summary>Gets the last simulation frame in which this pair was updated.</summary> |
| | | 58 | | public long LastFrame { get; private set; } |
| | | 59 | | /// <summary>Gets the last simulation frame in which this pair had contact.</summary> |
| | | 60 | | public long LastCollidedFrame { get; private set; } |
| | | 61 | | |
| | | 62 | | private Fixed64 _fastCollideDistance; |
| | | 63 | | private Fixed64 _fastDistance; |
| | | 64 | | /// <summary>Gets the narrow-phase dispatch type for this collider combination.</summary> |
| | | 65 | | public CollisionType CollisionType { get; private set; } |
| | 3104 | 66 | | private bool _doPhysics = true; |
| | | 67 | | |
| | | 68 | | /// <summary>Gets the remaining deferred updates before this pair is checked again.</summary> |
| | | 69 | | public short CullCounter { get; private set; } |
| | | 70 | | private bool _preventDistanceCull; |
| | | 71 | | private Fixed64 _fastDistanceOffset; |
| | | 72 | | private uint _lastColliderABroadPhaseVersion; |
| | | 73 | | private uint _lastColliderBBroadPhaseVersion; |
| | | 74 | | |
| | | 75 | | private bool _isColliding; |
| | | 76 | | private bool _isCollidingChanged; |
| | | 77 | | private bool _notificationInProgress; |
| | | 78 | | private bool _separationPending; |
| | | 79 | | private bool _colliderANotified; |
| | | 80 | | private bool _colliderBNotified; |
| | | 81 | | private SolidBody? _pendingBodyA; |
| | | 82 | | private SolidBody? _pendingBodyB; |
| | | 83 | | private long _lifetimeVersion; |
| | | 84 | | private uint _lastColliderAContactDependencyVersion; |
| | | 85 | | private uint _lastColliderBContactDependencyVersion; |
| | | 86 | | private bool _surfaceContactChanged; |
| | | 87 | | private ulong? _surfaceCollisionLayers; |
| | | 88 | | |
| | 147066 | 89 | | private bool ContactDependenciesChanged => _surfaceContactChanged |
| | 147066 | 90 | | || ColliderA.ContactDependencyVersion != _lastColliderAContactDependencyVersion |
| | 147066 | 91 | | || ColliderB.ContactDependencyVersion != _lastColliderBContactDependencyVersion |
| | 147066 | 92 | | || _surfaceCollisionLayers.HasValue && _surfaceCollisionLayers != CurrentSurfaceCollisionLayers; |
| | | 93 | | |
| | | 94 | | private ulong? CurrentSurfaceCollisionLayers |
| | | 95 | | { |
| | | 96 | | get |
| | | 97 | | { |
| | 190554 | 98 | | if (ColliderA.IsTrigger || ColliderB.IsTrigger) return null; |
| | 190488 | 99 | | LSCollider? mover = ColliderA is LSMeshCollider { StaticSurface: not null } ? ColliderB |
| | 190488 | 100 | | : ColliderB is LSMeshCollider { StaticSurface: not null } ? ColliderA : null; |
| | 190488 | 101 | | return mover == null ? null : Context.Settings.GetCollisionLayerMask(default, mover.Layer, false, true); |
| | | 102 | | } |
| | | 103 | | } |
| | | 104 | | |
| | | 105 | | internal (ulong? Layers, ContactManifold? Before) BeginCollisionLayerUpdate() |
| | | 106 | | { |
| | 67395 | 107 | | ulong? layers = CurrentSurfaceCollisionLayers; |
| | 67395 | 108 | | if (!_isColliding || !layers.HasValue || !_surfaceCollisionLayers.HasValue || layers == _surfaceCollisionLayers) |
| | 67390 | 109 | | return (layers, null); |
| | 5 | 110 | | ContactManifold before = Context.CollisionScratch.SurfaceContactSnapshot3D; |
| | 5 | 111 | | before.CopyFrom(Manifold, Manifold.LastUpdatedFrame); |
| | 5 | 112 | | return (layers, before); |
| | | 113 | | } |
| | | 114 | | |
| | | 115 | | internal void CompleteCollisionLayerUpdate((ulong? Layers, ContactManifold? Before) previous) |
| | | 116 | | { |
| | 67394 | 117 | | if (previous.Before != null && !previous.Before.HasSameContacts(Manifold)) |
| | | 118 | | { |
| | 4 | 119 | | ClearWarmStart(); WakeSleepingContactParticipants(); |
| | | 120 | | } |
| | 67394 | 121 | | _surfaceCollisionLayers = previous.Layers; |
| | 67394 | 122 | | } |
| | | 123 | | |
| | 127 | 124 | | internal bool IsNotificationInProgress => _notificationInProgress; |
| | | 125 | | |
| | 2427 | 126 | | internal bool HasContactTransition => _isCollidingChanged; |
| | | 127 | | |
| | 197474 | 128 | | internal long LifetimeVersion => _lifetimeVersion; |
| | | 129 | | |
| | | 130 | | /// <summary>Gets the deterministic contact manifold owned by this pair.</summary> |
| | | 131 | | public ContactManifold Manifold { get; } = new(); |
| | | 132 | | |
| | | 133 | | private ContactWarmStartCache _warmStart; |
| | | 134 | | |
| | | 135 | | internal ContactResponseSnapshot ResponseSnapshot; |
| | | 136 | | |
| | 6208 | 137 | | internal CollisionPair(LSCollider c1, LSCollider c2) => Initialize(c1, c2); |
| | | 138 | | |
| | | 139 | | internal void ReleaseEndpointsForPooling() |
| | | 140 | | { |
| | | 141 | | // A pending retirement token may still refer to this shell before it is reused. |
| | 114 | 142 | | _lifetimeVersion++; |
| | 114 | 143 | | ColliderA = ColliderB = null!; |
| | 114 | 144 | | } |
| | | 145 | | |
| | | 146 | | /// <summary> |
| | | 147 | | /// Initializes the CollisionPair with the given colliders. |
| | | 148 | | /// </summary> |
| | | 149 | | /// <param name="c1">The first collider.</param> |
| | | 150 | | /// <param name="c2">The second collider.</param> |
| | | 151 | | internal void Initialize(LSCollider c1, LSCollider c2) |
| | | 152 | | { |
| | 3129 | 153 | | ChronicleGuard.ThrowIfNull(c1, nameof(c1)); |
| | 3129 | 154 | | ChronicleGuard.ThrowIfNull(c2, nameof(c2)); |
| | 3129 | 155 | | ChronicleGuard.ThrowIfArgument(c1 == c2, nameof(c2), "Cannot create a CollisionPair with the same collider."); |
| | 3129 | 156 | | GravitasWorldContext context = c1.Context; |
| | 3129 | 157 | | ChronicleGuard.ThrowIfArgument( |
| | 3129 | 158 | | !ReferenceEquals(context, c2.Context), |
| | 3129 | 159 | | nameof(c2), |
| | 3129 | 160 | | "Colliders must be in the same context to create a CollisionPair."); |
| | | 161 | | |
| | 3128 | 162 | | Context = context; |
| | 3128 | 163 | | _lifetimeVersion++; |
| | | 164 | | |
| | 3128 | 165 | | Reset(); |
| | | 166 | | |
| | 3128 | 167 | | AssignPriority(c1, c2); |
| | 3128 | 168 | | Id1 = ColliderA.Id; |
| | 3128 | 169 | | Id2 = ColliderB.Id; |
| | | 170 | | |
| | 3128 | 171 | | CollisionType = ColliderSettings.GetCollisionType(ColliderA.Shape, ColliderB.Shape); |
| | | 172 | | |
| | | 173 | | // Calculate the square of the sum of the radii of the bounding spheres |
| | 3128 | 174 | | _fastCollideDistance = ColliderA!.Bounds.Scope.Magnitude + ColliderB!.Bounds.Scope.Magnitude; |
| | 3128 | 175 | | _fastCollideDistance *= _fastCollideDistance; |
| | | 176 | | |
| | 3128 | 177 | | _doPhysics = (ColliderA.Body != null || ColliderB.Body != null) |
| | 3128 | 178 | | && !ColliderA.IsTrigger && !ColliderB.IsTrigger; |
| | | 179 | | |
| | | 180 | | // Immediately check collision. If collision distance is too large, do |
| | | 181 | | // not cull based on distance. |
| | 3128 | 182 | | CullCounter = 0; |
| | 3128 | 183 | | _preventDistanceCull = _fastCollideDistance > Context.Environment.CullFastDistanceMax; |
| | 3128 | 184 | | _fastDistanceOffset = Fixed64.FromRaw((int)_fastCollideDistance) + (Fixed64.One * 2) * (Fixed64.One * 2); |
| | | 185 | | |
| | 3128 | 186 | | LastCollidedFrame = Context.FrameCount; |
| | 3128 | 187 | | RefreshBroadPhaseVersions(); |
| | 3128 | 188 | | _lastColliderAContactDependencyVersion = ColliderA.ContactDependencyVersion; |
| | 3128 | 189 | | _lastColliderBContactDependencyVersion = ColliderB.ContactDependencyVersion; |
| | 3128 | 190 | | _surfaceContactChanged = false; |
| | 3128 | 191 | | PairVersion++; |
| | 3128 | 192 | | Active = true; |
| | 3128 | 193 | | } |
| | | 194 | | |
| | | 195 | | /// <summary>Orders the colliders for stable narrow-phase dispatch.</summary> |
| | | 196 | | public void AssignPriority(LSCollider c1, LSCollider c2) |
| | | 197 | | { |
| | 3128 | 198 | | if (ShouldFirstColliderLead(c1, c2)) |
| | | 199 | | { |
| | 2766 | 200 | | ColliderA = c1; |
| | 2766 | 201 | | ColliderB = c2; |
| | 2766 | 202 | | return; |
| | | 203 | | } |
| | | 204 | | |
| | 362 | 205 | | ColliderA = c2; |
| | 362 | 206 | | ColliderB = c1; |
| | 362 | 207 | | } |
| | | 208 | | |
| | | 209 | | private static bool ShouldFirstColliderLead(LSCollider c1, LSCollider c2) |
| | | 210 | | { |
| | 3128 | 211 | | if (c1.Priority != c2.Priority) |
| | 1869 | 212 | | return c1.Priority > c2.Priority; |
| | | 213 | | |
| | 1259 | 214 | | if (c1.Body == null || c2.Body == null) |
| | 744 | 215 | | return true; |
| | | 216 | | |
| | 515 | 217 | | if (c1.Body.LinearSpeed != c2.Body.LinearSpeed) |
| | 123 | 218 | | return c1.Body.LinearSpeed > c2.Body.LinearSpeed; |
| | | 219 | | |
| | 392 | 220 | | return true; |
| | | 221 | | } |
| | | 222 | | |
| | | 223 | | /// <summary> |
| | | 224 | | /// Checks and distributes collisions between colliders. |
| | | 225 | | /// Called by Partition Manager every fixed update if 2 colliders are on the same partion. |
| | | 226 | | /// </summary> |
| | 27 | 227 | | public void UpdateCollision() => UpdateCollision(CollisionResponseDispatchMode.Immediate); |
| | | 228 | | |
| | 92725 | 229 | | internal void UpdateCollisionDeferred() => UpdateCollision(CollisionResponseDispatchMode.Deferred); |
| | | 230 | | |
| | | 231 | | private void UpdateCollision(CollisionResponseDispatchMode responseMode) |
| | | 232 | | { |
| | 92752 | 233 | | if (!Active) |
| | 1 | 234 | | return; |
| | | 235 | | |
| | 92751 | 236 | | UpdateLastFrame(); |
| | 92751 | 237 | | DeactivateAndPoolIfRequired(); |
| | | 238 | | |
| | 92751 | 239 | | if (CullCounter > 0 && IsCullStateInvalidated()) |
| | 2364 | 240 | | CullCounter = 0; |
| | | 241 | | |
| | 92751 | 242 | | if (CullCounter <= 0) |
| | | 243 | | { |
| | 92750 | 244 | | ProcessCollision(responseMode); |
| | 92750 | 245 | | RefreshBroadPhaseVersions(); |
| | 92750 | 246 | | if (_isCollidingChanged && !_isColliding) |
| | 774 | 247 | | Manifold.Reset(); |
| | | 248 | | |
| | 92750 | 249 | | HandleCullingIfNotColliding(); |
| | 92750 | 250 | | return; |
| | | 251 | | } |
| | | 252 | | |
| | 1 | 253 | | CullCounter--; // Culled and one step closer to checking again. |
| | 1 | 254 | | } |
| | | 255 | | |
| | 92751 | 256 | | private void UpdateLastFrame() => LastFrame = Context.FrameCount; |
| | | 257 | | |
| | | 258 | | private void DeactivateAndPoolIfRequired() |
| | | 259 | | { |
| | 92751 | 260 | | if (_isPooledForDeactivation) |
| | 91959 | 261 | | return; |
| | | 262 | | |
| | 792 | 263 | | Context.Physics.PoolForDeactivation(this); |
| | 792 | 264 | | _isPooledForDeactivation = true; |
| | 792 | 265 | | } |
| | | 266 | | |
| | | 267 | | private void ProcessCollision(CollisionResponseDispatchMode responseMode) |
| | | 268 | | { |
| | 92750 | 269 | | bool contactChanged = RefreshPhysicalContactState(); |
| | 92750 | 270 | | if (!ShouldPerformCollisionCheck()) |
| | | 271 | | { |
| | 33731 | 272 | | if (_isColliding && _doPhysics) |
| | 187 | 273 | | WakeSleepingContactParticipants(); |
| | 33731 | 274 | | _isCollidingChanged = _isColliding; |
| | 33731 | 275 | | _isColliding = false; |
| | 33731 | 276 | | Manifold.Reset(); |
| | 33731 | 277 | | ClearWarmStart(); |
| | 33731 | 278 | | return; |
| | | 279 | | } |
| | | 280 | | |
| | 59019 | 281 | | bool result = CheckCollision(contactChanged); |
| | 59019 | 282 | | if (result) |
| | 52814 | 283 | | Context.Diagnostics.EmitContact(this, result); |
| | | 284 | | |
| | 59019 | 285 | | if (result ^ _isColliding) |
| | | 286 | | { |
| | 2025 | 287 | | if (_isColliding && _doPhysics) |
| | 585 | 288 | | WakeSleepingContactParticipants(); |
| | 2025 | 289 | | _isColliding = result; |
| | 2025 | 290 | | _isCollidingChanged = true; |
| | | 291 | | } |
| | | 292 | | |
| | 59019 | 293 | | if (!result || !Manifold.HasContact) |
| | | 294 | | { |
| | 12411 | 295 | | ClearWarmStart(); |
| | 12411 | 296 | | return; |
| | | 297 | | } |
| | | 298 | | |
| | 46608 | 299 | | if (!_doPhysics) |
| | 30 | 300 | | return; |
| | | 301 | | |
| | 46578 | 302 | | if (responseMode == CollisionResponseDispatchMode.Deferred) |
| | | 303 | | { |
| | 46564 | 304 | | Context.Physics.QueueDiscreteResponsePair(this); |
| | 46564 | 305 | | return; |
| | | 306 | | } |
| | | 307 | | |
| | 14 | 308 | | WakeSleepingBodiesForCollision(); |
| | 14 | 309 | | CollisionResponse.CalculateImpulse(this); |
| | 14 | 310 | | } |
| | | 311 | | |
| | | 312 | | internal void WakeSleepingBodiesForCollision() |
| | | 313 | | { |
| | 38461 | 314 | | SolidBody? bodyA = ColliderA.Body; |
| | 38461 | 315 | | SolidBody? bodyB = ColliderB.Body; |
| | 38461 | 316 | | if (bodyA == null || bodyB == null) |
| | 32449 | 317 | | return; |
| | | 318 | | |
| | 6012 | 319 | | bool bodyAAwake = bodyA.IsAwakeForCollision; |
| | 6012 | 320 | | bool bodyBAwake = bodyB.IsAwakeForCollision; |
| | | 321 | | |
| | 6012 | 322 | | if (bodyA.IsSleeping && bodyBAwake) |
| | 1 | 323 | | bodyA.Wake(); |
| | 6012 | 324 | | if (bodyB.IsSleeping && bodyAAwake) |
| | 3 | 325 | | bodyB.Wake(); |
| | 6012 | 326 | | } |
| | | 327 | | |
| | | 328 | | /// <summary>Raises contact or separation notifications for the current pair state.</summary> |
| | | 329 | | public void NotifyCollidersOfContact() |
| | | 330 | | { |
| | 95650 | 331 | | bool isColliding = _isColliding; |
| | 95650 | 332 | | bool isChanged = _isCollidingChanged; |
| | | 333 | | |
| | 95650 | 334 | | var registrationA = new ColliderLifetimeToken(ColliderA); |
| | 95650 | 335 | | var registrationB = new ColliderLifetimeToken(ColliderB); |
| | 95650 | 336 | | LSCollider colliderA = registrationA.Collider; |
| | 95650 | 337 | | LSCollider colliderB = registrationB.Collider; |
| | 95650 | 338 | | SolidBody? bodyA = colliderA.Body; |
| | 95650 | 339 | | SolidBody? bodyB = colliderB.Body; |
| | 95650 | 340 | | bool isTriggerPair = colliderA.IsTrigger || colliderB.IsTrigger; |
| | 95650 | 341 | | bool shouldRaiseTriggerA = isTriggerPair && ColliderTriggerEventPolicy.ShouldRaise(colliderA, colliderB); |
| | 95650 | 342 | | bool shouldRaiseTriggerB = isTriggerPair && ColliderTriggerEventPolicy.ShouldRaise(colliderB, colliderA); |
| | 95650 | 343 | | _notificationInProgress = true; |
| | 95650 | 344 | | SwiftList<Exception>? notificationExceptions = null; |
| | | 345 | | try |
| | | 346 | | { |
| | 95650 | 347 | | if (isColliding) |
| | | 348 | | { |
| | 54303 | 349 | | bool notifyEnterA = isChanged && !_colliderANotified; |
| | 54303 | 350 | | _colliderANotified = true; |
| | 54303 | 351 | | colliderA.NotifyContact( |
| | 54303 | 352 | | colliderB, |
| | 54303 | 353 | | bodyB, |
| | 54303 | 354 | | isColliding: true, |
| | 54303 | 355 | | notifyEnterA, |
| | 54303 | 356 | | allowInactive: false, |
| | 54303 | 357 | | registrationA, |
| | 54303 | 358 | | registrationB, |
| | 54303 | 359 | | isTriggerPair, |
| | 54303 | 360 | | shouldRaiseTriggerA); |
| | 54300 | 361 | | if (registrationA.IsActive && registrationB.IsActive && !_separationPending) |
| | | 362 | | { |
| | 54292 | 363 | | bool notifyEnterB = isChanged && !_colliderBNotified; |
| | 54292 | 364 | | _colliderBNotified = true; |
| | 54292 | 365 | | colliderB.NotifyContact( |
| | 54292 | 366 | | colliderA, |
| | 54292 | 367 | | bodyA, |
| | 54292 | 368 | | isColliding: true, |
| | 54292 | 369 | | notifyEnterB, |
| | 54292 | 370 | | allowInactive: false, |
| | 54292 | 371 | | registrationB, |
| | 54292 | 372 | | registrationA, |
| | 54292 | 373 | | isTriggerPair, |
| | 54292 | 374 | | shouldRaiseTriggerB); |
| | | 375 | | } |
| | | 376 | | } |
| | | 377 | | else |
| | | 378 | | { |
| | 41347 | 379 | | NotifySeparation( |
| | 41347 | 380 | | registrationA, |
| | 41347 | 381 | | registrationB, |
| | 41347 | 382 | | bodyA, |
| | 41347 | 383 | | bodyB, |
| | 41347 | 384 | | isChanged, |
| | 41347 | 385 | | isTriggerPair, |
| | 41347 | 386 | | shouldRaiseTriggerA, |
| | 41347 | 387 | | shouldRaiseTriggerB); |
| | | 388 | | } |
| | 95640 | 389 | | } |
| | 10 | 390 | | catch (Exception exception) |
| | | 391 | | { |
| | 10 | 392 | | CollisionNotificationExceptions.Capture(ref notificationExceptions, exception); |
| | 10 | 393 | | } |
| | | 394 | | |
| | | 395 | | try |
| | | 396 | | { |
| | 95650 | 397 | | EndNotification( |
| | 95650 | 398 | | registrationA, |
| | 95650 | 399 | | registrationB, |
| | 95650 | 400 | | isTriggerPair, |
| | 95650 | 401 | | shouldRaiseTriggerA, |
| | 95650 | 402 | | shouldRaiseTriggerB); |
| | 95649 | 403 | | } |
| | 1 | 404 | | catch (Exception exception) |
| | | 405 | | { |
| | 1 | 406 | | CollisionNotificationExceptions.Capture(ref notificationExceptions, exception); |
| | 1 | 407 | | } |
| | | 408 | | |
| | 95650 | 409 | | _isCollidingChanged &= _isColliding |
| | 95650 | 410 | | & !(_colliderANotified & _colliderBNotified); |
| | | 411 | | |
| | 95650 | 412 | | CollisionNotificationExceptions.ThrowIfAny(notificationExceptions); |
| | 95639 | 413 | | } |
| | | 414 | | |
| | | 415 | | private void EndNotification( |
| | | 416 | | in ColliderLifetimeToken registrationA, |
| | | 417 | | in ColliderLifetimeToken registrationB, |
| | | 418 | | bool isTriggerPair, |
| | | 419 | | bool shouldRaiseTriggerA, |
| | | 420 | | bool shouldRaiseTriggerB) |
| | | 421 | | { |
| | | 422 | | try |
| | | 423 | | { |
| | 95650 | 424 | | if (!_separationPending) |
| | 95641 | 425 | | return; |
| | | 426 | | |
| | 9 | 427 | | SolidBody? bodyA = _pendingBodyA; |
| | 9 | 428 | | SolidBody? bodyB = _pendingBodyB; |
| | 9 | 429 | | ClearPendingNotificationState(); |
| | 9 | 430 | | NotifySeparation( |
| | 9 | 431 | | registrationA, |
| | 9 | 432 | | registrationB, |
| | 9 | 433 | | bodyA, |
| | 9 | 434 | | bodyB, |
| | 9 | 435 | | isChanged: true, |
| | 9 | 436 | | isTriggerPair, |
| | 9 | 437 | | shouldRaiseTriggerA, |
| | 9 | 438 | | shouldRaiseTriggerB); |
| | 8 | 439 | | } |
| | | 440 | | finally |
| | | 441 | | { |
| | 95650 | 442 | | ClearPendingNotificationState(); |
| | 95650 | 443 | | _notificationInProgress = false; |
| | 95650 | 444 | | } |
| | 95649 | 445 | | } |
| | | 446 | | |
| | | 447 | | private void NotifySeparation( |
| | | 448 | | in ColliderLifetimeToken registrationA, |
| | | 449 | | in ColliderLifetimeToken registrationB, |
| | | 450 | | SolidBody? bodyA, |
| | | 451 | | SolidBody? bodyB, |
| | | 452 | | bool isChanged, |
| | | 453 | | bool isTriggerPair, |
| | | 454 | | bool shouldRaiseTriggerA, |
| | | 455 | | bool shouldRaiseTriggerB) |
| | | 456 | | { |
| | 41356 | 457 | | bool notifyA = _colliderANotified; |
| | 41356 | 458 | | bool notifyB = _colliderBNotified; |
| | 41356 | 459 | | _colliderANotified = false; |
| | 41356 | 460 | | _colliderBNotified = false; |
| | | 461 | | |
| | 41356 | 462 | | SwiftList<Exception>? notificationExceptions = null; |
| | 41356 | 463 | | if (notifyA) |
| | | 464 | | { |
| | | 465 | | try |
| | | 466 | | { |
| | 873 | 467 | | registrationA.Collider.NotifyContact( |
| | 873 | 468 | | registrationB.Collider, |
| | 873 | 469 | | bodyB, |
| | 873 | 470 | | isColliding: false, |
| | 873 | 471 | | isChanged, |
| | 873 | 472 | | allowInactive: true, |
| | 873 | 473 | | registrationA, |
| | 873 | 474 | | registrationB, |
| | 873 | 475 | | isTriggerPair, |
| | 873 | 476 | | shouldRaiseTriggerA); |
| | 865 | 477 | | } |
| | 8 | 478 | | catch (Exception exception) |
| | | 479 | | { |
| | 8 | 480 | | CollisionNotificationExceptions.Capture(ref notificationExceptions, exception); |
| | 8 | 481 | | } |
| | | 482 | | } |
| | | 483 | | |
| | 41356 | 484 | | if (notifyB && registrationB.IsCurrentLifetime) |
| | | 485 | | { |
| | | 486 | | try |
| | | 487 | | { |
| | 862 | 488 | | registrationB.Collider.NotifyContact( |
| | 862 | 489 | | registrationA.Collider, |
| | 862 | 490 | | bodyA, |
| | 862 | 491 | | isColliding: false, |
| | 862 | 492 | | isChanged, |
| | 862 | 493 | | allowInactive: true, |
| | 862 | 494 | | registrationB, |
| | 862 | 495 | | registrationA, |
| | 862 | 496 | | isTriggerPair, |
| | 862 | 497 | | shouldRaiseTriggerB); |
| | 861 | 498 | | } |
| | 1 | 499 | | catch (Exception exception) |
| | | 500 | | { |
| | 1 | 501 | | CollisionNotificationExceptions.Capture(ref notificationExceptions, exception); |
| | 1 | 502 | | } |
| | | 503 | | } |
| | | 504 | | |
| | 41356 | 505 | | CollisionNotificationExceptions.ThrowIfAny(notificationExceptions); |
| | 41348 | 506 | | } |
| | | 507 | | |
| | | 508 | | private void ClearPendingNotificationState() |
| | | 509 | | { |
| | 98789 | 510 | | _separationPending = false; |
| | 98789 | 511 | | _pendingBodyA = null; |
| | 98789 | 512 | | _pendingBodyB = null; |
| | 98789 | 513 | | } |
| | | 514 | | |
| | | 515 | | private void HandleCullingIfNotColliding() |
| | | 516 | | { |
| | 92750 | 517 | | if (_isColliding) |
| | | 518 | | { |
| | 52814 | 519 | | LastCollidedFrame = Context.FrameCount; |
| | 52814 | 520 | | return; |
| | | 521 | | } |
| | | 522 | | |
| | 39936 | 523 | | CalculateCullScore(); |
| | 39936 | 524 | | } |
| | | 525 | | |
| | | 526 | | internal bool TryRefreshRetainedContact() |
| | | 527 | | { |
| | 98000 | 528 | | if (!_isColliding) |
| | 43682 | 529 | | return true; |
| | 54318 | 530 | | bool joined = ColliderA is LSMeshCollider { StaticSurface: not null } |
| | 54318 | 531 | | || ColliderB is LSMeshCollider { StaticSurface: not null }; |
| | 54318 | 532 | | bool contactChanged = RefreshPhysicalContactState(); |
| | | 533 | | // A sleeping participant does not make a moved or reshaped support's old manifold valid. |
| | | 534 | | // The broad-phase pass already checked live eligibility this frame. |
| | | 535 | | // An overlap can legitimately leave its selected rows to another member. |
| | 54318 | 536 | | if (LastFrame != Context.FrameCount && (!Manifold.HasContact || joined) |
| | 54318 | 537 | | || contactChanged || BroadPhaseVersionChanged()) |
| | | 538 | | { |
| | 145 | 539 | | if (!BoundsOverlapInclusive(ColliderA, ColliderB) |
| | 145 | 540 | | || !CollisionDetection.DoCollisionCheck(this)) |
| | 22 | 541 | | return false; |
| | 123 | 542 | | RefreshBroadPhaseVersions(); |
| | | 543 | | } |
| | | 544 | | |
| | 54296 | 545 | | if (ColliderA.Body?.IsSleeping == true || ColliderB.Body?.IsSleeping == true) |
| | 1558 | 546 | | LastCollidedFrame = Context.FrameCount; |
| | 54296 | 547 | | return true; |
| | | 548 | | } |
| | | 549 | | |
| | | 550 | | private bool RefreshPhysicalContactState() |
| | | 551 | | { |
| | 147068 | 552 | | bool doPhysics = (ColliderA.Body != null || ColliderB.Body != null) |
| | 147068 | 553 | | && !ColliderA.IsTrigger && !ColliderB.IsTrigger; |
| | 147068 | 554 | | bool contactChanged = _doPhysics != doPhysics || ContactDependenciesChanged; |
| | | 555 | | // A changed solid constraint can carry frictional load even if its new geometry still overlaps. |
| | 147068 | 556 | | if (_isColliding) |
| | | 557 | | { |
| | 106467 | 558 | | if (_doPhysics != doPhysics) |
| | | 559 | | { |
| | 3 | 560 | | ClearWarmStart(); |
| | 3 | 561 | | WakeSleepingContactParticipants(); |
| | | 562 | | } |
| | 106464 | 563 | | else if (_doPhysics) |
| | | 564 | | { |
| | 106428 | 565 | | if (ColliderB.ContactDependencyVersion != _lastColliderBContactDependencyVersion) |
| | 409 | 566 | | ColliderA.Body?.WakeForContactChange(); |
| | 106428 | 567 | | if (ColliderA.ContactDependencyVersion != _lastColliderAContactDependencyVersion) |
| | 50 | 568 | | ColliderB.Body?.WakeForContactChange(); |
| | | 569 | | } |
| | | 570 | | } |
| | 147068 | 571 | | _doPhysics = doPhysics; |
| | 147068 | 572 | | _lastColliderAContactDependencyVersion = ColliderA.ContactDependencyVersion; |
| | 147068 | 573 | | _lastColliderBContactDependencyVersion = ColliderB.ContactDependencyVersion; |
| | 147068 | 574 | | _surfaceContactChanged = false; |
| | 147068 | 575 | | return contactChanged; |
| | | 576 | | } |
| | | 577 | | |
| | | 578 | | private void WakeSleepingContactParticipants() |
| | | 579 | | { |
| | 957 | 580 | | ColliderA.Body?.WakeForContactChange(); |
| | 957 | 581 | | ColliderB.Body?.WakeForContactChange(); |
| | 836 | 582 | | } |
| | | 583 | | |
| | | 584 | | private bool CheckCollision(bool contactChanged) |
| | | 585 | | { |
| | | 586 | | // Child material overrides are manifold data and can change without a bounds publication. |
| | 59019 | 587 | | if (!contactChanged && !BroadPhaseVersionChanged() && _isColliding |
| | 59019 | 588 | | && ColliderA is not LSMeshCollider { StaticSurface: not null } |
| | 59019 | 589 | | && ColliderB is not LSMeshCollider { StaticSurface: not null }) |
| | 700 | 590 | | return _isColliding; |
| | | 591 | | |
| | 58319 | 592 | | return CollisionDetection.DoCollisionCheck(this); |
| | | 593 | | } |
| | | 594 | | |
| | | 595 | | private bool IsCullStateInvalidated() |
| | | 596 | | { |
| | 2365 | 597 | | return ColliderA.PartitionChanged |
| | 2365 | 598 | | || ColliderB.PartitionChanged |
| | 2365 | 599 | | || BroadPhaseVersionChanged() || ContactDependenciesChanged; |
| | | 600 | | } |
| | | 601 | | |
| | | 602 | | private bool BroadPhaseVersionChanged() |
| | | 603 | | { |
| | 112791 | 604 | | return ColliderA.BroadPhaseVersion != _lastColliderABroadPhaseVersion |
| | 112791 | 605 | | || ColliderB.BroadPhaseVersion != _lastColliderBBroadPhaseVersion; |
| | | 606 | | } |
| | | 607 | | |
| | | 608 | | private void RefreshBroadPhaseVersions() |
| | | 609 | | { |
| | 96001 | 610 | | _lastColliderABroadPhaseVersion = ColliderA.BroadPhaseVersion; |
| | 96001 | 611 | | _lastColliderBBroadPhaseVersion = ColliderB.BroadPhaseVersion; |
| | 96001 | 612 | | } |
| | | 613 | | |
| | | 614 | | private bool ShouldPerformCollisionCheck() |
| | | 615 | | { |
| | | 616 | | // Center distance remains a scheduling signal only. Canonical geometry |
| | | 617 | | // may extend beyond a scalar face while its conservative broad-phase |
| | | 618 | | // bounds are clipped to the representable domain, which can shorten |
| | | 619 | | // Bounds.Scope without shortening the physical shape. |
| | 92750 | 620 | | _fastDistance = Vector3d.DistanceSquared(ColliderA.Center, ColliderB.Center); |
| | | 621 | | |
| | | 622 | | // Inclusive bounds overlap preserves zero-depth touching contacts for the manifold pass. |
| | 92750 | 623 | | return BoundsOverlapInclusive(ColliderA, ColliderB); |
| | | 624 | | } |
| | | 625 | | |
| | | 626 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 627 | | internal static bool BoundsOverlapInclusive(LSCollider colliderA, LSCollider colliderB) |
| | | 628 | | { |
| | 307574 | 629 | | return colliderA.BoundsMin.X <= colliderB.BoundsMax.X |
| | 307574 | 630 | | && colliderA.BoundsMax.X >= colliderB.BoundsMin.X |
| | 307574 | 631 | | && colliderA.BoundsMin.Y <= colliderB.BoundsMax.Y |
| | 307574 | 632 | | && colliderA.BoundsMax.Y >= colliderB.BoundsMin.Y |
| | 307574 | 633 | | && colliderA.BoundsMin.Z <= colliderB.BoundsMax.Z |
| | 307574 | 634 | | && colliderA.BoundsMax.Z >= colliderB.BoundsMin.Z; |
| | | 635 | | } |
| | | 636 | | |
| | | 637 | | private void CalculateCullScore() |
| | | 638 | | { |
| | 39936 | 639 | | int distanceScore = 0; |
| | 39936 | 640 | | int velocityScore = 0; |
| | 39936 | 641 | | if (!_preventDistanceCull) |
| | | 642 | | { |
| | 1867 | 643 | | int distanceMax = Context.Environment.CullDistanceMax; |
| | 1867 | 644 | | if (distanceMax > 0) |
| | | 645 | | { |
| | 1866 | 646 | | int step = GetCullDistanceStep(World!); |
| | 1866 | 647 | | distanceScore = Math.Clamp((int)(_fastDistance - _fastDistanceOffset) / step + Context.Collisions.CullDi |
| | | 648 | | } |
| | | 649 | | |
| | 1867 | 650 | | int cullVelocityStep = Context.Environment.CullVelocityStep; |
| | 1867 | 651 | | if (cullVelocityStep > 0) |
| | 1866 | 652 | | velocityScore = Math.Clamp((int)(ColliderA.Velocity - ColliderB.Velocity).Magnitude / cullVelocityStep, |
| | | 653 | | } |
| | | 654 | | |
| | 39936 | 655 | | int timeScore = 0; |
| | 39936 | 656 | | int cullTimeStep = Context.Environment.CullTimeStep; |
| | 39936 | 657 | | if (cullTimeStep > 0) |
| | 39935 | 658 | | timeScore = (int)Math.Clamp((Context.FrameCount - LastCollidedFrame) / cullTimeStep, 0, Context.Environment. |
| | | 659 | | |
| | 39936 | 660 | | CullCounter = (short)Math.Clamp(distanceScore + timeScore - velocityScore, 0, short.MaxValue); |
| | 39936 | 661 | | } |
| | | 662 | | |
| | | 663 | | /// <summary> |
| | | 664 | | /// Defines the step value for distance-based culling. The score is increased |
| | | 665 | | /// when the distance between objects increases. Higher values make the culling more aggressive for distant objects. |
| | | 666 | | /// </summary> |
| | | 667 | | private int GetCullDistanceStep(GridWorld world) |
| | | 668 | | { |
| | 1866 | 669 | | int distanceMax = Context.Environment.CullDistanceMax; |
| | 1866 | 670 | | Fixed64 cellEdge = GridTopologyMetricUtility.GetRepresentativeCellEdge(world); |
| | 1866 | 671 | | int step = ((cellEdge + Fixed64.One * 2) * (cellEdge + Fixed64.One * 2) / distanceMax).CeilToInt(); |
| | 1866 | 672 | | return Math.Max(1, step); |
| | | 673 | | } |
| | | 674 | | |
| | | 675 | | /// <summary>Clears transient contact and notification state before reuse.</summary> |
| | | 676 | | public void Reset() |
| | | 677 | | { |
| | 3130 | 678 | | Manifold.Reset(); |
| | 3130 | 679 | | ClearWarmStart(); |
| | 3130 | 680 | | _isColliding = false; |
| | 3130 | 681 | | _surfaceCollisionLayers = null; |
| | 3130 | 682 | | _isCollidingChanged = false; |
| | 3130 | 683 | | _isPooledForDeactivation = false; |
| | 3130 | 684 | | _notificationInProgress = false; |
| | 3130 | 685 | | _colliderANotified = false; |
| | 3130 | 686 | | _colliderBNotified = false; |
| | 3130 | 687 | | ClearPendingNotificationState(); |
| | 3130 | 688 | | } |
| | | 689 | | |
| | | 690 | | internal void StoreWarmStartImpulse( |
| | | 691 | | ulong contactId, |
| | | 692 | | Vector3d normal, |
| | | 693 | | Fixed64 normalImpulse, |
| | | 694 | | Fixed64 tangentImpulse, |
| | | 695 | | Fixed64 secondaryTangentImpulse = default) => |
| | 58 | 696 | | _warmStart.Set(contactId, normal, normalImpulse, tangentImpulse, secondaryTangentImpulse); |
| | | 697 | | |
| | | 698 | | internal bool TryGetWarmStartImpulse(ulong contactId, out ContactWarmStartImpulse impulse) => |
| | 68 | 699 | | _warmStart.TryGet(contactId, out impulse); |
| | | 700 | | |
| | | 701 | | internal void ClearWarmStart() |
| | | 702 | | { |
| | 49556 | 703 | | _warmStart.Clear(); |
| | 49556 | 704 | | ResponseSnapshot = default; |
| | 49556 | 705 | | } |
| | | 706 | | |
| | | 707 | | internal void InvalidateSurfaceContact(LSCollider member, SwiftHashSet<int> suppliers) |
| | | 708 | | { |
| | 308 | 709 | | if (!Manifold.ReferencesSuppliers(suppliers, ColliderA == member, member.Id + 1)) return; |
| | 114 | 710 | | _surfaceContactChanged = true; |
| | 114 | 711 | | ClearWarmStart(); |
| | 209 | 712 | | if (_isColliding && _doPhysics) WakeSleepingContactParticipants(); |
| | 114 | 713 | | } |
| | | 714 | | |
| | | 715 | | internal void StoreWarmStartImpulse(in ContactGroupKey group, ulong contactId, |
| | | 716 | | Vector3d normal, Fixed64 normalImpulse, Fixed64 tangentImpulse, |
| | | 717 | | Fixed64 secondaryTangentImpulse) => |
| | 603716 | 718 | | _warmStart.Set(group, contactId, normal, normalImpulse, tangentImpulse, secondaryTangentImpulse); |
| | | 719 | | |
| | | 720 | | internal bool TryGetWarmStartImpulse(in ContactGroupKey group, ulong contactId, |
| | 714512 | 721 | | out ContactWarmStartImpulse impulse) => _warmStart.TryGet(group, contactId, out impulse); |
| | | 722 | | |
| | | 723 | | internal void RemoveWarmStartImpulse(in ContactGroupKey group, ulong contactId) => |
| | 13 | 724 | | _warmStart.Remove(group, contactId); |
| | | 725 | | |
| | 115019 | 726 | | internal void RetainWarmStarts() => _warmStart.Retain(Manifold); |
| | | 727 | | |
| | | 728 | | /// <summary> |
| | | 729 | | /// Deactivates the CollisionPair. |
| | | 730 | | /// </summary> |
| | | 731 | | public void Deactivate() |
| | | 732 | | { |
| | 139 | 733 | | if (_isColliding && _doPhysics) |
| | 85 | 734 | | WakeSleepingContactParticipants(); |
| | 139 | 735 | | bool notifySeparation = _isColliding || _colliderANotified || _colliderBNotified; |
| | 139 | 736 | | if (notifySeparation) |
| | | 737 | | { |
| | 103 | 738 | | _isColliding = false; |
| | 103 | 739 | | _isCollidingChanged = true; |
| | 103 | 740 | | if (_notificationInProgress) |
| | | 741 | | { |
| | 9 | 742 | | _separationPending = true; |
| | 9 | 743 | | _pendingBodyA = ColliderA.Body; |
| | 9 | 744 | | _pendingBodyB = ColliderB.Body; |
| | | 745 | | } |
| | | 746 | | } |
| | | 747 | | |
| | 139 | 748 | | Manifold.Reset(); |
| | 139 | 749 | | ClearWarmStart(); |
| | 139 | 750 | | _isPooledForDeactivation = false; |
| | 139 | 751 | | Active = false; |
| | | 752 | | |
| | 139 | 753 | | if (notifySeparation && !_notificationInProgress) |
| | 94 | 754 | | NotifyCollidersOfContact(); |
| | 134 | 755 | | } |
| | | 756 | | } |