| | | 1 | | //======================================================================= |
| | | 2 | | // ExactContactResponseKernel.Coulomb.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; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | |
| | | 11 | | using FixedMathSharp; |
| | | 12 | | |
| | | 13 | | namespace Gravitas.CollisionHandling; |
| | | 14 | | |
| | | 15 | | /// <content> |
| | | 16 | | /// Owns exact line and disk Coulomb-friction policy and materialization. |
| | | 17 | | /// </content> |
| | | 18 | | internal static partial class ExactContactResponseKernel |
| | | 19 | | { |
| | | 20 | | private const int MaxCoulombWords = MaxResponseWords * 2; |
| | | 21 | | private const int MaxCoulombSquareWords = MaxCoulombWords * 2; |
| | | 22 | | // Normal-response ratios use fewer than 46 active words. A common tangent |
| | | 23 | | // denominator therefore uses fewer than 92, its squared magnitude fewer |
| | | 24 | | // than 185 including carry, and the largest radial comparison fewer than |
| | | 25 | | // 307. The remaining words prevent silent product truncation. |
| | | 26 | | private const int MaxCoulombComparisonWords = 320; |
| | | 27 | | |
| | | 28 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 29 | | internal static bool TryGetCoulombLineResponse( |
| | | 30 | | in ExactNormalConstraint3D normalConstraint, |
| | | 31 | | in ExactContactResponseOperand3D firstTangent, |
| | | 32 | | in ExactContactResponseOperand3D secondTangent, |
| | | 33 | | Vector3d tangent, |
| | | 34 | | Fixed64 accumulatedTangentImpulse, |
| | | 35 | | Fixed64 staticFriction, |
| | | 36 | | Fixed64 dynamicFriction, |
| | | 37 | | out ExactCoulombResponse3D response, |
| | | 38 | | in ContactResponseSnapshot impact = default) |
| | | 39 | | { |
| | 93 | 40 | | response = default; |
| | 93 | 41 | | bool inputsValid = |
| | 93 | 42 | | staticFriction >= Fixed64.Zero |
| | 93 | 43 | | & dynamicFriction >= Fixed64.Zero |
| | 93 | 44 | | & tangent.IsNormalized(); |
| | 93 | 45 | | if (!inputsValid) |
| | 3 | 46 | | return false; |
| | | 47 | | |
| | 90 | 48 | | bool constraintValid = |
| | 90 | 49 | | FixedMath.Abs(Vector3d.Dot( |
| | 90 | 50 | | normalConstraint.Normal, |
| | 90 | 51 | | tangent)) <= Fixed64.Epsilon |
| | 90 | 52 | | & HaveMatchingParticipants( |
| | 90 | 53 | | normalConstraint.First, |
| | 90 | 54 | | firstTangent) |
| | 90 | 55 | | & HaveMatchingParticipants( |
| | 90 | 56 | | normalConstraint.Second, |
| | 90 | 57 | | secondTangent); |
| | 90 | 58 | | if (!constraintValid) |
| | 2 | 59 | | return false; |
| | | 60 | | |
| | 88 | 61 | | Span<ulong> normalNumerator = stackalloc ulong[MaxResponseWords]; |
| | 88 | 62 | | Span<ulong> normalDenominator = stackalloc ulong[MaxResponseWords]; |
| | 88 | 63 | | Span<ulong> tangentNumerator = stackalloc ulong[MaxResponseWords]; |
| | 88 | 64 | | Span<ulong> tangentDenominator = stackalloc ulong[MaxResponseWords]; |
| | 88 | 65 | | if (!TryGetCompletedNormalAccumulatorRatio( |
| | 88 | 66 | | normalConstraint, |
| | 88 | 67 | | normalNumerator, |
| | 88 | 68 | | normalDenominator, impact)) |
| | 1 | 69 | | return false; |
| | 87 | 70 | | GetBilateralImpulseRatio( |
| | 87 | 71 | | firstTangent, |
| | 87 | 72 | | secondTangent, |
| | 87 | 73 | | tangent, |
| | 87 | 74 | | Fixed64.Zero, |
| | 87 | 75 | | tangentNumerator, |
| | 87 | 76 | | tangentDenominator, |
| | 87 | 77 | | out int tangentSign); |
| | | 78 | | |
| | 87 | 79 | | Span<ulong> desiredNumerator = stackalloc ulong[MaxResponseWords]; |
| | 87 | 80 | | AddFixedToRatio( |
| | 87 | 81 | | tangentNumerator, |
| | 87 | 82 | | tangentDenominator, |
| | 87 | 83 | | tangentSign, |
| | 87 | 84 | | accumulatedTangentImpulse, |
| | 87 | 85 | | desiredNumerator, |
| | 87 | 86 | | out int desiredSign); |
| | | 87 | | |
| | 87 | 88 | | Span<ulong> staticNumerator = stackalloc ulong[MaxResponseWords]; |
| | 87 | 89 | | Span<ulong> staticDenominator = stackalloc ulong[MaxResponseWords]; |
| | 87 | 90 | | GetFrictionLimit( |
| | 87 | 91 | | normalNumerator, |
| | 87 | 92 | | normalDenominator, |
| | 87 | 93 | | staticFriction, |
| | 87 | 94 | | staticNumerator, |
| | 87 | 95 | | staticDenominator); |
| | | 96 | | |
| | 87 | 97 | | Span<ulong> appliedNumerator = stackalloc ulong[MaxCoulombWords]; |
| | 87 | 98 | | Span<ulong> appliedDenominator = stackalloc ulong[MaxCoulombWords]; |
| | | 99 | | int appliedSign; |
| | 87 | 100 | | Span<ulong> accumulatedNumerator = stackalloc ulong[MaxCoulombWords]; |
| | 87 | 101 | | Span<ulong> accumulatedDenominator = stackalloc ulong[MaxCoulombWords]; |
| | | 102 | | int accumulatedSign; |
| | 87 | 103 | | appliedNumerator.Clear(); |
| | 87 | 104 | | appliedDenominator.Clear(); |
| | 87 | 105 | | accumulatedNumerator.Clear(); |
| | 87 | 106 | | accumulatedDenominator.Clear(); |
| | 87 | 107 | | Span<ulong> clampedNumerator = stackalloc ulong[MaxCoulombWords]; |
| | 87 | 108 | | Span<ulong> clampedDenominator = stackalloc ulong[MaxResponseWords]; |
| | 87 | 109 | | if (CompareRatios( |
| | 87 | 110 | | desiredNumerator, |
| | 87 | 111 | | tangentDenominator, |
| | 87 | 112 | | staticNumerator, |
| | 87 | 113 | | staticDenominator) <= 0) |
| | | 114 | | { |
| | 68 | 115 | | tangentNumerator.CopyTo(appliedNumerator); |
| | 68 | 116 | | tangentDenominator.CopyTo(appliedDenominator); |
| | 68 | 117 | | appliedSign = tangentSign; |
| | 68 | 118 | | desiredNumerator.CopyTo(accumulatedNumerator); |
| | 68 | 119 | | tangentDenominator.CopyTo(accumulatedDenominator); |
| | 68 | 120 | | accumulatedSign = desiredSign; |
| | | 121 | | } |
| | | 122 | | else |
| | | 123 | | { |
| | 19 | 124 | | GetFrictionLimit( |
| | 19 | 125 | | normalNumerator, |
| | 19 | 126 | | normalDenominator, |
| | 19 | 127 | | dynamicFriction, |
| | 19 | 128 | | clampedNumerator, |
| | 19 | 129 | | clampedDenominator); |
| | 19 | 130 | | clampedNumerator.CopyTo(accumulatedNumerator); |
| | 19 | 131 | | clampedDenominator.CopyTo(accumulatedDenominator); |
| | 19 | 132 | | accumulatedSign = WideArithmetic.IsZeroMagnitude( |
| | 19 | 133 | | clampedNumerator) |
| | 19 | 134 | | ? 0 |
| | 19 | 135 | | : desiredSign; |
| | 19 | 136 | | SubtractFixedFromRatio( |
| | 19 | 137 | | clampedNumerator, |
| | 19 | 138 | | clampedDenominator, |
| | 19 | 139 | | accumulatedSign, |
| | 19 | 140 | | accumulatedTangentImpulse, |
| | 19 | 141 | | appliedNumerator, |
| | 19 | 142 | | appliedDenominator, |
| | 19 | 143 | | out appliedSign); |
| | | 144 | | } |
| | | 145 | | |
| | 87 | 146 | | bool resolved = TryGetLinearVelocityDelta( |
| | 87 | 147 | | firstTangent.LinearImpulseAxis, |
| | 87 | 148 | | firstTangent.InverseMass, |
| | 87 | 149 | | appliedNumerator, |
| | 87 | 150 | | appliedDenominator, |
| | 87 | 151 | | appliedSign, |
| | 87 | 152 | | out Vector3d firstLinear); |
| | 87 | 153 | | resolved &= TryGetAngularVelocityDelta( |
| | 87 | 154 | | firstTangent.Lever, |
| | 87 | 155 | | -tangent, |
| | 87 | 156 | | firstTangent.InverseInertia, |
| | 87 | 157 | | appliedNumerator, |
| | 87 | 158 | | appliedDenominator, |
| | 87 | 159 | | appliedSign, |
| | 87 | 160 | | out Vector3d firstAngular); |
| | 87 | 161 | | resolved &= TryGetLinearVelocityDelta( |
| | 87 | 162 | | secondTangent.LinearImpulseAxis, |
| | 87 | 163 | | secondTangent.InverseMass, |
| | 87 | 164 | | appliedNumerator, |
| | 87 | 165 | | appliedDenominator, |
| | 87 | 166 | | appliedSign, |
| | 87 | 167 | | out Vector3d secondLinear); |
| | 87 | 168 | | resolved &= TryGetAngularVelocityDelta( |
| | 87 | 169 | | secondTangent.Lever, |
| | 87 | 170 | | tangent, |
| | 87 | 171 | | secondTangent.InverseInertia, |
| | 87 | 172 | | appliedNumerator, |
| | 87 | 173 | | appliedDenominator, |
| | 87 | 174 | | appliedSign, |
| | 87 | 175 | | out Vector3d secondAngular); |
| | 87 | 176 | | resolved &= firstTangent.CanRepresentAngularDelta(firstAngular); |
| | 87 | 177 | | resolved &= secondTangent.CanRepresentAngularDelta(secondAngular); |
| | 87 | 178 | | if (!resolved) |
| | 7 | 179 | | return false; |
| | | 180 | | |
| | 80 | 181 | | bool hasAccumulatedProjection = Fixed64.TryGetSignedRawRatio( |
| | 80 | 182 | | accumulatedNumerator, |
| | 80 | 183 | | accumulatedDenominator, |
| | 80 | 184 | | accumulatedSign < 0, |
| | 80 | 185 | | out Fixed64 accumulatedProjection); |
| | 80 | 186 | | response = new ExactCoulombResponse3D( |
| | 80 | 187 | | !WideArithmetic.IsZeroMagnitude(appliedNumerator), |
| | 80 | 188 | | firstLinear, |
| | 80 | 189 | | firstAngular, |
| | 80 | 190 | | secondLinear, |
| | 80 | 191 | | secondAngular, |
| | 80 | 192 | | hasAccumulatedProjection, |
| | 80 | 193 | | accumulatedProjection, |
| | 80 | 194 | | hasSecondaryAccumulatedImpulse: false, |
| | 80 | 195 | | secondaryAccumulatedImpulse: default); |
| | 80 | 196 | | return true; |
| | | 197 | | } |
| | | 198 | | |
| | | 199 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 200 | | internal static bool TryGetCoulombDiskResponse( |
| | | 201 | | Vector3d normal, |
| | | 202 | | Fixed64 completedNormalImpulse, |
| | | 203 | | in ExactContactResponseOperand3D primaryFirst, |
| | | 204 | | in ExactContactResponseOperand3D primarySecond, |
| | | 205 | | Vector3d primaryTangent, |
| | | 206 | | Fixed64 accumulatedPrimaryTangentImpulse, |
| | | 207 | | in ExactContactResponseOperand3D secondaryFirst, |
| | | 208 | | in ExactContactResponseOperand3D secondarySecond, |
| | | 209 | | Vector3d secondaryTangent, |
| | | 210 | | Fixed64 accumulatedSecondaryTangentImpulse, |
| | | 211 | | Fixed64 staticFriction, |
| | | 212 | | Fixed64 dynamicFriction, |
| | | 213 | | out ExactCoulombResponse3D response) |
| | | 214 | | { |
| | 662 | 215 | | ExactContactImpulseState? unused = null; |
| | 662 | 216 | | return TryGetCoulombDiskResponseRetained(normal, completedNormalImpulse, primaryFirst, primarySecond, primaryTan |
| | 662 | 217 | | accumulatedPrimaryTangentImpulse, secondaryFirst, secondarySecond, secondaryTangent, |
| | 662 | 218 | | accumulatedSecondaryTangentImpulse, staticFriction, dynamicFriction, out response, |
| | 662 | 219 | | ref unused, retainOversized: false); |
| | | 220 | | } |
| | | 221 | | |
| | | 222 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 223 | | internal static bool TryGetCoulombDiskResponse( |
| | | 224 | | Vector3d normal, |
| | | 225 | | Fixed64 completedNormalImpulse, |
| | | 226 | | in ExactContactResponseOperand3D primaryFirst, |
| | | 227 | | in ExactContactResponseOperand3D primarySecond, |
| | | 228 | | Vector3d primaryTangent, |
| | | 229 | | Fixed64 accumulatedPrimaryTangentImpulse, |
| | | 230 | | in ExactContactResponseOperand3D secondaryFirst, |
| | | 231 | | in ExactContactResponseOperand3D secondarySecond, |
| | | 232 | | Vector3d secondaryTangent, |
| | | 233 | | Fixed64 accumulatedSecondaryTangentImpulse, |
| | | 234 | | Fixed64 staticFriction, |
| | | 235 | | Fixed64 dynamicFriction, |
| | | 236 | | out ExactCoulombResponse3D response, |
| | | 237 | | ref ExactContactImpulseState? exactImpulse) => |
| | 354 | 238 | | TryGetCoulombDiskResponseRetained(normal, completedNormalImpulse, primaryFirst, primarySecond, primaryTangent, |
| | 354 | 239 | | accumulatedPrimaryTangentImpulse, secondaryFirst, secondarySecond, secondaryTangent, |
| | 354 | 240 | | accumulatedSecondaryTangentImpulse, staticFriction, dynamicFriction, out response, |
| | 354 | 241 | | ref exactImpulse, retainOversized: true); |
| | | 242 | | |
| | | 243 | | |
| | | 244 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 245 | | private static bool TryGetCoulombDiskResponseRetained( |
| | | 246 | | Vector3d normal, |
| | | 247 | | Fixed64 completedNormalImpulse, |
| | | 248 | | in ExactContactResponseOperand3D primaryFirst, |
| | | 249 | | in ExactContactResponseOperand3D primarySecond, |
| | | 250 | | Vector3d primaryTangent, |
| | | 251 | | Fixed64 accumulatedPrimaryTangentImpulse, |
| | | 252 | | in ExactContactResponseOperand3D secondaryFirst, |
| | | 253 | | in ExactContactResponseOperand3D secondarySecond, |
| | | 254 | | Vector3d secondaryTangent, |
| | | 255 | | Fixed64 accumulatedSecondaryTangentImpulse, |
| | | 256 | | Fixed64 staticFriction, |
| | | 257 | | Fixed64 dynamicFriction, |
| | | 258 | | out ExactCoulombResponse3D response, |
| | | 259 | | ref ExactContactImpulseState? exactImpulse, |
| | | 260 | | bool retainOversized) |
| | | 261 | | { |
| | 1016 | 262 | | response = default; |
| | 1016 | 263 | | if (completedNormalImpulse < Fixed64.Zero |
| | 1016 | 264 | | || !AreCoulombDiskInputsValid( |
| | 1016 | 265 | | normal, |
| | 1016 | 266 | | primaryFirst, |
| | 1016 | 267 | | primarySecond, |
| | 1016 | 268 | | primaryTangent, |
| | 1016 | 269 | | secondaryFirst, |
| | 1016 | 270 | | secondarySecond, |
| | 1016 | 271 | | secondaryTangent, |
| | 1016 | 272 | | staticFriction, |
| | 1016 | 273 | | dynamicFriction)) |
| | | 274 | | { |
| | 2 | 275 | | return false; |
| | | 276 | | } |
| | | 277 | | |
| | 1014 | 278 | | Span<ulong> normalNumerator = stackalloc ulong[MaxResponseWords]; |
| | 1014 | 279 | | Span<ulong> normalDenominator = stackalloc ulong[MaxResponseWords]; |
| | 1014 | 280 | | if (exactImpulse?.HasNormal == true) |
| | | 281 | | { |
| | 10 | 282 | | exactImpulse.NormalNumerator.CopyTo(normalNumerator); |
| | 10 | 283 | | exactImpulse.NormalDenominator.CopyTo(normalDenominator); |
| | | 284 | | } |
| | | 285 | | else |
| | | 286 | | { |
| | 1004 | 287 | | SetMagnitude(completedNormalImpulse, normalNumerator); |
| | 1004 | 288 | | normalDenominator.Clear(); |
| | 1004 | 289 | | normalDenominator[0] = 1UL; |
| | | 290 | | } |
| | 1014 | 291 | | return TryGetCoulombDiskResponseCore( |
| | 1014 | 292 | | normalNumerator, |
| | 1014 | 293 | | normalDenominator, |
| | 1014 | 294 | | primaryFirst, |
| | 1014 | 295 | | primarySecond, |
| | 1014 | 296 | | primaryTangent, |
| | 1014 | 297 | | accumulatedPrimaryTangentImpulse, |
| | 1014 | 298 | | secondaryFirst, |
| | 1014 | 299 | | secondarySecond, |
| | 1014 | 300 | | secondaryTangent, |
| | 1014 | 301 | | accumulatedSecondaryTangentImpulse, |
| | 1014 | 302 | | staticFriction, |
| | 1014 | 303 | | dynamicFriction, |
| | 1014 | 304 | | Fixed64.Epsilon, |
| | 1014 | 305 | | out response, ref exactImpulse, retainOversized); |
| | | 306 | | } |
| | | 307 | | |
| | | 308 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 309 | | internal static bool TryGetCoulombDiskResponse( |
| | | 310 | | in ExactNormalConstraint3D normalConstraint, |
| | | 311 | | in ExactContactResponseOperand3D primaryFirst, |
| | | 312 | | in ExactContactResponseOperand3D primarySecond, |
| | | 313 | | Vector3d primaryTangent, |
| | | 314 | | Fixed64 accumulatedPrimaryTangentImpulse, |
| | | 315 | | in ExactContactResponseOperand3D secondaryFirst, |
| | | 316 | | in ExactContactResponseOperand3D secondarySecond, |
| | | 317 | | Vector3d secondaryTangent, |
| | | 318 | | Fixed64 accumulatedSecondaryTangentImpulse, |
| | | 319 | | Fixed64 staticFriction, |
| | | 320 | | Fixed64 dynamicFriction, |
| | | 321 | | out ExactCoulombResponse3D response) |
| | | 322 | | { |
| | 60 | 323 | | response = default; |
| | 60 | 324 | | bool inputsValid = AreCoulombDiskInputsValid( |
| | 60 | 325 | | normalConstraint.Normal, |
| | 60 | 326 | | primaryFirst, |
| | 60 | 327 | | primarySecond, |
| | 60 | 328 | | primaryTangent, |
| | 60 | 329 | | secondaryFirst, |
| | 60 | 330 | | secondarySecond, |
| | 60 | 331 | | secondaryTangent, |
| | 60 | 332 | | staticFriction, |
| | 60 | 333 | | dynamicFriction) |
| | 60 | 334 | | & HaveMatchingParticipants( |
| | 60 | 335 | | normalConstraint.First, |
| | 60 | 336 | | primaryFirst) |
| | 60 | 337 | | & HaveMatchingParticipants( |
| | 60 | 338 | | normalConstraint.Second, |
| | 60 | 339 | | primarySecond) |
| | 60 | 340 | | & HaveMatchingParticipants(primaryFirst, secondaryFirst) |
| | 60 | 341 | | & HaveMatchingParticipants(primarySecond, secondarySecond); |
| | 60 | 342 | | if (!inputsValid) |
| | 3 | 343 | | return false; |
| | | 344 | | |
| | 57 | 345 | | Span<ulong> normalNumerator = stackalloc ulong[MaxResponseWords]; |
| | 57 | 346 | | Span<ulong> normalDenominator = stackalloc ulong[MaxResponseWords]; |
| | 57 | 347 | | if (!TryGetCompletedNormalAccumulatorRatio( |
| | 57 | 348 | | normalConstraint, |
| | 57 | 349 | | normalNumerator, |
| | 57 | 350 | | normalDenominator)) |
| | 1 | 351 | | return false; |
| | | 352 | | |
| | 56 | 353 | | ExactContactImpulseState? unused = null; |
| | 56 | 354 | | return TryGetCoulombDiskResponseCore( |
| | 56 | 355 | | normalNumerator, |
| | 56 | 356 | | normalDenominator, |
| | 56 | 357 | | primaryFirst, |
| | 56 | 358 | | primarySecond, |
| | 56 | 359 | | primaryTangent, |
| | 56 | 360 | | accumulatedPrimaryTangentImpulse, |
| | 56 | 361 | | secondaryFirst, |
| | 56 | 362 | | secondarySecond, |
| | 56 | 363 | | secondaryTangent, |
| | 56 | 364 | | accumulatedSecondaryTangentImpulse, |
| | 56 | 365 | | staticFriction, |
| | 56 | 366 | | dynamicFriction, |
| | 56 | 367 | | Fixed64.Zero, |
| | 56 | 368 | | out response, ref unused, retainOversized: false); |
| | | 369 | | } |
| | | 370 | | |
| | | 371 | | private static bool TryGetCoulombDiskResponseCore( |
| | | 372 | | ReadOnlySpan<ulong> normalNumerator, |
| | | 373 | | ReadOnlySpan<ulong> normalDenominator, |
| | | 374 | | in ExactContactResponseOperand3D primaryFirst, |
| | | 375 | | in ExactContactResponseOperand3D primarySecond, |
| | | 376 | | Vector3d primaryTangent, |
| | | 377 | | Fixed64 accumulatedPrimaryTangentImpulse, |
| | | 378 | | in ExactContactResponseOperand3D secondaryFirst, |
| | | 379 | | in ExactContactResponseOperand3D secondarySecond, |
| | | 380 | | Vector3d secondaryTangent, |
| | | 381 | | Fixed64 accumulatedSecondaryTangentImpulse, |
| | | 382 | | Fixed64 staticFriction, |
| | | 383 | | Fixed64 dynamicFriction, |
| | | 384 | | Fixed64 velocityDeadzone, |
| | | 385 | | out ExactCoulombResponse3D response, |
| | | 386 | | ref ExactContactImpulseState? exactImpulse, |
| | | 387 | | bool retainOversized) |
| | | 388 | | { |
| | 1070 | 389 | | response = default; |
| | 1070 | 390 | | bool wideTangents = exactImpulse?.HasTangents == true; |
| | 1070 | 391 | | int desiredWords = wideTangents ? MaxCoulombWords : MaxResponseWords; |
| | 1070 | 392 | | Span<ulong> primaryNumerator = stackalloc ulong[MaxResponseWords]; |
| | 1070 | 393 | | Span<ulong> primaryDenominator = stackalloc ulong[MaxResponseWords]; |
| | 1070 | 394 | | Span<ulong> secondaryNumerator = stackalloc ulong[MaxResponseWords]; |
| | 1070 | 395 | | Span<ulong> secondaryDenominator = stackalloc ulong[MaxResponseWords]; |
| | 1070 | 396 | | GetBilateralImpulseRatio( |
| | 1070 | 397 | | primaryFirst, |
| | 1070 | 398 | | primarySecond, |
| | 1070 | 399 | | primaryTangent, |
| | 1070 | 400 | | velocityDeadzone, |
| | 1070 | 401 | | primaryNumerator, |
| | 1070 | 402 | | primaryDenominator, |
| | 1070 | 403 | | out int primarySign); |
| | 1070 | 404 | | GetBilateralImpulseRatio( |
| | 1070 | 405 | | secondaryFirst, |
| | 1070 | 406 | | secondarySecond, |
| | 1070 | 407 | | secondaryTangent, |
| | 1070 | 408 | | velocityDeadzone, |
| | 1070 | 409 | | secondaryNumerator, |
| | 1070 | 410 | | secondaryDenominator, |
| | 1070 | 411 | | out int secondarySign); |
| | | 412 | | |
| | 1070 | 413 | | Span<ulong> desiredPrimaryNumerator = |
| | 1070 | 414 | | stackalloc ulong[desiredWords]; |
| | 1070 | 415 | | Span<ulong> desiredSecondaryNumerator = |
| | 1070 | 416 | | stackalloc ulong[desiredWords]; |
| | 1070 | 417 | | AddCachedTangentToRatio( |
| | 1070 | 418 | | primaryNumerator, |
| | 1070 | 419 | | primaryDenominator, |
| | 1070 | 420 | | primarySign, |
| | 1070 | 421 | | accumulatedPrimaryTangentImpulse, exactImpulse, primary: true, |
| | 1070 | 422 | | desiredPrimaryNumerator, |
| | 1070 | 423 | | out int desiredPrimarySign); |
| | 1070 | 424 | | AddCachedTangentToRatio( |
| | 1070 | 425 | | secondaryNumerator, |
| | 1070 | 426 | | secondaryDenominator, |
| | 1070 | 427 | | secondarySign, |
| | 1070 | 428 | | accumulatedSecondaryTangentImpulse, exactImpulse, primary: false, |
| | 1070 | 429 | | desiredSecondaryNumerator, |
| | 1070 | 430 | | out int desiredSecondarySign); |
| | | 431 | | |
| | 1070 | 432 | | Span<ulong> commonDenominator = stackalloc ulong[MaxCoulombWords]; |
| | 1070 | 433 | | Span<ulong> primaryAtCommon = stackalloc ulong[MaxCoulombWords]; |
| | 1070 | 434 | | Span<ulong> secondaryAtCommon = stackalloc ulong[MaxCoulombWords]; |
| | 1070 | 435 | | WideArithmetic.MultiplyMagnitudes( |
| | 1070 | 436 | | primaryDenominator, |
| | 1070 | 437 | | secondaryDenominator, |
| | 1070 | 438 | | commonDenominator); |
| | 1070 | 439 | | WideArithmetic.MultiplyMagnitudes( |
| | 1070 | 440 | | desiredPrimaryNumerator, |
| | 1070 | 441 | | secondaryDenominator, |
| | 1070 | 442 | | primaryAtCommon); |
| | 1070 | 443 | | WideArithmetic.MultiplyMagnitudes( |
| | 1070 | 444 | | desiredSecondaryNumerator, |
| | 1070 | 445 | | primaryDenominator, |
| | 1070 | 446 | | secondaryAtCommon); |
| | | 447 | | |
| | 1070 | 448 | | Span<ulong> magnitudeSquared = stackalloc ulong[MaxCoulombSquareWords]; |
| | 1070 | 449 | | Span<ulong> square = stackalloc ulong[MaxCoulombSquareWords]; |
| | 1070 | 450 | | WideArithmetic.MultiplyMagnitudes( |
| | 1070 | 451 | | primaryAtCommon, |
| | 1070 | 452 | | primaryAtCommon, |
| | 1070 | 453 | | magnitudeSquared); |
| | 1070 | 454 | | WideArithmetic.MultiplyMagnitudes( |
| | 1070 | 455 | | secondaryAtCommon, |
| | 1070 | 456 | | secondaryAtCommon, |
| | 1070 | 457 | | square); |
| | 1070 | 458 | | WideArithmetic.AddMagnitudeInto(square, magnitudeSquared); |
| | | 459 | | |
| | 1070 | 460 | | Span<ulong> staticNumerator = stackalloc ulong[MaxResponseWords]; |
| | 1070 | 461 | | Span<ulong> staticDenominator = stackalloc ulong[MaxResponseWords]; |
| | 1070 | 462 | | GetFrictionLimit( |
| | 1070 | 463 | | normalNumerator, |
| | 1070 | 464 | | normalDenominator, |
| | 1070 | 465 | | staticFriction, |
| | 1070 | 466 | | staticNumerator, |
| | 1070 | 467 | | staticDenominator); |
| | 1070 | 468 | | bool withinStaticLimit = IsVectorWithinLimit( |
| | 1070 | 469 | | magnitudeSquared, |
| | 1070 | 470 | | commonDenominator, |
| | 1070 | 471 | | staticNumerator, |
| | 1070 | 472 | | staticDenominator); |
| | | 473 | | |
| | 1070 | 474 | | Span<ulong> dynamicNumerator = stackalloc ulong[MaxResponseWords]; |
| | 1070 | 475 | | Span<ulong> dynamicDenominator = stackalloc ulong[MaxResponseWords]; |
| | 1070 | 476 | | if (!withinStaticLimit) |
| | | 477 | | { |
| | 87 | 478 | | GetFrictionLimit( |
| | 87 | 479 | | normalNumerator, |
| | 87 | 480 | | normalDenominator, |
| | 87 | 481 | | dynamicFriction, |
| | 87 | 482 | | dynamicNumerator, |
| | 87 | 483 | | dynamicDenominator); |
| | | 484 | | } |
| | | 485 | | |
| | 1070 | 486 | | bool useDynamicProjection = |
| | 1070 | 487 | | !withinStaticLimit |
| | 1070 | 488 | | && !WideArithmetic.IsZeroMagnitude(magnitudeSquared) |
| | 1070 | 489 | | && !WideArithmetic.IsZeroMagnitude(dynamicNumerator); |
| | | 490 | | bool hasAppliedImpulse; |
| | | 491 | | bool resolved; |
| | | 492 | | Vector3d firstLinear; |
| | | 493 | | Vector3d firstAngular; |
| | | 494 | | Vector3d secondLinear; |
| | | 495 | | Vector3d secondAngular; |
| | 1070 | 496 | | if (useDynamicProjection) |
| | | 497 | | { |
| | 56 | 498 | | hasAppliedImpulse = |
| | 56 | 499 | | !IsRadialProjectionEqualToFixed( |
| | 56 | 500 | | primaryAtCommon, |
| | 56 | 501 | | desiredPrimarySign, |
| | 56 | 502 | | dynamicNumerator, |
| | 56 | 503 | | dynamicDenominator, |
| | 56 | 504 | | magnitudeSquared, |
| | 56 | 505 | | accumulatedPrimaryTangentImpulse, exactImpulse, primary: true) |
| | 56 | 506 | | || !IsRadialProjectionEqualToFixed( |
| | 56 | 507 | | secondaryAtCommon, |
| | 56 | 508 | | desiredSecondarySign, |
| | 56 | 509 | | dynamicNumerator, |
| | 56 | 510 | | dynamicDenominator, |
| | 56 | 511 | | magnitudeSquared, |
| | 56 | 512 | | accumulatedSecondaryTangentImpulse, exactImpulse, primary: false); |
| | 56 | 513 | | resolved = TryGetDiskVelocityDeltas( |
| | 56 | 514 | | primaryFirst, |
| | 56 | 515 | | primarySecond, |
| | 56 | 516 | | secondaryFirst, |
| | 56 | 517 | | secondarySecond, |
| | 56 | 518 | | primaryTangent, |
| | 56 | 519 | | secondaryTangent, |
| | 56 | 520 | | primaryAtCommon, |
| | 56 | 521 | | desiredPrimarySign, |
| | 56 | 522 | | secondaryAtCommon, |
| | 56 | 523 | | desiredSecondarySign, |
| | 56 | 524 | | commonDenominator, |
| | 56 | 525 | | rational: false, |
| | 56 | 526 | | dynamicNumerator, |
| | 56 | 527 | | dynamicDenominator, |
| | 56 | 528 | | magnitudeSquared, |
| | 56 | 529 | | accumulatedPrimaryTangentImpulse, |
| | 56 | 530 | | accumulatedSecondaryTangentImpulse, |
| | 56 | 531 | | out firstLinear, |
| | 56 | 532 | | out firstAngular, |
| | 56 | 533 | | out secondLinear, |
| | 56 | 534 | | out secondAngular, exactImpulse); |
| | | 535 | | } |
| | | 536 | | else |
| | | 537 | | { |
| | 1014 | 538 | | Span<ulong> zero = stackalloc ulong[MaxCoulombWords]; |
| | 1014 | 539 | | Span<ulong> appliedPrimaryNumerator = |
| | 1014 | 540 | | stackalloc ulong[MaxCoulombWords]; |
| | 1014 | 541 | | Span<ulong> appliedDenominator = |
| | 1014 | 542 | | stackalloc ulong[MaxCoulombWords]; |
| | 1014 | 543 | | Span<ulong> appliedSecondaryNumerator = |
| | 1014 | 544 | | stackalloc ulong[MaxCoulombWords]; |
| | 1014 | 545 | | zero.Clear(); |
| | 1014 | 546 | | SubtractCachedTangentFromRatio( |
| | 1014 | 547 | | withinStaticLimit ? primaryAtCommon : zero, |
| | 1014 | 548 | | commonDenominator, |
| | 1014 | 549 | | withinStaticLimit ? desiredPrimarySign : 0, |
| | 1014 | 550 | | accumulatedPrimaryTangentImpulse, exactImpulse, primary: true, |
| | 1014 | 551 | | appliedPrimaryNumerator, |
| | 1014 | 552 | | appliedDenominator, |
| | 1014 | 553 | | out int appliedPrimarySign); |
| | 1014 | 554 | | SubtractCachedTangentFromRatio( |
| | 1014 | 555 | | withinStaticLimit ? secondaryAtCommon : zero, |
| | 1014 | 556 | | commonDenominator, |
| | 1014 | 557 | | withinStaticLimit ? desiredSecondarySign : 0, |
| | 1014 | 558 | | accumulatedSecondaryTangentImpulse, exactImpulse, primary: false, |
| | 1014 | 559 | | appliedSecondaryNumerator, |
| | 1014 | 560 | | appliedDenominator, |
| | 1014 | 561 | | out int appliedSecondarySign); |
| | 1014 | 562 | | hasAppliedImpulse = |
| | 1014 | 563 | | appliedPrimarySign != 0 |
| | 1014 | 564 | | || appliedSecondarySign != 0; |
| | 1014 | 565 | | resolved = TryGetDiskVelocityDeltas( |
| | 1014 | 566 | | primaryFirst, |
| | 1014 | 567 | | primarySecond, |
| | 1014 | 568 | | secondaryFirst, |
| | 1014 | 569 | | secondarySecond, |
| | 1014 | 570 | | primaryTangent, |
| | 1014 | 571 | | secondaryTangent, |
| | 1014 | 572 | | appliedPrimaryNumerator, |
| | 1014 | 573 | | appliedPrimarySign, |
| | 1014 | 574 | | appliedSecondaryNumerator, |
| | 1014 | 575 | | appliedSecondarySign, |
| | 1014 | 576 | | appliedDenominator, |
| | 1014 | 577 | | rational: true, |
| | 1014 | 578 | | dynamicNumerator, |
| | 1014 | 579 | | dynamicDenominator, |
| | 1014 | 580 | | magnitudeSquared, |
| | 1014 | 581 | | Fixed64.Zero, |
| | 1014 | 582 | | Fixed64.Zero, |
| | 1014 | 583 | | out firstLinear, |
| | 1014 | 584 | | out firstAngular, |
| | 1014 | 585 | | out secondLinear, |
| | 1014 | 586 | | out secondAngular, exactImpulse); |
| | | 587 | | } |
| | 1070 | 588 | | resolved &= primaryFirst.CanRepresentAngularDelta(firstAngular); |
| | 1070 | 589 | | resolved &= primarySecond.CanRepresentAngularDelta(secondAngular); |
| | 1070 | 590 | | if (!resolved) |
| | 8 | 591 | | return false; |
| | | 592 | | |
| | | 593 | | bool hasPrimaryProjection; |
| | | 594 | | bool hasSecondaryProjection; |
| | | 595 | | Fixed64 primaryProjection; |
| | | 596 | | Fixed64 secondaryProjection; |
| | 1062 | 597 | | if (withinStaticLimit) |
| | | 598 | | { |
| | 980 | 599 | | hasPrimaryProjection = Fixed64.TryGetSignedRawRatio( |
| | 980 | 600 | | primaryAtCommon, |
| | 980 | 601 | | commonDenominator, |
| | 980 | 602 | | desiredPrimarySign < 0, |
| | 980 | 603 | | out primaryProjection); |
| | 980 | 604 | | hasSecondaryProjection = Fixed64.TryGetSignedRawRatio( |
| | 980 | 605 | | secondaryAtCommon, |
| | 980 | 606 | | commonDenominator, |
| | 980 | 607 | | desiredSecondarySign < 0, |
| | 980 | 608 | | out secondaryProjection); |
| | | 609 | | } |
| | 82 | 610 | | else if (useDynamicProjection) |
| | | 611 | | { |
| | 53 | 612 | | Span<ulong> projectedNumerator = |
| | 53 | 613 | | stackalloc ulong[MaxCoulombWords + MaxResponseWords]; |
| | 53 | 614 | | WideArithmetic.MultiplyMagnitudes( |
| | 53 | 615 | | primaryAtCommon, |
| | 53 | 616 | | dynamicNumerator, |
| | 53 | 617 | | projectedNumerator); |
| | 53 | 618 | | hasPrimaryProjection = TryGetSignedRatioOverSquareRoot( |
| | 53 | 619 | | projectedNumerator, |
| | 53 | 620 | | dynamicDenominator, |
| | 53 | 621 | | magnitudeSquared, |
| | 53 | 622 | | desiredPrimarySign < 0, |
| | 53 | 623 | | out primaryProjection); |
| | 53 | 624 | | WideArithmetic.MultiplyMagnitudes( |
| | 53 | 625 | | secondaryAtCommon, |
| | 53 | 626 | | dynamicNumerator, |
| | 53 | 627 | | projectedNumerator); |
| | 53 | 628 | | hasSecondaryProjection = TryGetSignedRatioOverSquareRoot( |
| | 53 | 629 | | projectedNumerator, |
| | 53 | 630 | | dynamicDenominator, |
| | 53 | 631 | | magnitudeSquared, |
| | 53 | 632 | | desiredSecondarySign < 0, |
| | 53 | 633 | | out secondaryProjection); |
| | | 634 | | } |
| | | 635 | | else |
| | | 636 | | { |
| | 29 | 637 | | hasPrimaryProjection = true; |
| | 29 | 638 | | hasSecondaryProjection = true; |
| | 29 | 639 | | primaryProjection = Fixed64.Zero; |
| | 29 | 640 | | secondaryProjection = Fixed64.Zero; |
| | | 641 | | } |
| | | 642 | | |
| | 1062 | 643 | | if (retainOversized |
| | 1062 | 644 | | && (!hasPrimaryProjection || !hasSecondaryProjection || wideTangents)) |
| | | 645 | | { |
| | 29 | 646 | | if (!Vector3d.TryAdd(primaryFirst.LinearVelocity, firstLinear, out _) |
| | 29 | 647 | | || !primaryFirst.CanApplyAngularDelta(firstAngular) |
| | 29 | 648 | | || !Vector3d.TryAdd(primarySecond.LinearVelocity, secondLinear, out _) |
| | 34 | 649 | | || !primarySecond.CanApplyAngularDelta(secondAngular)) return false; |
| | 24 | 650 | | Span<ulong> widePrimary = stackalloc ulong[MaxResponseWords]; |
| | 24 | 651 | | Span<ulong> wideSecondary = stackalloc ulong[MaxResponseWords]; |
| | 24 | 652 | | if (withinStaticLimit) |
| | | 653 | | { |
| | 12 | 654 | | RoundCachedRatio(primaryAtCommon, commonDenominator, widePrimary); |
| | 12 | 655 | | RoundCachedRatio(secondaryAtCommon, commonDenominator, wideSecondary); |
| | | 656 | | } |
| | 12 | 657 | | else if (useDynamicProjection) |
| | | 658 | | { |
| | 8 | 659 | | Span<ulong> projected = stackalloc ulong[MaxCoulombWords + MaxResponseWords]; |
| | 8 | 660 | | WideArithmetic.MultiplyMagnitudes(primaryAtCommon, dynamicNumerator, projected); |
| | 8 | 661 | | RoundCachedRadialRatio(projected, dynamicDenominator, magnitudeSquared, widePrimary); |
| | 8 | 662 | | WideArithmetic.MultiplyMagnitudes(secondaryAtCommon, dynamicNumerator, projected); |
| | 8 | 663 | | RoundCachedRadialRatio(projected, dynamicDenominator, magnitudeSquared, wideSecondary); |
| | | 664 | | } |
| | | 665 | | else |
| | | 666 | | { |
| | 4 | 667 | | widePrimary.Clear(); |
| | 4 | 668 | | wideSecondary.Clear(); |
| | | 669 | | } |
| | 24 | 670 | | (exactImpulse ??= new ExactContactImpulseState()).SetTangents( |
| | 24 | 671 | | widePrimary, desiredPrimarySign, wideSecondary, desiredSecondarySign); |
| | | 672 | | } |
| | | 673 | | |
| | 1057 | 674 | | response = new ExactCoulombResponse3D( |
| | 1057 | 675 | | hasAppliedImpulse, |
| | 1057 | 676 | | firstLinear, |
| | 1057 | 677 | | firstAngular, |
| | 1057 | 678 | | secondLinear, |
| | 1057 | 679 | | secondAngular, |
| | 1057 | 680 | | hasPrimaryProjection, |
| | 1057 | 681 | | primaryProjection, |
| | 1057 | 682 | | hasSecondaryProjection, |
| | 1057 | 683 | | secondaryProjection); |
| | 1057 | 684 | | return true; |
| | | 685 | | } |
| | | 686 | | |
| | | 687 | | private static bool AreCoulombDiskInputsValid( |
| | | 688 | | Vector3d normal, |
| | | 689 | | in ExactContactResponseOperand3D primaryFirst, |
| | | 690 | | in ExactContactResponseOperand3D primarySecond, |
| | | 691 | | Vector3d primaryTangent, |
| | | 692 | | in ExactContactResponseOperand3D secondaryFirst, |
| | | 693 | | in ExactContactResponseOperand3D secondarySecond, |
| | | 694 | | Vector3d secondaryTangent, |
| | | 695 | | Fixed64 staticFriction, |
| | | 696 | | Fixed64 dynamicFriction) => |
| | 1075 | 697 | | staticFriction >= Fixed64.Zero |
| | 1075 | 698 | | & dynamicFriction >= Fixed64.Zero |
| | 1075 | 699 | | & primaryFirst.Lever.Denominator.Sign != 0 |
| | 1075 | 700 | | & primarySecond.Lever.Denominator.Sign != 0 |
| | 1075 | 701 | | & primaryFirst.InverseMass >= Fixed64.Zero |
| | 1075 | 702 | | & primarySecond.InverseMass >= Fixed64.Zero |
| | 1075 | 703 | | & normal.IsNormalized() |
| | 1075 | 704 | | & primaryTangent.IsNormalized() |
| | 1075 | 705 | | & secondaryTangent.IsNormalized() |
| | 1075 | 706 | | & FixedMath.Abs(Vector3d.Dot( |
| | 1075 | 707 | | primaryTangent, |
| | 1075 | 708 | | secondaryTangent)) <= Fixed64.Epsilon |
| | 1075 | 709 | | & FixedMath.Abs(Vector3d.Dot( |
| | 1075 | 710 | | normal, |
| | 1075 | 711 | | primaryTangent)) <= Fixed64.Epsilon |
| | 1075 | 712 | | & FixedMath.Abs(Vector3d.Dot( |
| | 1075 | 713 | | normal, |
| | 1075 | 714 | | secondaryTangent)) <= Fixed64.Epsilon |
| | 1075 | 715 | | & HaveMatchingParticipants(primaryFirst, secondaryFirst) |
| | 1075 | 716 | | & HaveMatchingParticipants(primarySecond, secondarySecond); |
| | | 717 | | |
| | | 718 | | private static bool TryGetCompletedNormalAccumulatorRatio( |
| | | 719 | | in ExactNormalConstraint3D constraint, |
| | | 720 | | Span<ulong> numerator, |
| | | 721 | | Span<ulong> denominator, |
| | | 722 | | in ContactResponseSnapshot impact = default) |
| | | 723 | | { |
| | 145 | 724 | | numerator.Clear(); |
| | 145 | 725 | | denominator.Clear(); |
| | 145 | 726 | | ExactContactResponseOperand3D first = constraint.First; |
| | 145 | 727 | | ExactContactResponseOperand3D second = constraint.Second; |
| | 145 | 728 | | bool inputsValid = |
| | 145 | 729 | | first.Lever.Denominator.Sign != 0 |
| | 145 | 730 | | & second.Lever.Denominator.Sign != 0 |
| | 145 | 731 | | & constraint.Restitution >= Fixed64.Zero |
| | 145 | 732 | | & constraint.RestitutionVelocityThreshold >= Fixed64.Zero |
| | 145 | 733 | | & constraint.AccumulatedImpulse >= Fixed64.Zero |
| | 145 | 734 | | & constraint.PositiveImpulseScale >= Fixed64.Zero |
| | 145 | 735 | | & constraint.NegativeImpulseScale >= Fixed64.Zero |
| | 145 | 736 | | & first.InverseMass >= Fixed64.Zero |
| | 145 | 737 | | & second.InverseMass >= Fixed64.Zero |
| | 145 | 738 | | & constraint.Normal.IsNormalized(); |
| | 145 | 739 | | if (!inputsValid) |
| | 2 | 740 | | return false; |
| | | 741 | | |
| | 143 | 742 | | ExactLever3D.GetRelativePointVelocityRatio( |
| | 143 | 743 | | first.LinearVelocity, |
| | 143 | 744 | | first.AngularVelocity, |
| | 143 | 745 | | first.Lever, |
| | 143 | 746 | | second.LinearVelocity, |
| | 143 | 747 | | second.AngularVelocity, |
| | 143 | 748 | | second.Lever, |
| | 143 | 749 | | constraint.Normal, |
| | 143 | 750 | | out Signed832 velocityNumerator, |
| | 143 | 751 | | out Signed832 velocityDenominator, first.PlanarAngularVelocity, second.PlanarAngularVelocity); |
| | | 752 | | |
| | 143 | 753 | | Span<ulong> effectiveNumerator = stackalloc ulong[MaxResponseWords]; |
| | 143 | 754 | | Span<ulong> effectiveDenominator = stackalloc ulong[MaxResponseWords]; |
| | 143 | 755 | | GetEffectiveMassRatio( |
| | 143 | 756 | | first, |
| | 143 | 757 | | second, |
| | 143 | 758 | | constraint.Normal, |
| | 143 | 759 | | effectiveNumerator, |
| | 143 | 760 | | effectiveDenominator); |
| | 143 | 761 | | int velocitySign = |
| | 143 | 762 | | velocityNumerator.Sign * velocityDenominator.Sign; |
| | 143 | 763 | | if (velocitySign == 0 && !impact.IsValid |
| | 143 | 764 | | || WideArithmetic.IsZeroMagnitude(effectiveNumerator)) |
| | | 765 | | { |
| | 38 | 766 | | SetMagnitude(constraint.AccumulatedImpulse, numerator); |
| | 38 | 767 | | denominator[0] = 1UL; |
| | 38 | 768 | | return true; |
| | | 769 | | } |
| | | 770 | | |
| | 105 | 771 | | bool hasVelocityProjection = Fixed64.TryGetSignedRawRatio( |
| | 105 | 772 | | velocityNumerator, |
| | 105 | 773 | | velocityDenominator, |
| | 105 | 774 | | 0, |
| | 105 | 775 | | out Fixed64 velocityProjection); |
| | 105 | 776 | | bool applyRestitution = velocitySign < 0 |
| | 105 | 777 | | & (!hasVelocityProjection |
| | 105 | 778 | | | velocityProjection |
| | 105 | 779 | | < -constraint.RestitutionVelocityThreshold); |
| | 105 | 780 | | Fixed64 scale = velocitySign < 0 |
| | 105 | 781 | | ? constraint.PositiveImpulseScale |
| | 105 | 782 | | : constraint.NegativeImpulseScale; |
| | 105 | 783 | | Span<ulong> impulseNumerator = stackalloc ulong[MaxResponseWords]; |
| | 105 | 784 | | Span<ulong> impulseDenominator = stackalloc ulong[MaxResponseWords]; |
| | | 785 | | int impulseSign; |
| | 105 | 786 | | if (impact.IsValid) |
| | | 787 | | { |
| | 4 | 788 | | impulseSign = BuildImpactImpulseRatio(first, second, constraint.Normal, impact, |
| | 4 | 789 | | constraint.Restitution, constraint.RestitutionVelocityThreshold, |
| | 4 | 790 | | velocityNumerator, velocityDenominator, effectiveNumerator, effectiveDenominator, |
| | 4 | 791 | | impulseNumerator, impulseDenominator); |
| | | 792 | | } |
| | | 793 | | else |
| | | 794 | | { |
| | 101 | 795 | | BuildImpulseRatio( |
| | 101 | 796 | | velocityNumerator, |
| | 101 | 797 | | velocityDenominator, |
| | 101 | 798 | | effectiveNumerator, |
| | 101 | 799 | | effectiveDenominator, |
| | 101 | 800 | | applyRestitution ? constraint.Restitution : Fixed64.Zero, |
| | 101 | 801 | | scale, |
| | 101 | 802 | | impulseNumerator, |
| | 101 | 803 | | impulseDenominator); |
| | 101 | 804 | | impulseSign = -velocitySign; |
| | | 805 | | } |
| | | 806 | | |
| | 105 | 807 | | Span<ulong> accumulatorMagnitude = |
| | 105 | 808 | | stackalloc ulong[MaxResponseWords]; |
| | 105 | 809 | | Span<ulong> accumulatorAtImpulseDenominator = |
| | 105 | 810 | | stackalloc ulong[MaxResponseWords]; |
| | 105 | 811 | | SetMagnitude(constraint.AccumulatedImpulse, accumulatorMagnitude); |
| | 105 | 812 | | WideArithmetic.MultiplyMagnitudes( |
| | 105 | 813 | | accumulatorMagnitude, |
| | 105 | 814 | | impulseDenominator, |
| | 105 | 815 | | accumulatorAtImpulseDenominator); |
| | 105 | 816 | | if (impulseSign < 0 |
| | 105 | 817 | | && WideArithmetic.CompareMagnitudeEqualLength( |
| | 105 | 818 | | impulseNumerator, |
| | 105 | 819 | | accumulatorAtImpulseDenominator) >= 0) |
| | | 820 | | { |
| | 1 | 821 | | denominator[0] = 1UL; |
| | 1 | 822 | | return true; |
| | | 823 | | } |
| | | 824 | | |
| | 104 | 825 | | accumulatorAtImpulseDenominator.CopyTo(numerator); |
| | 104 | 826 | | if (impulseSign > 0) |
| | 102 | 827 | | WideArithmetic.AddMagnitudeInto(impulseNumerator, numerator); |
| | | 828 | | else |
| | 2 | 829 | | WideArithmetic.SubtractEqualMagnitudes( |
| | 2 | 830 | | accumulatorAtImpulseDenominator, |
| | 2 | 831 | | impulseNumerator, |
| | 2 | 832 | | numerator); |
| | 104 | 833 | | impulseDenominator.CopyTo(denominator); |
| | 104 | 834 | | return true; |
| | | 835 | | } |
| | | 836 | | |
| | | 837 | | private static void GetBilateralImpulseRatio( |
| | | 838 | | in ExactContactResponseOperand3D first, |
| | | 839 | | in ExactContactResponseOperand3D second, |
| | | 840 | | Vector3d tangent, |
| | | 841 | | Fixed64 velocityDeadzone, |
| | | 842 | | Span<ulong> numerator, |
| | | 843 | | Span<ulong> denominator, |
| | | 844 | | out int sign) |
| | | 845 | | { |
| | 2227 | 846 | | numerator.Clear(); |
| | 2227 | 847 | | denominator.Clear(); |
| | 2227 | 848 | | sign = 0; |
| | 2227 | 849 | | ExactLever3D.GetRelativePointVelocityRatio( |
| | 2227 | 850 | | first.LinearVelocity, |
| | 2227 | 851 | | first.AngularVelocity, |
| | 2227 | 852 | | first.Lever, |
| | 2227 | 853 | | second.LinearVelocity, |
| | 2227 | 854 | | second.AngularVelocity, |
| | 2227 | 855 | | second.Lever, |
| | 2227 | 856 | | tangent, |
| | 2227 | 857 | | out Signed832 velocityNumerator, |
| | 2227 | 858 | | out Signed832 velocityDenominator, first.PlanarAngularVelocity, second.PlanarAngularVelocity); |
| | | 859 | | |
| | 2227 | 860 | | Span<ulong> effectiveNumerator = stackalloc ulong[MaxResponseWords]; |
| | 2227 | 861 | | Span<ulong> effectiveDenominator = stackalloc ulong[MaxResponseWords]; |
| | 2227 | 862 | | GetEffectiveMassRatio( |
| | 2227 | 863 | | first, |
| | 2227 | 864 | | second, |
| | 2227 | 865 | | tangent, |
| | 2227 | 866 | | effectiveNumerator, |
| | 2227 | 867 | | effectiveDenominator); |
| | 2227 | 868 | | int velocitySign = |
| | 2227 | 869 | | velocityNumerator.Sign * velocityDenominator.Sign; |
| | 2227 | 870 | | bool suppressVelocity = velocityDeadzone != Fixed64.Zero |
| | 2227 | 871 | | && Fixed64.TryGetSignedRawRatio( |
| | 2227 | 872 | | velocityNumerator, |
| | 2227 | 873 | | velocityDenominator, |
| | 2227 | 874 | | 0, |
| | 2227 | 875 | | out Fixed64 velocityProjection) |
| | 2227 | 876 | | && velocityProjection >= -velocityDeadzone |
| | 2227 | 877 | | && velocityProjection <= velocityDeadzone; |
| | 2227 | 878 | | if (velocitySign == 0 |
| | 2227 | 879 | | || suppressVelocity |
| | 2227 | 880 | | || WideArithmetic.IsZeroMagnitude(effectiveNumerator)) |
| | | 881 | | { |
| | 1283 | 882 | | denominator[0] = 1UL; |
| | 1283 | 883 | | return; |
| | | 884 | | } |
| | | 885 | | |
| | 944 | 886 | | BuildImpulseRatio( |
| | 944 | 887 | | velocityNumerator, |
| | 944 | 888 | | velocityDenominator, |
| | 944 | 889 | | effectiveNumerator, |
| | 944 | 890 | | effectiveDenominator, |
| | 944 | 891 | | Fixed64.Zero, |
| | 944 | 892 | | Fixed64.One, |
| | 944 | 893 | | numerator, |
| | 944 | 894 | | denominator); |
| | 944 | 895 | | sign = -velocitySign; |
| | 944 | 896 | | } |
| | | 897 | | |
| | | 898 | | private static void GetFrictionLimit( |
| | | 899 | | ReadOnlySpan<ulong> normalNumerator, |
| | | 900 | | ReadOnlySpan<ulong> normalDenominator, |
| | | 901 | | Fixed64 friction, |
| | | 902 | | Span<ulong> numerator, |
| | | 903 | | Span<ulong> denominator) |
| | | 904 | | { |
| | 1263 | 905 | | Span<ulong> frictionMagnitude = |
| | 1263 | 906 | | stackalloc ulong[3]; |
| | 1263 | 907 | | Span<ulong> fixedScale = |
| | 1263 | 908 | | stackalloc ulong[1]; |
| | 1263 | 909 | | SetMagnitude(friction, frictionMagnitude); |
| | 1263 | 910 | | fixedScale.Clear(); |
| | 1263 | 911 | | fixedScale[0] = (ulong)FixedMath.ONE_L; |
| | 1263 | 912 | | WideArithmetic.MultiplyMagnitudes( |
| | 1263 | 913 | | normalNumerator, |
| | 1263 | 914 | | frictionMagnitude, |
| | 1263 | 915 | | numerator); |
| | 1263 | 916 | | WideArithmetic.MultiplyMagnitudes( |
| | 1263 | 917 | | normalDenominator, |
| | 1263 | 918 | | fixedScale, |
| | 1263 | 919 | | denominator); |
| | 1263 | 920 | | } |
| | | 921 | | |
| | | 922 | | private static void AddFixedToRatio( |
| | | 923 | | ReadOnlySpan<ulong> ratioNumerator, |
| | | 924 | | ReadOnlySpan<ulong> ratioDenominator, |
| | | 925 | | int ratioSign, |
| | | 926 | | Fixed64 value, |
| | | 927 | | Span<ulong> resultNumerator, |
| | | 928 | | out int resultSign) |
| | | 929 | | { |
| | 2199 | 930 | | Span<ulong> valueMagnitude = stackalloc ulong[3]; |
| | 2199 | 931 | | Span<ulong> valueAtDenominator = |
| | 2199 | 932 | | stackalloc ulong[MaxResponseWords]; |
| | 2199 | 933 | | SetMagnitude(value, valueMagnitude); |
| | 2199 | 934 | | WideArithmetic.MultiplyMagnitudes( |
| | 2199 | 935 | | valueMagnitude, |
| | 2199 | 936 | | ratioDenominator, |
| | 2199 | 937 | | valueAtDenominator); |
| | 2199 | 938 | | AddSignedMagnitudes( |
| | 2199 | 939 | | ratioNumerator, |
| | 2199 | 940 | | ratioSign, |
| | 2199 | 941 | | valueAtDenominator, |
| | 2199 | 942 | | value == Fixed64.Zero ? 0 : value < Fixed64.Zero ? -1 : 1, |
| | 2199 | 943 | | resultNumerator, |
| | 2199 | 944 | | out resultSign); |
| | 2199 | 945 | | } |
| | | 946 | | |
| | | 947 | | private static void SubtractFixedFromRatio( |
| | | 948 | | ReadOnlySpan<ulong> ratioNumerator, |
| | | 949 | | ReadOnlySpan<ulong> ratioDenominator, |
| | | 950 | | int ratioSign, |
| | | 951 | | Fixed64 value, |
| | | 952 | | Span<ulong> resultNumerator, |
| | | 953 | | Span<ulong> resultDenominator, |
| | | 954 | | out int resultSign) |
| | | 955 | | { |
| | 2023 | 956 | | Span<ulong> valueMagnitude = stackalloc ulong[3]; |
| | 2023 | 957 | | Span<ulong> valueAtDenominator = |
| | 2023 | 958 | | stackalloc ulong[MaxCoulombWords]; |
| | 2023 | 959 | | SetMagnitude(value, valueMagnitude); |
| | 2023 | 960 | | WideArithmetic.MultiplyMagnitudes( |
| | 2023 | 961 | | valueMagnitude, |
| | 2023 | 962 | | ratioDenominator, |
| | 2023 | 963 | | valueAtDenominator); |
| | 2023 | 964 | | AddSignedMagnitudes( |
| | 2023 | 965 | | ratioNumerator, |
| | 2023 | 966 | | ratioSign, |
| | 2023 | 967 | | valueAtDenominator, |
| | 2023 | 968 | | value == Fixed64.Zero ? 0 : value < Fixed64.Zero ? 1 : -1, |
| | 2023 | 969 | | resultNumerator, |
| | 2023 | 970 | | out resultSign); |
| | 2023 | 971 | | resultDenominator.Clear(); |
| | 2023 | 972 | | ratioDenominator.CopyTo(resultDenominator); |
| | 2023 | 973 | | } |
| | | 974 | | |
| | | 975 | | private static int CompareRatios( |
| | | 976 | | ReadOnlySpan<ulong> firstNumerator, |
| | | 977 | | ReadOnlySpan<ulong> firstDenominator, |
| | | 978 | | ReadOnlySpan<ulong> secondNumerator, |
| | | 979 | | ReadOnlySpan<ulong> secondDenominator) |
| | | 980 | | { |
| | 87 | 981 | | Span<ulong> first = stackalloc ulong[MaxCoulombWords]; |
| | 87 | 982 | | Span<ulong> second = stackalloc ulong[MaxCoulombWords]; |
| | 87 | 983 | | WideArithmetic.MultiplyMagnitudes( |
| | 87 | 984 | | firstNumerator, |
| | 87 | 985 | | secondDenominator, |
| | 87 | 986 | | first); |
| | 87 | 987 | | WideArithmetic.MultiplyMagnitudes( |
| | 87 | 988 | | secondNumerator, |
| | 87 | 989 | | firstDenominator, |
| | 87 | 990 | | second); |
| | 87 | 991 | | return WideArithmetic.CompareMagnitudeEqualLength(first, second); |
| | | 992 | | } |
| | | 993 | | |
| | | 994 | | private static bool IsVectorWithinLimit( |
| | | 995 | | ReadOnlySpan<ulong> vectorMagnitudeSquared, |
| | | 996 | | ReadOnlySpan<ulong> vectorDenominator, |
| | | 997 | | ReadOnlySpan<ulong> limitNumerator, |
| | | 998 | | ReadOnlySpan<ulong> limitDenominator) |
| | | 999 | | { |
| | 1070 | 1000 | | Span<ulong> denominatorSquared = |
| | 1070 | 1001 | | stackalloc ulong[MaxCoulombSquareWords]; |
| | 1070 | 1002 | | Span<ulong> limitNumeratorSquared = |
| | 1070 | 1003 | | stackalloc ulong[MaxCoulombSquareWords]; |
| | 1070 | 1004 | | Span<ulong> limitDenominatorSquared = |
| | 1070 | 1005 | | stackalloc ulong[MaxCoulombSquareWords]; |
| | 1070 | 1006 | | Span<ulong> left = stackalloc ulong[MaxCoulombComparisonWords]; |
| | 1070 | 1007 | | Span<ulong> right = stackalloc ulong[MaxCoulombComparisonWords]; |
| | 1070 | 1008 | | WideArithmetic.MultiplyMagnitudes( |
| | 1070 | 1009 | | vectorDenominator, |
| | 1070 | 1010 | | vectorDenominator, |
| | 1070 | 1011 | | denominatorSquared); |
| | 1070 | 1012 | | WideArithmetic.MultiplyMagnitudes( |
| | 1070 | 1013 | | limitNumerator, |
| | 1070 | 1014 | | limitNumerator, |
| | 1070 | 1015 | | limitNumeratorSquared); |
| | 1070 | 1016 | | WideArithmetic.MultiplyMagnitudes( |
| | 1070 | 1017 | | limitDenominator, |
| | 1070 | 1018 | | limitDenominator, |
| | 1070 | 1019 | | limitDenominatorSquared); |
| | 1070 | 1020 | | WideArithmetic.MultiplyMagnitudes( |
| | 1070 | 1021 | | vectorMagnitudeSquared, |
| | 1070 | 1022 | | limitDenominatorSquared, |
| | 1070 | 1023 | | left); |
| | 1070 | 1024 | | WideArithmetic.MultiplyMagnitudes( |
| | 1070 | 1025 | | limitNumeratorSquared, |
| | 1070 | 1026 | | denominatorSquared, |
| | 1070 | 1027 | | right); |
| | 1070 | 1028 | | return WideArithmetic.CompareMagnitudeEqualLength(left, right) <= 0; |
| | | 1029 | | } |
| | | 1030 | | |
| | | 1031 | | private static void AddSignedMagnitudes( |
| | | 1032 | | ReadOnlySpan<ulong> first, |
| | | 1033 | | int firstSign, |
| | | 1034 | | ReadOnlySpan<ulong> second, |
| | | 1035 | | int secondSign, |
| | | 1036 | | Span<ulong> result, |
| | | 1037 | | out int resultSign) |
| | | 1038 | | { |
| | 33850 | 1039 | | result.Clear(); |
| | 33850 | 1040 | | resultSign = 0; |
| | 33850 | 1041 | | if (firstSign != 0 & !WideArithmetic.IsZeroMagnitude(first)) |
| | 17705 | 1042 | | WideArithmetic.AddShiftedSignedMagnitude( |
| | 17705 | 1043 | | first, |
| | 17705 | 1044 | | firstSign, |
| | 17705 | 1045 | | 0, |
| | 17705 | 1046 | | result, |
| | 17705 | 1047 | | ref resultSign); |
| | 33850 | 1048 | | if (secondSign != 0 & !WideArithmetic.IsZeroMagnitude(second)) |
| | 5090 | 1049 | | WideArithmetic.AddShiftedSignedMagnitude( |
| | 5090 | 1050 | | second, |
| | 5090 | 1051 | | secondSign, |
| | 5090 | 1052 | | 0, |
| | 5090 | 1053 | | result, |
| | 5090 | 1054 | | ref resultSign); |
| | 33850 | 1055 | | } |
| | | 1056 | | |
| | | 1057 | | private static bool TryGetSignedRatioOverSquareRoot( |
| | | 1058 | | ReadOnlySpan<ulong> numerator, |
| | | 1059 | | ReadOnlySpan<ulong> denominator, |
| | | 1060 | | ReadOnlySpan<ulong> radicand, |
| | | 1061 | | bool negative, |
| | | 1062 | | out Fixed64 result) |
| | | 1063 | | { |
| | 106 | 1064 | | if (WideArithmetic.IsZeroMagnitude(numerator)) |
| | | 1065 | | { |
| | 34 | 1066 | | result = Fixed64.Zero; |
| | 34 | 1067 | | return true; |
| | | 1068 | | } |
| | | 1069 | | |
| | 72 | 1070 | | ulong limit = negative ? 1UL << 63 : (ulong)long.MaxValue; |
| | 72 | 1071 | | if (CompareRatioOverSquareRoot( |
| | 72 | 1072 | | numerator, |
| | 72 | 1073 | | denominator, |
| | 72 | 1074 | | radicand, |
| | 72 | 1075 | | limit) > 0) |
| | | 1076 | | { |
| | 11 | 1077 | | result = default; |
| | 11 | 1078 | | return false; |
| | | 1079 | | } |
| | | 1080 | | |
| | 61 | 1081 | | ulong low = 0UL; |
| | 61 | 1082 | | ulong high = limit; |
| | 3907 | 1083 | | while (low < high) |
| | | 1084 | | { |
| | 3846 | 1085 | | ulong difference = high - low; |
| | 3846 | 1086 | | ulong middle = low + (difference >> 1) + (difference & 1UL); |
| | 3846 | 1087 | | if (CompareRatioOverSquareRoot( |
| | 3846 | 1088 | | numerator, |
| | 3846 | 1089 | | denominator, |
| | 3846 | 1090 | | radicand, |
| | 3846 | 1091 | | middle) >= 0) |
| | | 1092 | | { |
| | 1047 | 1093 | | low = middle; |
| | | 1094 | | } |
| | | 1095 | | else |
| | | 1096 | | { |
| | 2799 | 1097 | | high = middle - 1UL; |
| | | 1098 | | } |
| | | 1099 | | } |
| | | 1100 | | |
| | 61 | 1101 | | ulong quotient = low; |
| | 61 | 1102 | | Span<ulong> doubledQuotient = stackalloc ulong[2]; |
| | 61 | 1103 | | doubledQuotient[0] = (quotient << 1) | 1UL; |
| | 61 | 1104 | | doubledQuotient[1] = quotient >> 63; |
| | 61 | 1105 | | int midpointComparison = CompareRatioOverSquareRoot( |
| | 61 | 1106 | | numerator, |
| | 61 | 1107 | | denominator, |
| | 61 | 1108 | | radicand, |
| | 61 | 1109 | | doubledQuotient, |
| | 61 | 1110 | | numeratorMultiplier: 2UL); |
| | 61 | 1111 | | bool roundUp = midpointComparison > 0 |
| | 61 | 1112 | | | (midpointComparison == 0 & (quotient & 1UL) != 0UL); |
| | 61 | 1113 | | if (roundUp & quotient < limit) |
| | | 1114 | | { |
| | 19 | 1115 | | quotient++; |
| | | 1116 | | } |
| | | 1117 | | |
| | 61 | 1118 | | result = new Fixed64( |
| | 61 | 1119 | | negative ? unchecked(-(long)quotient) : (long)quotient); |
| | 61 | 1120 | | return true; |
| | | 1121 | | } |
| | | 1122 | | |
| | | 1123 | | private static int CompareRatioOverSquareRoot( |
| | | 1124 | | ReadOnlySpan<ulong> numerator, |
| | | 1125 | | ReadOnlySpan<ulong> denominator, |
| | | 1126 | | ReadOnlySpan<ulong> radicand, |
| | | 1127 | | ulong candidate) |
| | | 1128 | | { |
| | 3918 | 1129 | | Span<ulong> candidateMagnitude = stackalloc ulong[1]; |
| | 3918 | 1130 | | candidateMagnitude.Clear(); |
| | 3918 | 1131 | | candidateMagnitude[0] = candidate; |
| | 3918 | 1132 | | return CompareRatioOverSquareRoot( |
| | 3918 | 1133 | | numerator, |
| | 3918 | 1134 | | denominator, |
| | 3918 | 1135 | | radicand, |
| | 3918 | 1136 | | candidateMagnitude, |
| | 3918 | 1137 | | numeratorMultiplier: 1UL); |
| | | 1138 | | } |
| | | 1139 | | |
| | | 1140 | | private static int CompareRatioOverSquareRoot( |
| | | 1141 | | ReadOnlySpan<ulong> numerator, |
| | | 1142 | | ReadOnlySpan<ulong> denominator, |
| | | 1143 | | ReadOnlySpan<ulong> radicand, |
| | | 1144 | | ReadOnlySpan<ulong> candidate, |
| | | 1145 | | ulong numeratorMultiplier) |
| | | 1146 | | { |
| | 4651 | 1147 | | Span<ulong> multiplier = stackalloc ulong[1]; |
| | 4651 | 1148 | | multiplier.Clear(); |
| | 4651 | 1149 | | multiplier[0] = numeratorMultiplier; |
| | 4651 | 1150 | | Span<ulong> scaledNumerator = |
| | 4651 | 1151 | | stackalloc ulong[(MaxCoulombComparisonWords / 2) + 1]; |
| | 4651 | 1152 | | Span<ulong> left = stackalloc ulong[MaxCoulombComparisonWords]; |
| | 4651 | 1153 | | Span<ulong> denominatorTimesCandidate = |
| | 4651 | 1154 | | stackalloc ulong[(MaxCoulombComparisonWords / 2) + 1]; |
| | 4651 | 1155 | | Span<ulong> rightBase = |
| | 4651 | 1156 | | stackalloc ulong[MaxCoulombComparisonWords]; |
| | 4651 | 1157 | | Span<ulong> right = stackalloc ulong[MaxCoulombComparisonWords]; |
| | 4651 | 1158 | | WideArithmetic.MultiplyMagnitudes( |
| | 4651 | 1159 | | numerator, |
| | 4651 | 1160 | | multiplier, |
| | 4651 | 1161 | | scaledNumerator); |
| | 4651 | 1162 | | WideArithmetic.MultiplyMagnitudes( |
| | 4651 | 1163 | | scaledNumerator, |
| | 4651 | 1164 | | scaledNumerator, |
| | 4651 | 1165 | | left); |
| | 4651 | 1166 | | WideArithmetic.MultiplyMagnitudes( |
| | 4651 | 1167 | | denominator, |
| | 4651 | 1168 | | candidate, |
| | 4651 | 1169 | | denominatorTimesCandidate); |
| | 4651 | 1170 | | WideArithmetic.MultiplyMagnitudes( |
| | 4651 | 1171 | | denominatorTimesCandidate, |
| | 4651 | 1172 | | denominatorTimesCandidate, |
| | 4651 | 1173 | | rightBase); |
| | 4651 | 1174 | | WideArithmetic.MultiplyMagnitudes(rightBase, radicand, right); |
| | 4651 | 1175 | | return WideArithmetic.CompareMagnitudeEqualLength(left, right); |
| | | 1176 | | } |
| | | 1177 | | |
| | | 1178 | | private static bool HaveMatchingParticipants( |
| | | 1179 | | in ExactContactResponseOperand3D first, |
| | | 1180 | | in ExactContactResponseOperand3D second) => |
| | 2570 | 1181 | | first.Lever.XNumerator.Equals(second.Lever.XNumerator) |
| | 2570 | 1182 | | & first.Lever.YNumerator.Equals(second.Lever.YNumerator) |
| | 2570 | 1183 | | & first.Lever.ZNumerator.Equals(second.Lever.ZNumerator) |
| | 2570 | 1184 | | & first.Lever.Denominator.Equals(second.Lever.Denominator) |
| | 2570 | 1185 | | & first.InverseMass == second.InverseMass |
| | 2570 | 1186 | | & first.InverseInertia == second.InverseInertia; |
| | | 1187 | | } |