| | | 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 | | |
| | | 10 | | namespace Gravitas.CollisionHandling; |
| | | 11 | | |
| | | 12 | | |
| | | 13 | | |
| | | 14 | | |
| | | 15 | | /// <summary>Retained pair-owned warm starts with independent four-point groups.</summary> |
| | | 16 | | internal struct ContactWarmStartCache |
| | | 17 | | { |
| | | 18 | | private ContactWarmStartGroup first; |
| | | 19 | | private SwiftCollections.SwiftList<ContactWarmStartGroup>? additional; |
| | | 20 | | internal int GroupCount { get; private set; } |
| | | 21 | | public int Count |
| | | 22 | | { |
| | | 23 | | get |
| | | 24 | | { |
| | 1432 | 25 | | int count = 0; |
| | 8567 | 26 | | for (int i = 0; i < GroupCount; i++) count += i == 0 ? first.Count : additional!.InnerArray[i - 1].Count; |
| | 1432 | 27 | | return count; |
| | | 28 | | } |
| | | 29 | | } |
| | | 30 | | |
| | | 31 | | internal ContactWarmStartGroup GetGroup(int index) => |
| | 7743 | 32 | | index == 0 ? first : additional!.InnerArray[index - 1]; |
| | | 33 | | |
| | | 34 | | private void SetGroup(int index, in ContactWarmStartGroup value) |
| | | 35 | | { |
| | 91667 | 36 | | if (index == 0) first = value; |
| | 15869 | 37 | | else additional!.InnerArray[index - 1] = value; |
| | 15869 | 38 | | } |
| | | 39 | | |
| | | 40 | | public void Clear() |
| | | 41 | | { |
| | 77304 | 42 | | GroupCount = 0; |
| | 77304 | 43 | | first = default; |
| | 77304 | 44 | | additional?.FastClear(); |
| | 12982 | 45 | | } |
| | | 46 | | |
| | | 47 | | public void Set(ulong contactId, Vector3d normal, Fixed64 normalImpulse, |
| | | 48 | | Fixed64 tangentImpulse, Fixed64 secondaryTangentImpulse = default) => |
| | 74 | 49 | | Set(default, contactId, normal, normalImpulse, tangentImpulse, secondaryTangentImpulse); |
| | | 50 | | |
| | | 51 | | internal void Set(in ContactGroupKey key, ulong contactId, Vector3d normal, |
| | | 52 | | Fixed64 normalImpulse, Fixed64 tangentImpulse, Fixed64 secondaryTangentImpulse) |
| | | 53 | | { |
| | 1923386 | 54 | | int index = 0, order = -1; |
| | 1156292 | 55 | | while (index < GroupCount) |
| | | 56 | | { |
| | | 57 | | // Key lookup must not copy the group's four retained impulse rows. |
| | 1109676 | 58 | | ref readonly ContactWarmStartGroup candidate = ref (index == 0 ? ref first : ref additional!.InnerArray[inde |
| | 1109676 | 59 | | order = candidate.Key.CompareTo(key); |
| | 1109676 | 60 | | if (order >= 0) break; |
| | 194599 | 61 | | index++; |
| | | 62 | | } |
| | 961693 | 63 | | if (index == GroupCount || order != 0) |
| | | 64 | | { |
| | 47914 | 65 | | if (GroupCount > 0) |
| | | 66 | | { |
| | 11314 | 67 | | additional ??= new SwiftCollections.SwiftList<ContactWarmStartGroup>(); |
| | 11314 | 68 | | additional.Add(default); |
| | 40133 | 69 | | for (int i = GroupCount; i > index; i--) SetGroup(i, GetGroup(i - 1)); |
| | | 70 | | } |
| | 47914 | 71 | | SetGroup(index, new ContactWarmStartGroup { Key = key }); |
| | 47914 | 72 | | GroupCount++; |
| | | 73 | | } |
| | 961693 | 74 | | ref ContactWarmStartGroup current = ref (index == 0 ? ref first : ref additional!.InnerArray[index - 1]); |
| | 961693 | 75 | | current.Set(contactId, normal, normalImpulse, tangentImpulse, secondaryTangentImpulse); |
| | 961693 | 76 | | } |
| | | 77 | | |
| | | 78 | | public bool TryGet(ulong contactId, out ContactWarmStartImpulse impulse) => |
| | 80 | 79 | | TryGet(default, contactId, out impulse); |
| | | 80 | | |
| | | 81 | | internal bool TryGet(in ContactGroupKey key, ulong contactId, out ContactWarmStartImpulse impulse) |
| | | 82 | | { |
| | 893782 | 83 | | int index = Find(key); |
| | 893782 | 84 | | if (index >= 0) |
| | 775783 | 85 | | return index == 0 ? first.TryGet(contactId, out impulse) |
| | 775783 | 86 | | : additional!.InnerArray[index - 1].TryGet(contactId, out impulse); |
| | 117999 | 87 | | impulse = default; |
| | 117999 | 88 | | return false; |
| | | 89 | | } |
| | | 90 | | |
| | 2 | 91 | | public bool Remove(ulong contactId) => Remove(default, contactId); |
| | | 92 | | |
| | | 93 | | internal bool Remove(in ContactGroupKey key, ulong contactId) |
| | | 94 | | { |
| | 20 | 95 | | int index = Find(key); |
| | 24 | 96 | | if (index < 0) return false; |
| | 16 | 97 | | ref ContactWarmStartGroup current = ref (index == 0 ? ref first : ref additional!.InnerArray[index - 1]); |
| | 18 | 98 | | if (!current.Remove(contactId)) return false; |
| | 27 | 99 | | if (current.Count == 0) RemoveGroup(index); |
| | 14 | 100 | | return true; |
| | | 101 | | } |
| | | 102 | | |
| | | 103 | | internal void Retain(ContactManifold manifold) |
| | | 104 | | { |
| | 389138 | 105 | | for (int group = GroupCount - 1; group >= 0; group--) |
| | | 106 | | { |
| | 79388 | 107 | | ref ContactWarmStartGroup cached = ref (group == 0 ? ref first : ref additional!.InnerArray[group - 1]); |
| | 79388 | 108 | | int current = manifold.FindGroup(cached.Key); |
| | 79388 | 109 | | if (current < 0) |
| | | 110 | | { |
| | 1313 | 111 | | RemoveGroup(group); |
| | 1313 | 112 | | continue; |
| | | 113 | | } |
| | 78075 | 114 | | ref ContactGroup admitted = ref manifold.GetGroup(current); |
| | 456098 | 115 | | for (int point = cached.Count - 1; point >= 0; point--) |
| | | 116 | | { |
| | 149974 | 117 | | ulong id = cached.GetContactId(point); |
| | 149974 | 118 | | bool present = false; |
| | 1529345 | 119 | | for (int i = 0; i < admitted.Count; i++) present |= admitted[i].ContactId == id; |
| | 217277 | 120 | | if (!present) cached.Remove(id); |
| | | 121 | | } |
| | 110993 | 122 | | if (cached.Count == 0) RemoveGroup(group); |
| | | 123 | | } |
| | 115181 | 124 | | } |
| | | 125 | | |
| | | 126 | | private int Find(in ContactGroupKey key) |
| | | 127 | | { |
| | 1787604 | 128 | | int low = 0, high = GroupCount - 1; |
| | 1043441 | 129 | | while (low <= high) |
| | | 130 | | { |
| | 925438 | 131 | | int middle = low + ((high - low) >> 1); |
| | 925438 | 132 | | ref readonly ContactWarmStartGroup current = ref (middle == 0 ? ref first : ref additional!.InnerArray[middl |
| | 925438 | 133 | | int order = current.Key.CompareTo(key); |
| | 1701237 | 134 | | if (order == 0) return middle; |
| | 279599 | 135 | | if (order < 0) low = middle + 1; |
| | 19679 | 136 | | else high = middle - 1; |
| | | 137 | | } |
| | 118003 | 138 | | return -1; |
| | | 139 | | } |
| | | 140 | | |
| | | 141 | | private void RemoveGroup(int index) |
| | | 142 | | { |
| | 68545 | 143 | | for (int i = index; i < GroupCount - 1; i++) SetGroup(i, GetGroup(i + 1)); |
| | 42857 | 144 | | if (GroupCount > 1) additional!.RemoveAt(additional.Count - 1); |
| | 25631 | 145 | | else first = default; |
| | 34244 | 146 | | GroupCount--; |
| | 34244 | 147 | | } |
| | | 148 | | |
| | | 149 | | } |