| | | 1 | | //======================================================================= |
| | | 2 | | // ContactGroup2D.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 | | |
| | | 10 | | namespace Gravitas.CollisionHandling; |
| | | 11 | | |
| | | 12 | | /// <summary>Two inline samples belonging to one independent planar surface region.</summary> |
| | | 13 | | internal struct ContactGroup2D |
| | | 14 | | { |
| | | 15 | | internal ContactGroupKey Key; |
| | | 16 | | internal int Count; |
| | | 17 | | private ManifoldContact2D _contact0, _contact1; |
| | | 18 | | |
| | 53559 | 19 | | internal ManifoldContact2D this[int index] => GetContactUnchecked(index); |
| | | 20 | | |
| | | 21 | | internal int Add(in ManifoldContact2D contact) |
| | | 22 | | { |
| | 11308 | 23 | | ulong contactId = contact.ContactId; |
| | 25866 | 24 | | for (int i = 0; i < Count; i++) |
| | | 25 | | { |
| | 1630 | 26 | | ManifoldContact2D existing = GetContactUnchecked(i); |
| | 1630 | 27 | | if (existing.ContactId != contactId) |
| | | 28 | | continue; |
| | | 29 | | |
| | 5 | 30 | | if (IsDeeper(contact, existing)) |
| | 3 | 31 | | SetContactUnchecked(i, contact); |
| | 5 | 32 | | SortContactsById(); |
| | 5 | 33 | | return 0; |
| | | 34 | | } |
| | | 35 | | |
| | 11303 | 36 | | if (Count < ContactManifold2D.MaxContactsPerGroup) |
| | | 37 | | { |
| | 11293 | 38 | | SetContactUnchecked(Count, contact); |
| | 11293 | 39 | | Count++; |
| | 11293 | 40 | | SortContactsById(); |
| | 11293 | 41 | | return 1; |
| | | 42 | | } |
| | | 43 | | |
| | 10 | 44 | | int replaceIndex = FindShallowestReplacementIndex(contact); |
| | 10 | 45 | | if (replaceIndex < 0) |
| | 1 | 46 | | return 0; |
| | | 47 | | |
| | 9 | 48 | | SetContactUnchecked(replaceIndex, contact); |
| | 9 | 49 | | SortContactsById(); |
| | 9 | 50 | | return 0; |
| | | 51 | | } |
| | | 52 | | |
| | | 53 | | private int FindShallowestReplacementIndex(ManifoldContact2D candidate) |
| | | 54 | | { |
| | 10 | 55 | | int replaceIndex = !IsDeeper(_contact1, _contact0) |
| | 10 | 56 | | ? 1 |
| | 10 | 57 | | : 0; |
| | | 58 | | |
| | 10 | 59 | | ManifoldContact2D shallowest = GetContactUnchecked(replaceIndex); |
| | 10 | 60 | | if (IsDeeper(candidate, shallowest)) |
| | 8 | 61 | | return replaceIndex; |
| | | 62 | | |
| | 2 | 63 | | if (HasEqualDepth(candidate, shallowest) && candidate.ContactId < shallowest.ContactId) |
| | 1 | 64 | | return replaceIndex; |
| | | 65 | | |
| | 1 | 66 | | return -1; |
| | | 67 | | } |
| | | 68 | | |
| | | 69 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 70 | | private static bool IsDeeper(ManifoldContact2D candidate, ManifoldContact2D existing) => |
| | 25 | 71 | | candidate.Depth > existing.Depth |
| | 25 | 72 | | || candidate.Depth == existing.Depth |
| | 25 | 73 | | && candidate.DepthIsClamped |
| | 25 | 74 | | && !existing.DepthIsClamped; |
| | | 75 | | |
| | | 76 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 77 | | private static bool HasEqualDepth(ManifoldContact2D left, ManifoldContact2D right) => |
| | 2 | 78 | | left.Depth == right.Depth |
| | 2 | 79 | | && left.DepthIsClamped == right.DepthIsClamped; |
| | | 80 | | |
| | | 81 | | private void SortContactsById() |
| | | 82 | | { |
| | 11307 | 83 | | if (Count < 2 || _contact0.ContactId <= _contact1.ContactId) |
| | 10811 | 84 | | return; |
| | | 85 | | |
| | 496 | 86 | | (_contact0, _contact1) = (_contact1, _contact0); |
| | 496 | 87 | | } |
| | | 88 | | |
| | | 89 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 90 | | private ManifoldContact2D GetContactUnchecked(int index) => |
| | 55199 | 91 | | index == 0 ? _contact0 : _contact1; |
| | | 92 | | |
| | | 93 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 94 | | private void SetContactUnchecked(int index, ManifoldContact2D contact) |
| | | 95 | | { |
| | 11305 | 96 | | if (index == 0) |
| | | 97 | | { |
| | 9698 | 98 | | _contact0 = contact; |
| | 9698 | 99 | | return; |
| | | 100 | | } |
| | | 101 | | |
| | 1607 | 102 | | _contact1 = contact; |
| | 1607 | 103 | | } |
| | | 104 | | |
| | | 105 | | } |