| | | 1 | | //======================================================================= |
| | | 2 | | // ConstraintEndpointJointIndex.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 SwiftCollections; |
| | | 9 | | using System; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.Constraints; |
| | | 12 | | |
| | | 13 | | /// <summary> |
| | | 14 | | /// Tracks registered joint IDs in stable endpoint order without scanning a context's peak joint range. |
| | | 15 | | /// </summary> |
| | | 16 | | internal sealed class ConstraintEndpointJointIndex<TBody> |
| | | 17 | | where TBody : class |
| | | 18 | | { |
| | | 19 | | private const int DefaultJointCapacity = 64; |
| | | 20 | | |
| | 10166 | 21 | | private readonly SwiftDictionary<TBody, EndpointChain> _chainsByBody = new(); |
| | 10166 | 22 | | private JointEndpoints[] _endpointsByJoint = new JointEndpoints[DefaultJointCapacity]; |
| | | 23 | | |
| | | 24 | | internal void Add(TBody bodyA, TBody bodyB, int jointId) |
| | | 25 | | { |
| | 507 | 26 | | SwiftThrowHelper.ThrowIfTrue( |
| | 507 | 27 | | ReferenceEquals(bodyA, bodyB), |
| | 507 | 28 | | nameof(bodyB), |
| | 507 | 29 | | "A joint endpoint index requires two distinct bodies."); |
| | 507 | 30 | | EnsureJointCapacity(jointId + 1); |
| | 507 | 31 | | ref JointEndpoints endpoints = ref _endpointsByJoint[jointId]; |
| | 507 | 32 | | SwiftThrowHelper.ThrowIfTrue( |
| | 507 | 33 | | endpoints.IsRegistered, |
| | 507 | 34 | | nameof(jointId), |
| | 507 | 35 | | "Joint endpoint ownership is already registered."); |
| | | 36 | | |
| | 507 | 37 | | endpoints = new JointEndpoints(bodyA, bodyB); |
| | 507 | 38 | | Append(bodyA, jointId, ref endpoints.EndpointA); |
| | 507 | 39 | | Append(bodyB, jointId, ref endpoints.EndpointB); |
| | 507 | 40 | | endpoints.IsRegistered = true; |
| | 507 | 41 | | } |
| | | 42 | | |
| | | 43 | | internal void Remove(int jointId) |
| | | 44 | | { |
| | 57 | 45 | | ref JointEndpoints endpoints = ref _endpointsByJoint[jointId]; |
| | 57 | 46 | | RemoveEndpoint(jointId, ref endpoints.EndpointA); |
| | 57 | 47 | | RemoveEndpoint(jointId, ref endpoints.EndpointB); |
| | 57 | 48 | | endpoints = default; |
| | 57 | 49 | | } |
| | | 50 | | |
| | | 51 | | internal bool TryGetLast(TBody body, out int jointId) |
| | | 52 | | { |
| | 381 | 53 | | if (_chainsByBody.TryGetValue(body, out EndpointChain chain)) |
| | | 54 | | { |
| | 55 | 55 | | jointId = chain.LastJointId; |
| | 55 | 56 | | return true; |
| | | 57 | | } |
| | | 58 | | |
| | 326 | 59 | | jointId = -1; |
| | 326 | 60 | | return false; |
| | | 61 | | } |
| | | 62 | | |
| | | 63 | | internal bool TryGetPrevious(TBody body, int jointId, out int previousJointId) |
| | | 64 | | { |
| | 33 | 65 | | previousJointId = GetEndpoint(jointId, body).PreviousJointId; |
| | 33 | 66 | | return previousJointId != 0; |
| | | 67 | | } |
| | | 68 | | |
| | | 69 | | internal void Clear() |
| | | 70 | | { |
| | 10228 | 71 | | _chainsByBody.Clear(); |
| | 10228 | 72 | | Array.Clear(_endpointsByJoint, 0, _endpointsByJoint.Length); |
| | 10228 | 73 | | } |
| | | 74 | | |
| | | 75 | | private void Append(TBody body, int jointId, ref EndpointLink endpoint) |
| | | 76 | | { |
| | 1014 | 77 | | endpoint.Body = body; |
| | 1014 | 78 | | if (!_chainsByBody.TryGetValue(body, out EndpointChain chain)) |
| | | 79 | | { |
| | 538 | 80 | | _chainsByBody.Add(body, new EndpointChain(jointId, jointId, 1)); |
| | 538 | 81 | | return; |
| | | 82 | | } |
| | | 83 | | |
| | 476 | 84 | | endpoint.PreviousJointId = chain.LastJointId; |
| | 476 | 85 | | ref EndpointLink previous = ref GetEndpoint(chain.LastJointId, body); |
| | 476 | 86 | | previous.NextJointId = jointId; |
| | 476 | 87 | | chain.LastJointId = jointId; |
| | 476 | 88 | | chain.Count++; |
| | 476 | 89 | | _chainsByBody[body] = chain; |
| | 476 | 90 | | } |
| | | 91 | | |
| | | 92 | | private void RemoveEndpoint(int jointId, ref EndpointLink endpoint) |
| | | 93 | | { |
| | 114 | 94 | | TBody body = endpoint.Body!; |
| | 114 | 95 | | SwiftThrowHelper.ThrowIfTrue( |
| | 114 | 96 | | !_chainsByBody.TryGetValue(body, out EndpointChain chain), |
| | 114 | 97 | | nameof(jointId), |
| | 114 | 98 | | "Joint endpoint chain is missing its body."); |
| | | 99 | | |
| | 114 | 100 | | if (endpoint.PreviousJointId == 0) |
| | 98 | 101 | | chain.FirstJointId = endpoint.NextJointId; |
| | | 102 | | else |
| | 16 | 103 | | GetEndpoint(endpoint.PreviousJointId, body).NextJointId = endpoint.NextJointId; |
| | | 104 | | |
| | 114 | 105 | | if (endpoint.NextJointId == 0) |
| | 99 | 106 | | chain.LastJointId = endpoint.PreviousJointId; |
| | | 107 | | else |
| | 15 | 108 | | GetEndpoint(endpoint.NextJointId, body).PreviousJointId = endpoint.PreviousJointId; |
| | | 109 | | |
| | 114 | 110 | | chain.Count--; |
| | 114 | 111 | | if (chain.Count == 0) |
| | 87 | 112 | | _chainsByBody.Remove(body); |
| | | 113 | | else |
| | 27 | 114 | | _chainsByBody[body] = chain; |
| | 27 | 115 | | } |
| | | 116 | | |
| | | 117 | | private ref EndpointLink GetEndpoint(int jointId, TBody body) |
| | | 118 | | { |
| | 540 | 119 | | ref JointEndpoints endpoints = ref _endpointsByJoint[jointId]; |
| | 540 | 120 | | if (ReferenceEquals(endpoints.EndpointA.Body, body)) |
| | 237 | 121 | | return ref endpoints.EndpointA; |
| | | 122 | | |
| | 303 | 123 | | SwiftThrowHelper.ThrowIfTrue( |
| | 303 | 124 | | !ReferenceEquals(endpoints.EndpointB.Body, body), |
| | 303 | 125 | | nameof(body), |
| | 303 | 126 | | "Joint endpoint chain references an unrelated body."); |
| | 303 | 127 | | return ref endpoints.EndpointB; |
| | | 128 | | } |
| | | 129 | | |
| | | 130 | | private void EnsureJointCapacity(int required) |
| | | 131 | | { |
| | 507 | 132 | | if (required <= _endpointsByJoint.Length) |
| | 505 | 133 | | return; |
| | | 134 | | |
| | 2 | 135 | | int newSize = _endpointsByJoint.Length; |
| | 4 | 136 | | while (newSize < required) |
| | 2 | 137 | | newSize *= 2; |
| | | 138 | | |
| | 2 | 139 | | Array.Resize(ref _endpointsByJoint, newSize); |
| | 2 | 140 | | } |
| | | 141 | | |
| | | 142 | | private struct EndpointChain |
| | | 143 | | { |
| | | 144 | | internal EndpointChain(int firstJointId, int lastJointId, int count) |
| | | 145 | | { |
| | 538 | 146 | | FirstJointId = firstJointId; |
| | 538 | 147 | | LastJointId = lastJointId; |
| | 538 | 148 | | Count = count; |
| | 538 | 149 | | } |
| | | 150 | | |
| | | 151 | | internal int FirstJointId; |
| | | 152 | | internal int LastJointId; |
| | | 153 | | internal int Count; |
| | | 154 | | } |
| | | 155 | | |
| | | 156 | | private struct JointEndpoints |
| | | 157 | | { |
| | | 158 | | internal JointEndpoints(TBody bodyA, TBody bodyB) |
| | | 159 | | { |
| | 507 | 160 | | EndpointA = new EndpointLink(bodyA); |
| | 507 | 161 | | EndpointB = new EndpointLink(bodyB); |
| | 507 | 162 | | IsRegistered = false; |
| | 507 | 163 | | } |
| | | 164 | | |
| | | 165 | | internal EndpointLink EndpointA; |
| | | 166 | | internal EndpointLink EndpointB; |
| | | 167 | | internal bool IsRegistered; |
| | | 168 | | } |
| | | 169 | | |
| | | 170 | | private struct EndpointLink |
| | | 171 | | { |
| | | 172 | | internal EndpointLink(TBody body) |
| | | 173 | | { |
| | 1014 | 174 | | Body = body; |
| | 1014 | 175 | | PreviousJointId = 0; |
| | 1014 | 176 | | NextJointId = 0; |
| | 1014 | 177 | | } |
| | | 178 | | |
| | | 179 | | internal TBody? Body; |
| | | 180 | | internal int PreviousJointId; |
| | | 181 | | internal int NextJointId; |
| | | 182 | | } |
| | | 183 | | } |