| | | 1 | | //======================================================================= |
| | | 2 | | // ContactWarmStartCache.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 FixedMathSharp; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.CollisionHandling; |
| | | 12 | | |
| | | 13 | | /// <summary> |
| | | 14 | | /// Fixed-size 3D warm-start cache keyed by stable manifold contact identity. |
| | | 15 | | /// </summary> |
| | | 16 | | internal struct ContactWarmStartCache |
| | | 17 | | { |
| | | 18 | | private ulong _contactId0; |
| | | 19 | | private ulong _contactId1; |
| | | 20 | | private ulong _contactId2; |
| | | 21 | | private ulong _contactId3; |
| | | 22 | | private ContactWarmStartImpulse _impulse0; |
| | | 23 | | private ContactWarmStartImpulse _impulse1; |
| | | 24 | | private ContactWarmStartImpulse _impulse2; |
| | | 25 | | private ContactWarmStartImpulse _impulse3; |
| | | 26 | | |
| | | 27 | | public int Count { get; private set; } |
| | | 28 | | |
| | | 29 | | public void Clear() |
| | | 30 | | { |
| | 2510 | 31 | | Count = 0; |
| | 2510 | 32 | | _contactId0 = 0UL; |
| | 2510 | 33 | | _contactId1 = 0UL; |
| | 2510 | 34 | | _contactId2 = 0UL; |
| | 2510 | 35 | | _contactId3 = 0UL; |
| | 2510 | 36 | | _impulse0 = default; |
| | 2510 | 37 | | _impulse1 = default; |
| | 2510 | 38 | | _impulse2 = default; |
| | 2510 | 39 | | _impulse3 = default; |
| | 2510 | 40 | | } |
| | | 41 | | |
| | | 42 | | public void Set( |
| | | 43 | | ulong contactId, |
| | | 44 | | Vector3d normal, |
| | | 45 | | Fixed64 normalImpulse, |
| | | 46 | | Fixed64 tangentImpulse, |
| | | 47 | | Fixed64 secondaryTangentImpulse = default) |
| | | 48 | | { |
| | 9121 | 49 | | ContactWarmStartImpulse impulse = new(normal, normalImpulse, tangentImpulse, secondaryTangentImpulse); |
| | 66304 | 50 | | for (int i = 0; i < Count; i++) |
| | | 51 | | { |
| | 32046 | 52 | | if (GetContactId(i) != contactId) |
| | | 53 | | continue; |
| | | 54 | | |
| | 8015 | 55 | | SetImpulseUnchecked(i, impulse); |
| | 8015 | 56 | | return; |
| | | 57 | | } |
| | | 58 | | |
| | 1106 | 59 | | if (Count < ContactManifold.MaxContactCount) |
| | | 60 | | { |
| | 380 | 61 | | SetContactIdUnchecked(Count, contactId); |
| | 380 | 62 | | SetImpulseUnchecked(Count, impulse); |
| | 380 | 63 | | Count++; |
| | 380 | 64 | | return; |
| | | 65 | | } |
| | | 66 | | |
| | | 67 | | // Contact manifolds are already reduced to four stable contacts; replacement preserves bounded state if an olde |
| | 726 | 68 | | SetContactIdUnchecked(ContactManifold.MaxContactCount - 1, contactId); |
| | 726 | 69 | | SetImpulseUnchecked(ContactManifold.MaxContactCount - 1, impulse); |
| | 726 | 70 | | } |
| | | 71 | | |
| | | 72 | | public bool TryGet(ulong contactId, out ContactWarmStartImpulse impulse) |
| | | 73 | | { |
| | 66124 | 74 | | for (int i = 0; i < Count; i++) |
| | | 75 | | { |
| | 32022 | 76 | | if (GetContactId(i) != contactId) |
| | | 77 | | continue; |
| | | 78 | | |
| | 8093 | 79 | | impulse = GetImpulseUnchecked(i); |
| | 8093 | 80 | | return true; |
| | | 81 | | } |
| | | 82 | | |
| | 1040 | 83 | | impulse = default; |
| | 1040 | 84 | | return false; |
| | | 85 | | } |
| | | 86 | | |
| | | 87 | | public bool Remove(ulong contactId) |
| | | 88 | | { |
| | 32 | 89 | | for (int index = 0; index < Count; index++) |
| | | 90 | | { |
| | 12 | 91 | | if (GetContactId(index) != contactId) |
| | | 92 | | continue; |
| | | 93 | | |
| | 9 | 94 | | Count--; |
| | 20 | 95 | | for (int shift = index; shift < Count; shift++) |
| | | 96 | | { |
| | 1 | 97 | | SetContactIdUnchecked( |
| | 1 | 98 | | shift, |
| | 1 | 99 | | GetContactId(shift + 1)); |
| | 1 | 100 | | SetImpulseUnchecked( |
| | 1 | 101 | | shift, |
| | 1 | 102 | | GetImpulseUnchecked(shift + 1)); |
| | | 103 | | } |
| | | 104 | | |
| | 9 | 105 | | SetContactIdUnchecked(Count, 0UL); |
| | 9 | 106 | | SetImpulseUnchecked(Count, default); |
| | 9 | 107 | | return true; |
| | | 108 | | } |
| | | 109 | | |
| | 4 | 110 | | return false; |
| | | 111 | | } |
| | | 112 | | |
| | | 113 | | internal ulong GetContactIdForReplayHash(int index) |
| | | 114 | | { |
| | 3 | 115 | | SwiftThrowHelper.ThrowIfArrayIndexInvalid(index, Count, nameof(index)); |
| | 3 | 116 | | return GetContactId(index); |
| | | 117 | | } |
| | | 118 | | |
| | | 119 | | internal ContactWarmStartImpulse GetImpulseForReplayHash(int index) |
| | | 120 | | { |
| | 1 | 121 | | SwiftThrowHelper.ThrowIfArrayIndexInvalid(index, Count, nameof(index)); |
| | 1 | 122 | | return GetImpulseUnchecked(index); |
| | | 123 | | } |
| | | 124 | | |
| | | 125 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 126 | | private ulong GetContactId(int index) => |
| | 64084 | 127 | | index switch |
| | 64084 | 128 | | { |
| | 17808 | 129 | | 0 => _contactId0, |
| | 16377 | 130 | | 1 => _contactId1, |
| | 15387 | 131 | | 2 => _contactId2, |
| | 14512 | 132 | | _ => _contactId3 |
| | 64084 | 133 | | }; |
| | | 134 | | |
| | | 135 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 136 | | private ContactWarmStartImpulse GetImpulseUnchecked(int index) => |
| | 8095 | 137 | | index switch |
| | 8095 | 138 | | { |
| | 681 | 139 | | 0 => _impulse0, |
| | 450 | 140 | | 1 => _impulse1, |
| | 404 | 141 | | 2 => _impulse2, |
| | 6560 | 142 | | _ => _impulse3 |
| | 8095 | 143 | | }; |
| | | 144 | | |
| | | 145 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 146 | | private void SetContactIdUnchecked(int index, ulong contactId) |
| | | 147 | | { |
| | | 148 | | switch (index) |
| | | 149 | | { |
| | | 150 | | case 0: |
| | 237 | 151 | | _contactId0 = contactId; |
| | 237 | 152 | | break; |
| | | 153 | | case 1: |
| | 62 | 154 | | _contactId1 = contactId; |
| | 62 | 155 | | break; |
| | | 156 | | case 2: |
| | 51 | 157 | | _contactId2 = contactId; |
| | 51 | 158 | | break; |
| | | 159 | | default: |
| | 766 | 160 | | _contactId3 = contactId; |
| | | 161 | | break; |
| | | 162 | | } |
| | 766 | 163 | | } |
| | | 164 | | |
| | | 165 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 166 | | private void SetImpulseUnchecked(int index, ContactWarmStartImpulse impulse) |
| | | 167 | | { |
| | | 168 | | switch (index) |
| | | 169 | | { |
| | | 170 | | case 0: |
| | 869 | 171 | | _impulse0 = impulse; |
| | 869 | 172 | | break; |
| | | 173 | | case 1: |
| | 512 | 174 | | _impulse1 = impulse; |
| | 512 | 175 | | break; |
| | | 176 | | case 2: |
| | 454 | 177 | | _impulse2 = impulse; |
| | 454 | 178 | | break; |
| | | 179 | | default: |
| | 7296 | 180 | | _impulse3 = impulse; |
| | | 181 | | break; |
| | | 182 | | } |
| | 7296 | 183 | | } |
| | | 184 | | } |