| | | 1 | | //======================================================================= |
| | | 2 | | // ContactWarmStartGroup.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.Runtime.CompilerServices; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.CollisionHandling; |
| | | 12 | | |
| | | 13 | | /// <summary> |
| | | 14 | | /// Four cached point impulses belonging to one geometric surface group. |
| | | 15 | | /// </summary> |
| | | 16 | | internal struct ContactWarmStartGroup |
| | | 17 | | { |
| | | 18 | | internal ContactGroupKey Key; |
| | | 19 | | private ulong _contactId0; |
| | | 20 | | private ulong _contactId1; |
| | | 21 | | private ulong _contactId2; |
| | | 22 | | private ulong _contactId3; |
| | | 23 | | private ContactWarmStartImpulse _impulse0; |
| | | 24 | | private ContactWarmStartImpulse _impulse1; |
| | | 25 | | private ContactWarmStartImpulse _impulse2; |
| | | 26 | | private ContactWarmStartImpulse _impulse3; |
| | | 27 | | |
| | | 28 | | public int Count { get; private set; } |
| | | 29 | | |
| | | 30 | | public void Set( |
| | | 31 | | ulong contactId, |
| | | 32 | | Vector3d normal, |
| | | 33 | | Fixed64 normalImpulse, |
| | | 34 | | Fixed64 tangentImpulse, |
| | | 35 | | Fixed64 secondaryTangentImpulse = default) |
| | | 36 | | { |
| | 961693 | 37 | | ContactWarmStartImpulse impulse = new(normal, normalImpulse, tangentImpulse, secondaryTangentImpulse); |
| | 3922406 | 38 | | for (int i = 0; i < Count; i++) |
| | | 39 | | { |
| | 1853178 | 40 | | if (GetContactId(i) != contactId) |
| | | 41 | | continue; |
| | | 42 | | |
| | 853668 | 43 | | SetImpulseUnchecked(i, impulse); |
| | 853668 | 44 | | return; |
| | | 45 | | } |
| | | 46 | | |
| | | 47 | | // Retain() removes vanished features before solving. A full stale cache |
| | | 48 | | // still stays bounded; insert by identity so replay is independent of discovery. |
| | 108025 | 49 | | int insertion = 0; |
| | 322211 | 50 | | while (insertion < Count && GetContactId(insertion) < contactId) insertion++; |
| | 108025 | 51 | | int last = Count < ContactManifold.MaxContactsPerGroup |
| | 108025 | 52 | | ? Count++ : ContactManifold.MaxContactsPerGroup - 1; |
| | 108026 | 53 | | if (insertion > last) insertion = last; |
| | 216404 | 54 | | for (int i = last; i > insertion; i--) |
| | | 55 | | { |
| | 177 | 56 | | SetContactIdUnchecked(i, GetContactId(i - 1)); |
| | 177 | 57 | | SetImpulseUnchecked(i, GetImpulseUnchecked(i - 1)); |
| | | 58 | | } |
| | 108025 | 59 | | SetContactIdUnchecked(insertion, contactId); |
| | 108025 | 60 | | SetImpulseUnchecked(insertion, impulse); |
| | 108025 | 61 | | } |
| | | 62 | | |
| | | 63 | | public bool TryGet(ulong contactId, out ContactWarmStartImpulse impulse) |
| | | 64 | | { |
| | 3189468 | 65 | | for (int i = 0; i < Count; i++) |
| | | 66 | | { |
| | 1539107 | 67 | | if (GetContactId(i) != contactId) |
| | | 68 | | continue; |
| | | 69 | | |
| | 720156 | 70 | | impulse = GetImpulseUnchecked(i); |
| | 720156 | 71 | | return true; |
| | | 72 | | } |
| | | 73 | | |
| | 55627 | 74 | | impulse = default; |
| | 55627 | 75 | | return false; |
| | | 76 | | } |
| | | 77 | | |
| | | 78 | | public bool Remove(ulong contactId) |
| | | 79 | | { |
| | 254140 | 80 | | for (int index = 0; index < Count; index++) |
| | | 81 | | { |
| | 127068 | 82 | | if (GetContactId(index) != contactId) |
| | | 83 | | continue; |
| | | 84 | | |
| | 67317 | 85 | | Count--; |
| | 134872 | 86 | | for (int shift = index; shift < Count; shift++) |
| | | 87 | | { |
| | 119 | 88 | | SetContactIdUnchecked( |
| | 119 | 89 | | shift, |
| | 119 | 90 | | GetContactId(shift + 1)); |
| | 119 | 91 | | SetImpulseUnchecked( |
| | 119 | 92 | | shift, |
| | 119 | 93 | | GetImpulseUnchecked(shift + 1)); |
| | | 94 | | } |
| | | 95 | | |
| | 67317 | 96 | | SetContactIdUnchecked(Count, 0UL); |
| | 67317 | 97 | | SetImpulseUnchecked(Count, default); |
| | 67317 | 98 | | return true; |
| | | 99 | | } |
| | | 100 | | |
| | 2 | 101 | | return false; |
| | | 102 | | } |
| | | 103 | | |
| | | 104 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 105 | | internal ulong GetContactId(int index) => |
| | 3780737 | 106 | | index switch |
| | 3780737 | 107 | | { |
| | 1897020 | 108 | | 0 => _contactId0, |
| | 1057789 | 109 | | 1 => _contactId1, |
| | 576984 | 110 | | 2 => _contactId2, |
| | 248944 | 111 | | _ => _contactId3 |
| | 3780737 | 112 | | }; |
| | | 113 | | |
| | | 114 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 115 | | internal ContactWarmStartImpulse GetImpulseUnchecked(int index) => |
| | 724347 | 116 | | index switch |
| | 724347 | 117 | | { |
| | 321562 | 118 | | 0 => _impulse0, |
| | 180821 | 119 | | 1 => _impulse1, |
| | 122374 | 120 | | 2 => _impulse2, |
| | 99590 | 121 | | _ => _impulse3 |
| | 724347 | 122 | | }; |
| | | 123 | | |
| | | 124 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 125 | | private void SetContactIdUnchecked(int index, ulong contactId) |
| | | 126 | | { |
| | | 127 | | switch (index) |
| | | 128 | | { |
| | | 129 | | case 0: |
| | 80964 | 130 | | _contactId0 = contactId; |
| | 80964 | 131 | | break; |
| | | 132 | | case 1: |
| | 44033 | 133 | | _contactId1 = contactId; |
| | 44033 | 134 | | break; |
| | | 135 | | case 2: |
| | 28567 | 136 | | _contactId2 = contactId; |
| | 28567 | 137 | | break; |
| | | 138 | | default: |
| | 22074 | 139 | | _contactId3 = contactId; |
| | | 140 | | break; |
| | | 141 | | } |
| | 22074 | 142 | | } |
| | | 143 | | |
| | | 144 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 145 | | private void SetImpulseUnchecked(int index, ContactWarmStartImpulse impulse) |
| | | 146 | | { |
| | | 147 | | switch (index) |
| | | 148 | | { |
| | | 149 | | case 0: |
| | 439514 | 150 | | _impulse0 = impulse; |
| | 439514 | 151 | | break; |
| | | 152 | | case 1: |
| | 265646 | 153 | | _impulse1 = impulse; |
| | 265646 | 154 | | break; |
| | | 155 | | case 2: |
| | 178456 | 156 | | _impulse2 = impulse; |
| | 178456 | 157 | | break; |
| | | 158 | | default: |
| | 145690 | 159 | | _impulse3 = impulse; |
| | | 160 | | break; |
| | | 161 | | } |
| | 145690 | 162 | | } |
| | | 163 | | } |