| | | 1 | | //======================================================================= |
| | | 2 | | // GravitasPhysicsService.Pairs.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 Gravitas.Colliders; |
| | | 9 | | using Gravitas.CollisionHandling; |
| | | 10 | | using Gravitas.Support; |
| | | 11 | | using SwiftCollections; |
| | | 12 | | |
| | | 13 | | namespace Gravitas; |
| | | 14 | | |
| | | 15 | | internal readonly struct CollisionPairLifetimeToken |
| | | 16 | | { |
| | | 17 | | internal CollisionPairLifetimeToken(CollisionPair pair) |
| | | 18 | | { |
| | 305 | 19 | | Pair = pair; |
| | 305 | 20 | | LifetimeVersion = pair.LifetimeVersion; |
| | 305 | 21 | | ColliderA = new ColliderLifetimeToken(pair.ColliderA); |
| | 305 | 22 | | ColliderB = new ColliderLifetimeToken(pair.ColliderB); |
| | 305 | 23 | | } |
| | | 24 | | |
| | | 25 | | internal CollisionPair Pair { get; } |
| | | 26 | | |
| | | 27 | | internal long LifetimeVersion { get; } |
| | | 28 | | |
| | | 29 | | private ColliderLifetimeToken ColliderA { get; } |
| | | 30 | | |
| | | 31 | | private ColliderLifetimeToken ColliderB { get; } |
| | | 32 | | |
| | 9493 | 33 | | internal bool IsCurrentLifetime => Pair.LifetimeVersion == LifetimeVersion |
| | 9493 | 34 | | && ColliderA.IsCurrentLifetime |
| | 9493 | 35 | | && ColliderB.IsCurrentLifetime; |
| | | 36 | | } |
| | | 37 | | |
| | | 38 | | public sealed partial class GravitasPhysicsService |
| | | 39 | | { |
| | | 40 | | internal CollisionPair? GetCollisionPair(int id1, int id2) |
| | | 41 | | { |
| | | 42 | | if (!TryGetColliderById(id1, out LSCollider? collider1) |
| | | 43 | | || !TryGetColliderById(id2, out LSCollider? collider2)) |
| | | 44 | | { |
| | | 45 | | return null; |
| | | 46 | | } |
| | | 47 | | |
| | | 48 | | if (collider1!.Id > collider2!.Id) |
| | | 49 | | (collider2, collider1) = (collider1, collider2); |
| | | 50 | | |
| | | 51 | | if (!RequireCollisionPair(collider1, collider2)) |
| | | 52 | | return null; |
| | | 53 | | |
| | | 54 | | if (!collider1.TryGetCollisionPair(collider2.Id, out CollisionPair? pair) |
| | | 55 | | && !collider2.TryGetCollisionPair(collider1.Id, out pair)) |
| | | 56 | | { |
| | | 57 | | pair = CreatePair(collider1, collider2); |
| | | 58 | | pair.ColliderA.TryAddCollisionPair(pair.ColliderB.Id, pair); |
| | | 59 | | pair.ColliderB.TryAddCollisionPairHolder(pair.ColliderA.Id); |
| | | 60 | | } |
| | | 61 | | |
| | | 62 | | return pair; |
| | | 63 | | } |
| | | 64 | | |
| | | 65 | | internal bool RequireCollisionPair(LSCollider collider1, LSCollider collider2) |
| | | 66 | | { |
| | | 67 | | return collider1.IsActive && collider2.IsActive |
| | | 68 | | && !collider1.IsDeactivationInProgress && !collider2.IsDeactivationInProgress |
| | | 69 | | && collider1.Shape != ColliderType.None && collider2.Shape != ColliderType.None |
| | | 70 | | && (collider1.Body != null || collider2.Body != null) |
| | | 71 | | && !IsLayerCollisionDisabled(collider1.Layer, collider2.Layer) |
| | | 72 | | && ColliderCollisionFilter.AllowsPhysicalPair(collider1, collider2) |
| | | 73 | | && !_context.Constraints3D.ShouldExcludeLinkedCollision(collider1, collider2) |
| | | 74 | | && !collider1.IsSibling(collider2); |
| | | 75 | | } |
| | | 76 | | |
| | | 77 | | internal bool IsLayerCollisionDisabled(PhysicsLayer layer1, PhysicsLayer layer2) |
| | | 78 | | { |
| | | 79 | | bool[,] matrix = _context.Settings.CollisionMatrix; |
| | | 80 | | int layerIndex1 = layer1.Index; |
| | | 81 | | int layerIndex2 = layer2.Index; |
| | | 82 | | if (layerIndex1 >= matrix.GetLength(0) || layerIndex2 >= matrix.GetLength(1)) |
| | | 83 | | return false; |
| | | 84 | | |
| | | 85 | | return !matrix[layerIndex1, layerIndex2]; |
| | | 86 | | } |
| | | 87 | | |
| | | 88 | | internal void PoolForDeactivation(CollisionPair pair) |
| | | 89 | | { |
| | | 90 | | SwiftThrowHelper.ThrowIfNull(pair, nameof(pair)); |
| | | 91 | | lock (_activeCollisionPairs) |
| | | 92 | | _activeCollisionPairs.Enqueue(new CollisionPairLifetimeToken(pair)); |
| | | 93 | | } |
| | | 94 | | |
| | | 95 | | internal void FullDeactivateCollisionPair(CollisionPair pair) |
| | | 96 | | { |
| | | 97 | | RemovePairReferences(pair); |
| | | 98 | | DeactivateAndPoolPair(pair); |
| | | 99 | | } |
| | | 100 | | |
| | | 101 | | internal void DeactivateAndPoolPair(CollisionPair pair) |
| | | 102 | | { |
| | | 103 | | if (!pair.Active) |
| | | 104 | | return; |
| | | 105 | | |
| | | 106 | | pair.Deactivate(); |
| | | 107 | | if (_context.Settings.PoolingEnabled && !pair.IsNotificationInProgress) |
| | | 108 | | _cachedCollisionPairs.Push(pair); |
| | | 109 | | } |
| | | 110 | | |
| | | 111 | | internal void RemovePairReferences(CollisionPair pair) |
| | | 112 | | { |
| | | 113 | | pair.ColliderA.TryRemoveCollisionPair(pair.Id2); |
| | | 114 | | pair.ColliderB.TryRemoveCollisionPairHolder(pair.Id1); |
| | | 115 | | } |
| | | 116 | | |
| | | 117 | | internal void RemovePairsForCollider(LSCollider collider) |
| | | 118 | | { |
| | | 119 | | int deactivationStart = _pairsPendingDeactivation.Count; |
| | | 120 | | SwiftDictionary<int, CollisionPair>? collisionPairs = collider.CollisionPairs; |
| | | 121 | | if (collisionPairs != null) |
| | | 122 | | { |
| | | 123 | | foreach (var pairEntry in collisionPairs) |
| | | 124 | | _pairsPendingDeactivation.Add(new CollisionPairLifetimeToken(pairEntry.Value)); |
| | | 125 | | } |
| | | 126 | | |
| | | 127 | | SwiftHashSet<int>? collisionPairHolders = collider.CollisionPairHolders; |
| | | 128 | | if (collisionPairHolders != null) |
| | | 129 | | { |
| | | 130 | | foreach (int holderId in collisionPairHolders) |
| | | 131 | | { |
| | | 132 | | if (!TryGetColliderById(holderId, out LSCollider? holder)) |
| | | 133 | | continue; |
| | | 134 | | |
| | | 135 | | if (holder!.TryGetCollisionPair(collider.Id, out CollisionPair? pair)) |
| | | 136 | | _pairsPendingDeactivation.Add(new CollisionPairLifetimeToken(pair!)); |
| | | 137 | | } |
| | | 138 | | } |
| | | 139 | | |
| | | 140 | | int deactivationEnd = _pairsPendingDeactivation.Count; |
| | | 141 | | try |
| | | 142 | | { |
| | | 143 | | for (int i = deactivationStart; i < deactivationEnd; i++) |
| | | 144 | | { |
| | | 145 | | CollisionPairLifetimeToken token = _pairsPendingDeactivation[i]; |
| | | 146 | | if (token.IsCurrentLifetime) |
| | | 147 | | FullDeactivateCollisionPair(token.Pair); |
| | | 148 | | } |
| | | 149 | | |
| | | 150 | | collider.ClearCollisionPairState(); |
| | | 151 | | collider.ClearRuntimeRelationships(); |
| | | 152 | | } |
| | | 153 | | finally |
| | | 154 | | { |
| | | 155 | | if (deactivationStart == 0) |
| | | 156 | | _pairsPendingDeactivation.FastClear(); |
| | | 157 | | } |
| | | 158 | | } |
| | | 159 | | |
| | | 160 | | internal void ClearWarmStartCachesForCollider(LSCollider collider) |
| | | 161 | | { |
| | | 162 | | SwiftDictionary<int, CollisionPair>? collisionPairs = collider.CollisionPairs; |
| | | 163 | | if (collisionPairs != null) |
| | | 164 | | { |
| | | 165 | | foreach (var pairEntry in collisionPairs) |
| | | 166 | | pairEntry.Value.ClearWarmStart(); |
| | | 167 | | } |
| | | 168 | | |
| | | 169 | | SwiftHashSet<int>? collisionPairHolders = collider.CollisionPairHolders; |
| | | 170 | | if (collisionPairHolders == null) |
| | | 171 | | return; |
| | | 172 | | |
| | | 173 | | foreach (int holderId in collisionPairHolders) |
| | | 174 | | { |
| | | 175 | | if (TryGetColliderById(holderId, out LSCollider? holder) |
| | | 176 | | && holder!.TryGetCollisionPair(collider.Id, out CollisionPair? pair)) |
| | | 177 | | { |
| | | 178 | | pair!.ClearWarmStart(); |
| | | 179 | | } |
| | | 180 | | } |
| | | 181 | | } |
| | | 182 | | |
| | | 183 | | private CollisionPair CreatePair(LSCollider collider1, LSCollider collider2) |
| | | 184 | | { |
| | | 185 | | if (!_context.Settings.PoolingEnabled || _cachedCollisionPairs.Count <= 0) |
| | | 186 | | return new CollisionPair(collider1, collider2); |
| | | 187 | | |
| | | 188 | | CollisionPair pair = _cachedCollisionPairs.Pop(); |
| | | 189 | | pair.Initialize(collider1, collider2); |
| | | 190 | | return pair; |
| | | 191 | | } |
| | | 192 | | |
| | | 193 | | private void ProcessActiveCollisionPairs() |
| | | 194 | | { |
| | | 195 | | int snapshotStart = _activeCollisionPairSnapshot.Count; |
| | | 196 | | int collisionCounter = _activeCollisionPairs.Count; |
| | | 197 | | for (int i = 0; i < collisionCounter; i++) |
| | | 198 | | _activeCollisionPairSnapshot.Add(_activeCollisionPairs.Dequeue()); |
| | | 199 | | |
| | | 200 | | int snapshotEnd = _activeCollisionPairSnapshot.Count; |
| | | 201 | | int processingIndex = snapshotStart; |
| | | 202 | | try |
| | | 203 | | { |
| | | 204 | | for (; processingIndex < snapshotEnd; processingIndex++) |
| | | 205 | | { |
| | | 206 | | CollisionPairLifetimeToken token = _activeCollisionPairSnapshot[processingIndex]; |
| | | 207 | | CollisionPair instancePair = token.Pair; |
| | | 208 | | if (!token.IsCurrentLifetime || !instancePair.Active) |
| | | 209 | | continue; |
| | | 210 | | |
| | | 211 | | if (!RequireCollisionPair(instancePair.ColliderA, instancePair.ColliderB)) |
| | | 212 | | { |
| | | 213 | | FullDeactivateCollisionPair(instancePair); |
| | | 214 | | continue; |
| | | 215 | | } |
| | | 216 | | |
| | | 217 | | bool preservedSleepingContact = instancePair.TryPreserveSleepingRestingContact(); |
| | | 218 | | int passedFrames = _context.FrameCount - instancePair.LastCollidedFrame; |
| | | 219 | | if (!preservedSleepingContact && passedFrames >= InactiveFrameThreshold) |
| | | 220 | | FullDeactivateCollisionPair(instancePair); |
| | | 221 | | else |
| | | 222 | | { |
| | | 223 | | if (instancePair.CullCounter <= 0) |
| | | 224 | | instancePair.NotifyCollidersOfContact(); |
| | | 225 | | if (token.IsCurrentLifetime && instancePair.Active) |
| | | 226 | | _activeCollisionPairs.Enqueue(token); |
| | | 227 | | } |
| | | 228 | | } |
| | | 229 | | } |
| | | 230 | | finally |
| | | 231 | | { |
| | | 232 | | for (; processingIndex < snapshotEnd; processingIndex++) |
| | | 233 | | { |
| | | 234 | | CollisionPairLifetimeToken token = _activeCollisionPairSnapshot[processingIndex]; |
| | | 235 | | if (token.IsCurrentLifetime && token.Pair.Active) |
| | | 236 | | _activeCollisionPairs.Enqueue(token); |
| | | 237 | | } |
| | | 238 | | |
| | | 239 | | if (snapshotStart == 0) |
| | | 240 | | _activeCollisionPairSnapshot.FastClear(); |
| | | 241 | | } |
| | | 242 | | } |
| | | 243 | | |
| | | 244 | | } |