| | | 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 | | |
| | | 10 | | namespace Gravitas.CollisionHandling; |
| | | 11 | | |
| | | 12 | | |
| | | 13 | | |
| | | 14 | | /// <summary>Retained pair-owned warm starts with independent two-point groups.</summary> |
| | | 15 | | internal struct ContactWarmStartCache2D |
| | | 16 | | { |
| | | 17 | | private ContactWarmStartGroup2D first; |
| | | 18 | | private SwiftCollections.SwiftList<ContactWarmStartGroup2D>? additional; |
| | | 19 | | internal int GroupCount { get; private set; } |
| | | 20 | | public int Count |
| | | 21 | | { |
| | | 22 | | get |
| | | 23 | | { |
| | 196 | 24 | | int count = 0; |
| | 956 | 25 | | for (int i = 0; i < GroupCount; i++) count += i == 0 ? first.Count : additional!.InnerArray[i - 1].Count; |
| | 196 | 26 | | return count; |
| | | 27 | | } |
| | | 28 | | } |
| | | 29 | | |
| | | 30 | | internal ContactWarmStartGroup2D GetGroup(int index) => |
| | 197 | 31 | | index == 0 ? first : additional!.InnerArray[index - 1]; |
| | | 32 | | |
| | | 33 | | private void SetGroup(int index, in ContactWarmStartGroup2D value) |
| | | 34 | | { |
| | 6932 | 35 | | if (index == 0) first = value; |
| | 296 | 36 | | else additional!.InnerArray[index - 1] = value; |
| | 296 | 37 | | } |
| | | 38 | | |
| | | 39 | | public void Clear() |
| | | 40 | | { |
| | 1558 | 41 | | GroupCount = 0; |
| | 1558 | 42 | | first = default; |
| | 1558 | 43 | | additional?.FastClear(); |
| | 234 | 44 | | } |
| | | 45 | | |
| | | 46 | | internal void Set(in ContactGroupKey key, ulong contactId, Fixed64 normalImpulse, Fixed64 tangentImpulse) |
| | | 47 | | { |
| | 33598 | 48 | | int index = 0, order = -1; |
| | 20300 | 49 | | while (index < GroupCount) |
| | | 50 | | { |
| | | 51 | | // Key lookup must not copy the group's two retained impulse rows. |
| | 16710 | 52 | | ref readonly ContactWarmStartGroup2D candidate = ref (index == 0 ? ref first : ref additional!.InnerArray[in |
| | 16710 | 53 | | order = candidate.Key.CompareTo(key); |
| | 16710 | 54 | | if (order >= 0) break; |
| | 3501 | 55 | | index++; |
| | | 56 | | } |
| | 16799 | 57 | | if (index == GroupCount || order != 0) |
| | | 58 | | { |
| | 3598 | 59 | | if (GroupCount > 0) |
| | | 60 | | { |
| | 286 | 61 | | additional ??= new SwiftCollections.SwiftList<ContactWarmStartGroup2D>(); |
| | 286 | 62 | | additional.Add(default); |
| | 608 | 63 | | for (int i = GroupCount; i > index; i--) SetGroup(i, GetGroup(i - 1)); |
| | | 64 | | } |
| | 3598 | 65 | | SetGroup(index, new ContactWarmStartGroup2D { Key = key }); |
| | 3598 | 66 | | GroupCount++; |
| | | 67 | | } |
| | 16799 | 68 | | ref ContactWarmStartGroup2D current = ref (index == 0 ? ref first : ref additional!.InnerArray[index - 1]); |
| | 16799 | 69 | | current.Set(contactId, normalImpulse, tangentImpulse); |
| | 16799 | 70 | | } |
| | | 71 | | |
| | | 72 | | internal bool TryGet(in ContactGroupKey key, ulong contactId, out ContactWarmStartImpulse impulse) |
| | | 73 | | { |
| | 17829 | 74 | | int index = Find(key); |
| | 17829 | 75 | | if (index >= 0) |
| | 13553 | 76 | | return index == 0 ? first.TryGet(contactId, out impulse) |
| | 13553 | 77 | | : additional!.InnerArray[index - 1].TryGet(contactId, out impulse); |
| | 4276 | 78 | | impulse = default; |
| | 4276 | 79 | | return false; |
| | | 80 | | } |
| | | 81 | | |
| | | 82 | | internal bool Remove(in ContactGroupKey key, ulong contactId) |
| | | 83 | | { |
| | 15 | 84 | | int index = Find(key); |
| | 20 | 85 | | if (index < 0) return false; |
| | 10 | 86 | | ref ContactWarmStartGroup2D current = ref (index == 0 ? ref first : ref additional!.InnerArray[index - 1]); |
| | 11 | 87 | | if (!current.Remove(contactId)) return false; |
| | 15 | 88 | | if (current.Count == 0) RemoveGroup(index); |
| | 9 | 89 | | return true; |
| | | 90 | | } |
| | | 91 | | |
| | | 92 | | internal void Retain(ContactManifold2D manifold) |
| | | 93 | | { |
| | 38440 | 94 | | for (int group = GroupCount - 1; group >= 0; group--) |
| | | 95 | | { |
| | 9405 | 96 | | ref ContactWarmStartGroup2D cached = ref (group == 0 ? ref first : ref additional!.InnerArray[group - 1]); |
| | 9405 | 97 | | int current = manifold.FindGroup(cached.Key); |
| | 9405 | 98 | | if (current < 0) |
| | | 99 | | { |
| | 9 | 100 | | RemoveGroup(group); |
| | 9 | 101 | | continue; |
| | | 102 | | } |
| | 9396 | 103 | | ref ContactGroup2D admitted = ref manifold.GetGroup(current); |
| | 45928 | 104 | | for (int point = cached.Count - 1; point >= 0; point--) |
| | | 105 | | { |
| | 13568 | 106 | | ulong id = cached.GetContactId(point); |
| | 13568 | 107 | | bool present = false; |
| | 92854 | 108 | | for (int i = 0; i < admitted.Count; i++) present |= admitted[i].ContactId == id; |
| | 16548 | 109 | | if (!present) cached.Remove(id); |
| | | 110 | | } |
| | 12254 | 111 | | if (cached.Count == 0) RemoveGroup(group); |
| | | 112 | | } |
| | 9815 | 113 | | } |
| | | 114 | | |
| | | 115 | | private int Find(in ContactGroupKey key) |
| | | 116 | | { |
| | 35688 | 117 | | int low = 0, high = GroupCount - 1; |
| | 20435 | 118 | | while (low <= high) |
| | | 119 | | { |
| | 16154 | 120 | | int middle = low + ((high - low) >> 1); |
| | 16154 | 121 | | ref readonly ContactWarmStartGroup2D current = ref (middle == 0 ? ref first : ref additional!.InnerArray[mid |
| | 16154 | 122 | | int order = current.Key.CompareTo(key); |
| | 29717 | 123 | | if (order == 0) return middle; |
| | 4459 | 124 | | if (order < 0) low = middle + 1; |
| | 723 | 125 | | else high = middle - 1; |
| | | 126 | | } |
| | 4281 | 127 | | return -1; |
| | | 128 | | } |
| | | 129 | | |
| | | 130 | | private void RemoveGroup(int index) |
| | | 131 | | { |
| | 5758 | 132 | | for (int i = index; i < GroupCount - 1; i++) SetGroup(i, GetGroup(i + 1)); |
| | 3034 | 133 | | if (GroupCount > 1) additional!.RemoveAt(additional.Count - 1); |
| | 2712 | 134 | | else first = default; |
| | 2873 | 135 | | GroupCount--; |
| | 2873 | 136 | | } |
| | | 137 | | |
| | | 138 | | } |