| | | 1 | | //======================================================================= |
| | | 2 | | // ContactWarmStartCache2D.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 2D warm-start cache keyed by stable manifold contact identity. |
| | | 15 | | /// </summary> |
| | | 16 | | internal struct ContactWarmStartCache2D |
| | | 17 | | { |
| | | 18 | | private ulong _contactId0; |
| | | 19 | | private ulong _contactId1; |
| | | 20 | | private ContactWarmStartImpulse _impulse0; |
| | | 21 | | private ContactWarmStartImpulse _impulse1; |
| | | 22 | | |
| | | 23 | | public int Count { get; private set; } |
| | | 24 | | |
| | | 25 | | public void Clear() |
| | | 26 | | { |
| | 433 | 27 | | Count = 0; |
| | 433 | 28 | | _contactId0 = 0UL; |
| | 433 | 29 | | _contactId1 = 0UL; |
| | 433 | 30 | | _impulse0 = default; |
| | 433 | 31 | | _impulse1 = default; |
| | 433 | 32 | | } |
| | | 33 | | |
| | | 34 | | public void Set(ulong contactId, Fixed64 normalImpulse, Fixed64 tangentImpulse) |
| | | 35 | | { |
| | 1037 | 36 | | ContactWarmStartImpulse impulse = new(normalImpulse, tangentImpulse); |
| | 2162 | 37 | | for (int i = 0; i < Count; i++) |
| | | 38 | | { |
| | 765 | 39 | | if (GetContactId(i) != contactId) |
| | | 40 | | continue; |
| | | 41 | | |
| | 721 | 42 | | SetImpulseUnchecked(i, impulse); |
| | 721 | 43 | | return; |
| | | 44 | | } |
| | | 45 | | |
| | 316 | 46 | | if (Count < ContactManifold2D.MaxContactCount) |
| | | 47 | | { |
| | 308 | 48 | | SetContactIdUnchecked(Count, contactId); |
| | 308 | 49 | | SetImpulseUnchecked(Count, impulse); |
| | 308 | 50 | | Count++; |
| | 308 | 51 | | return; |
| | | 52 | | } |
| | | 53 | | |
| | 8 | 54 | | SetContactIdUnchecked(ContactManifold2D.MaxContactCount - 1, contactId); |
| | 8 | 55 | | SetImpulseUnchecked(ContactManifold2D.MaxContactCount - 1, impulse); |
| | 8 | 56 | | } |
| | | 57 | | |
| | | 58 | | public bool TryGet(ulong contactId, out ContactWarmStartImpulse impulse) |
| | | 59 | | { |
| | 2160 | 60 | | for (int i = 0; i < Count; i++) |
| | | 61 | | { |
| | 775 | 62 | | if (GetContactId(i) != contactId) |
| | | 63 | | continue; |
| | | 64 | | |
| | 736 | 65 | | impulse = GetImpulseUnchecked(i); |
| | 736 | 66 | | return true; |
| | | 67 | | } |
| | | 68 | | |
| | 305 | 69 | | impulse = default; |
| | 305 | 70 | | return false; |
| | | 71 | | } |
| | | 72 | | |
| | | 73 | | public bool Remove(ulong contactId) |
| | | 74 | | { |
| | 20 | 75 | | for (int i = 0; i < Count; i++) |
| | | 76 | | { |
| | 6 | 77 | | if (GetContactId(i) != contactId) |
| | | 78 | | continue; |
| | | 79 | | |
| | 5 | 80 | | Count--; |
| | 14 | 81 | | for (int shift = i; shift < Count; shift++) |
| | | 82 | | { |
| | 2 | 83 | | SetContactIdUnchecked( |
| | 2 | 84 | | shift, |
| | 2 | 85 | | GetContactId(shift + 1)); |
| | 2 | 86 | | SetImpulseUnchecked( |
| | 2 | 87 | | shift, |
| | 2 | 88 | | GetImpulseUnchecked(shift + 1)); |
| | | 89 | | } |
| | | 90 | | |
| | 5 | 91 | | SetContactIdUnchecked(Count, 0UL); |
| | 5 | 92 | | SetImpulseUnchecked(Count, default); |
| | 5 | 93 | | return true; |
| | | 94 | | } |
| | | 95 | | |
| | 4 | 96 | | return false; |
| | | 97 | | } |
| | | 98 | | |
| | | 99 | | internal ulong GetContactIdForReplayHash(int index) |
| | | 100 | | { |
| | 3 | 101 | | SwiftThrowHelper.ThrowIfArrayIndexInvalid(index, Count, nameof(index)); |
| | 3 | 102 | | return GetContactId(index); |
| | | 103 | | } |
| | | 104 | | |
| | | 105 | | internal ContactWarmStartImpulse GetImpulseForReplayHash(int index) |
| | | 106 | | { |
| | 2 | 107 | | SwiftThrowHelper.ThrowIfArrayIndexInvalid(index, Count, nameof(index)); |
| | 2 | 108 | | return GetImpulseUnchecked(index); |
| | | 109 | | } |
| | | 110 | | |
| | | 111 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1551 | 112 | | private ulong GetContactId(int index) => index == 0 ? _contactId0 : _contactId1; |
| | | 113 | | |
| | | 114 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 740 | 115 | | private ContactWarmStartImpulse GetImpulseUnchecked(int index) => index == 0 ? _impulse0 : _impulse1; |
| | | 116 | | |
| | | 117 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 118 | | private void SetContactIdUnchecked(int index, ulong contactId) |
| | | 119 | | { |
| | 323 | 120 | | if (index == 0) |
| | | 121 | | { |
| | 300 | 122 | | _contactId0 = contactId; |
| | 300 | 123 | | return; |
| | | 124 | | } |
| | | 125 | | |
| | 23 | 126 | | _contactId1 = contactId; |
| | 23 | 127 | | } |
| | | 128 | | |
| | | 129 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 130 | | private void SetImpulseUnchecked(int index, ContactWarmStartImpulse impulse) |
| | | 131 | | { |
| | 1044 | 132 | | if (index == 0) |
| | | 133 | | { |
| | 1006 | 134 | | _impulse0 = impulse; |
| | 1006 | 135 | | return; |
| | | 136 | | } |
| | | 137 | | |
| | 38 | 138 | | _impulse1 = impulse; |
| | 38 | 139 | | } |
| | | 140 | | } |