| | | 1 | | //======================================================================= |
| | | 2 | | // ContactWarmStartGroup2D.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 | | /// Two inline warm-start rows belonging to one planar surface region. |
| | | 15 | | /// </summary> |
| | | 16 | | internal struct ContactWarmStartGroup2D |
| | | 17 | | { |
| | | 18 | | internal ContactGroupKey Key; |
| | | 19 | | private ulong _contactId0; |
| | | 20 | | private ulong _contactId1; |
| | | 21 | | private ContactWarmStartImpulse _impulse0; |
| | | 22 | | private ContactWarmStartImpulse _impulse1; |
| | | 23 | | |
| | | 24 | | public int Count { get; private set; } |
| | | 25 | | |
| | | 26 | | public void Set(ulong contactId, Fixed64 normalImpulse, Fixed64 tangentImpulse) |
| | | 27 | | { |
| | 16799 | 28 | | ContactWarmStartImpulse impulse = new(normalImpulse, tangentImpulse); |
| | 43192 | 29 | | for (int i = 0; i < Count; i++) |
| | | 30 | | { |
| | 17810 | 31 | | if (GetContactId(i) != contactId) |
| | | 32 | | continue; |
| | | 33 | | |
| | 13013 | 34 | | SetImpulseUnchecked(i, impulse); |
| | 13013 | 35 | | return; |
| | | 36 | | } |
| | | 37 | | |
| | 3786 | 38 | | if (Count < ContactManifold2D.MaxContactsPerGroup) |
| | | 39 | | { |
| | 3785 | 40 | | SetContactIdUnchecked(Count, contactId); |
| | 3785 | 41 | | SetImpulseUnchecked(Count, impulse); |
| | 3785 | 42 | | Count++; |
| | 3785 | 43 | | return; |
| | | 44 | | } |
| | | 45 | | |
| | 1 | 46 | | SetContactIdUnchecked(ContactManifold2D.MaxContactsPerGroup - 1, contactId); |
| | 1 | 47 | | SetImpulseUnchecked(ContactManifold2D.MaxContactsPerGroup - 1, impulse); |
| | 1 | 48 | | } |
| | | 49 | | |
| | | 50 | | public bool TryGet(ulong contactId, out ContactWarmStartImpulse impulse) |
| | | 51 | | { |
| | 36364 | 52 | | for (int i = 0; i < Count; i++) |
| | | 53 | | { |
| | 18175 | 54 | | if (GetContactId(i) != contactId) |
| | | 55 | | continue; |
| | | 56 | | |
| | 13546 | 57 | | impulse = GetImpulseUnchecked(i); |
| | 13546 | 58 | | return true; |
| | | 59 | | } |
| | | 60 | | |
| | 7 | 61 | | impulse = default; |
| | 7 | 62 | | return false; |
| | | 63 | | } |
| | | 64 | | |
| | | 65 | | public bool Remove(ulong contactId) |
| | | 66 | | { |
| | 6226 | 67 | | for (int i = 0; i < Count; i++) |
| | | 68 | | { |
| | 3112 | 69 | | if (GetContactId(i) != contactId) |
| | | 70 | | continue; |
| | | 71 | | |
| | 2989 | 72 | | Count--; |
| | 5984 | 73 | | for (int shift = i; shift < Count; shift++) |
| | | 74 | | { |
| | 3 | 75 | | SetContactIdUnchecked( |
| | 3 | 76 | | shift, |
| | 3 | 77 | | GetContactId(shift + 1)); |
| | 3 | 78 | | SetImpulseUnchecked( |
| | 3 | 79 | | shift, |
| | 3 | 80 | | GetImpulseUnchecked(shift + 1)); |
| | | 81 | | } |
| | | 82 | | |
| | 2989 | 83 | | SetContactIdUnchecked(Count, 0UL); |
| | 2989 | 84 | | SetImpulseUnchecked(Count, default); |
| | 2989 | 85 | | return true; |
| | | 86 | | } |
| | | 87 | | |
| | 1 | 88 | | return false; |
| | | 89 | | } |
| | | 90 | | |
| | | 91 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 52848 | 92 | | internal ulong GetContactId(int index) => index == 0 ? _contactId0 : _contactId1; |
| | | 93 | | |
| | | 94 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 13728 | 95 | | internal ContactWarmStartImpulse GetImpulseUnchecked(int index) => index == 0 ? _impulse0 : _impulse1; |
| | | 96 | | |
| | | 97 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 98 | | private void SetContactIdUnchecked(int index, ulong contactId) |
| | | 99 | | { |
| | 6778 | 100 | | if (index == 0) |
| | | 101 | | { |
| | 6465 | 102 | | _contactId0 = contactId; |
| | 6465 | 103 | | return; |
| | | 104 | | } |
| | | 105 | | |
| | 313 | 106 | | _contactId1 = contactId; |
| | 313 | 107 | | } |
| | | 108 | | |
| | | 109 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 110 | | private void SetImpulseUnchecked(int index, ContactWarmStartImpulse impulse) |
| | | 111 | | { |
| | 19791 | 112 | | if (index == 0) |
| | | 113 | | { |
| | 14870 | 114 | | _impulse0 = impulse; |
| | 14870 | 115 | | return; |
| | | 116 | | } |
| | | 117 | | |
| | 4921 | 118 | | _impulse1 = impulse; |
| | 4921 | 119 | | } |
| | | 120 | | } |