| | | 1 | | //======================================================================= |
| | | 2 | | // ContactNormalImpulseMixed.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 | | |
| | | 14 | | |
| | | 15 | | |
| | | 16 | | |
| | | 17 | | /// <summary> |
| | | 18 | | /// Calculates allocation-free mixed 3D/2D contact-point normal response |
| | | 19 | | /// without mutating either participant. |
| | | 20 | | /// </summary> |
| | | 21 | | internal static class ContactNormalImpulseMixed |
| | | 22 | | { |
| | | 23 | | internal static bool CanUseCompactResponse( |
| | | 24 | | SolidBody? body3D, |
| | | 25 | | Vector3d linearVelocity3D, |
| | | 26 | | Vector3d angularVelocity3D, |
| | | 27 | | Vector3d relativeContactPoint3D, |
| | | 28 | | SolidBody2D? body2D, |
| | | 29 | | Vector2d linearVelocity2D, |
| | | 30 | | Fixed64 angularVelocity2D, |
| | | 31 | | Vector2d relativeContactPoint2D, |
| | | 32 | | Vector3d axis) |
| | | 33 | | { |
| | 184429 | 34 | | Vector3d planarLever = |
| | 184429 | 35 | | ExactContactLever2D.ToSpatial(relativeContactPoint2D); |
| | 184429 | 36 | | return ContactResponseArithmetic3D.CanUseFastPointVelocity( |
| | 184429 | 37 | | linearVelocity3D, |
| | 184429 | 38 | | angularVelocity3D, |
| | 184429 | 39 | | relativeContactPoint3D, |
| | 184429 | 40 | | ExactContactLever2D.ToSpatial(linearVelocity2D), |
| | 184429 | 41 | | new Vector3d( |
| | 184429 | 42 | | Fixed64.Zero, |
| | 184429 | 43 | | -angularVelocity2D, |
| | 184429 | 44 | | Fixed64.Zero), |
| | 184429 | 45 | | planarLever, |
| | 184429 | 46 | | axis) |
| | 184429 | 47 | | && ContactResponseArithmetic3D.CanUseFastAngularResponse( |
| | 184429 | 48 | | relativeContactPoint3D, |
| | 184429 | 49 | | axis, |
| | 184429 | 50 | | body3D?.GetConstrainedInverseInertiaTensor() |
| | 184429 | 51 | | ?? Fixed3x3.Zero) |
| | 184429 | 52 | | && ContactResponseArithmetic3D.CanUseFastAngularResponse( |
| | 184429 | 53 | | planarLever, |
| | 184429 | 54 | | axis, |
| | 184429 | 55 | | ExactContactLever2D.CreateInverseInertia(body2D)); |
| | | 56 | | } |
| | | 57 | | |
| | | 58 | | internal static bool TryCalculateVelocityDeltas( |
| | | 59 | | SolidBody? body3D, |
| | | 60 | | Vector3d linearVelocity3D, |
| | | 61 | | Vector3d angularVelocity3D, |
| | | 62 | | Vector3d relativeContactPoint3D, |
| | | 63 | | SolidBody2D? body2D, |
| | | 64 | | Vector2d linearVelocity2D, |
| | | 65 | | Fixed64 angularVelocity2D, |
| | | 66 | | Vector2d relativeContactPoint2D, |
| | | 67 | | Vector3d normal, |
| | | 68 | | Fixed64 restitution, |
| | | 69 | | Fixed64 restitutionVelocityThreshold, |
| | | 70 | | out ContactNormalVelocityDeltaResultMixed result) |
| | | 71 | | { |
| | 26 | 72 | | result = default; |
| | 26 | 73 | | if (!TryComputeNormalVelocity( |
| | 26 | 74 | | linearVelocity3D, |
| | 26 | 75 | | angularVelocity3D, |
| | 26 | 76 | | relativeContactPoint3D, |
| | 26 | 77 | | linearVelocity2D, |
| | 26 | 78 | | angularVelocity2D, |
| | 26 | 79 | | relativeContactPoint2D, |
| | 26 | 80 | | normal, |
| | 26 | 81 | | out Fixed64 normalVelocity)) |
| | | 82 | | { |
| | 1 | 83 | | return false; |
| | | 84 | | } |
| | 25 | 85 | | if (normalVelocity >= Fixed64.Zero) |
| | | 86 | | { |
| | 1 | 87 | | result = ZeroVelocityDelta(normalVelocity); |
| | 1 | 88 | | return true; |
| | | 89 | | } |
| | | 90 | | |
| | 24 | 91 | | if (!TryComputeDenominator( |
| | 24 | 92 | | body3D, |
| | 24 | 93 | | relativeContactPoint3D, |
| | 24 | 94 | | body2D, |
| | 24 | 95 | | relativeContactPoint2D, |
| | 24 | 96 | | normal, |
| | 24 | 97 | | out Fixed64 denominator)) |
| | | 98 | | { |
| | 3 | 99 | | return false; |
| | | 100 | | } |
| | 21 | 101 | | if (denominator <= Fixed64.Zero) |
| | 1 | 102 | | return false; |
| | | 103 | | |
| | 20 | 104 | | Fixed64 appliedRestitution = normalVelocity < -restitutionVelocityThreshold |
| | 20 | 105 | | ? restitution |
| | 20 | 106 | | : Fixed64.Zero; |
| | 20 | 107 | | Fixed64 responseFactor = -(Fixed64.One + appliedRestitution); |
| | 20 | 108 | | Vector2d planarNormal = normal.ToVector2d(); |
| | 20 | 109 | | bool linear3DResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta( |
| | 20 | 110 | | body3D?.ProjectLinearMotion(-normal) ?? Vector3d.Zero, |
| | 20 | 111 | | normalVelocity, |
| | 20 | 112 | | responseFactor, |
| | 20 | 113 | | body3D?.EffectiveInverseMass ?? Fixed64.Zero, |
| | 20 | 114 | | denominator, |
| | 20 | 115 | | out Vector3d linearVelocityDelta3D); |
| | 20 | 116 | | bool angular3DResolved = TryResolveAngularVelocityDelta3D( |
| | 20 | 117 | | body3D, |
| | 20 | 118 | | relativeContactPoint3D, |
| | 20 | 119 | | -normal, |
| | 20 | 120 | | normalVelocity, |
| | 20 | 121 | | responseFactor, |
| | 20 | 122 | | denominator, |
| | 20 | 123 | | out Vector3d angularVelocityDelta3D); |
| | 20 | 124 | | bool linear2DResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta( |
| | 20 | 125 | | body2D?.ProjectLinearMotion(planarNormal) ?? Vector2d.Zero, |
| | 20 | 126 | | normalVelocity, |
| | 20 | 127 | | responseFactor, |
| | 20 | 128 | | body2D?.EffectiveInverseMass ?? Fixed64.Zero, |
| | 20 | 129 | | denominator, |
| | 20 | 130 | | out Vector2d linearVelocityDelta2D); |
| | 20 | 131 | | bool angular2DResolved = TryResolveAngularVelocityDelta2D( |
| | 20 | 132 | | body2D, |
| | 20 | 133 | | relativeContactPoint2D, |
| | 20 | 134 | | planarNormal, |
| | 20 | 135 | | normalVelocity, |
| | 20 | 136 | | responseFactor, |
| | 20 | 137 | | denominator, |
| | 20 | 138 | | out Fixed64 angularVelocityDelta2D); |
| | 20 | 139 | | if (!(linear3DResolved |
| | 20 | 140 | | & angular3DResolved |
| | 20 | 141 | | & linear2DResolved |
| | 20 | 142 | | & angular2DResolved)) |
| | | 143 | | { |
| | 1 | 144 | | return false; |
| | | 145 | | } |
| | | 146 | | |
| | 19 | 147 | | result = new ContactNormalVelocityDeltaResultMixed( |
| | 19 | 148 | | normalVelocity, |
| | 19 | 149 | | linearVelocityDelta3D, |
| | 19 | 150 | | angularVelocityDelta3D, |
| | 19 | 151 | | linearVelocityDelta2D, |
| | 19 | 152 | | angularVelocityDelta2D); |
| | 19 | 153 | | return true; |
| | | 154 | | } |
| | | 155 | | |
| | | 156 | | internal static bool TryCalculateAccumulatedDelta( |
| | | 157 | | SolidBody? body3D, |
| | | 158 | | Vector3d linearVelocity3D, |
| | | 159 | | Vector3d angularVelocity3D, |
| | | 160 | | Vector3d relativeContactPoint3D, |
| | | 161 | | SolidBody2D? body2D, |
| | | 162 | | Vector2d linearVelocity2D, |
| | | 163 | | Fixed64 angularVelocity2D, |
| | | 164 | | Vector2d relativeContactPoint2D, |
| | | 165 | | Vector3d normal, |
| | | 166 | | Fixed64 restitution, |
| | | 167 | | Fixed64 restitutionVelocityThreshold, |
| | | 168 | | Fixed64 accumulatedImpulse, |
| | | 169 | | Fixed64 positiveImpulseScale, |
| | | 170 | | Fixed64 negativeImpulseScale, |
| | | 171 | | out ContactNormalImpulseResultMixed result) |
| | | 172 | | { |
| | 184307 | 173 | | result = default; |
| | 184307 | 174 | | bool inputsValid = |
| | 184307 | 175 | | accumulatedImpulse >= Fixed64.Zero |
| | 184307 | 176 | | & positiveImpulseScale >= Fixed64.Zero |
| | 184307 | 177 | | & negativeImpulseScale >= Fixed64.Zero; |
| | 184307 | 178 | | if (!inputsValid) |
| | 3 | 179 | | return false; |
| | 184304 | 180 | | if (!TryComputeNormalVelocity( |
| | 184304 | 181 | | linearVelocity3D, |
| | 184304 | 182 | | angularVelocity3D, |
| | 184304 | 183 | | relativeContactPoint3D, |
| | 184304 | 184 | | linearVelocity2D, |
| | 184304 | 185 | | angularVelocity2D, |
| | 184304 | 186 | | relativeContactPoint2D, |
| | 184304 | 187 | | normal, |
| | 184304 | 188 | | out Fixed64 normalVelocity) |
| | 184304 | 189 | | || !TryComputeDenominator( |
| | 184304 | 190 | | body3D, |
| | 184304 | 191 | | relativeContactPoint3D, |
| | 184304 | 192 | | body2D, |
| | 184304 | 193 | | relativeContactPoint2D, |
| | 184304 | 194 | | normal, |
| | 184304 | 195 | | out Fixed64 denominator)) |
| | | 196 | | { |
| | 615 | 197 | | return false; |
| | | 198 | | } |
| | 183689 | 199 | | if (denominator <= Fixed64.Zero) |
| | | 200 | | { |
| | 38232 | 201 | | result = ZeroImpulse(normalVelocity); |
| | 38232 | 202 | | return true; |
| | | 203 | | } |
| | | 204 | | |
| | 145457 | 205 | | Fixed64 appliedRestitution = normalVelocity < -restitutionVelocityThreshold |
| | 145457 | 206 | | ? restitution |
| | 145457 | 207 | | : Fixed64.Zero; |
| | 145457 | 208 | | Fixed64 responseFactor = -(Fixed64.One + appliedRestitution); |
| | 145457 | 209 | | Fixed64 impulseScale = normalVelocity < Fixed64.Zero |
| | 145457 | 210 | | ? positiveImpulseScale |
| | 145457 | 211 | | : negativeImpulseScale; |
| | | 212 | | Fixed64 impulseScalar; |
| | 145457 | 213 | | if (!Fixed64.TryMultiplyDivide( |
| | 145457 | 214 | | normalVelocity, |
| | 145457 | 215 | | responseFactor, |
| | 145457 | 216 | | impulseScale, |
| | 145457 | 217 | | denominator, |
| | 145457 | 218 | | out Fixed64 scaledImpulse)) |
| | | 219 | | { |
| | 46 | 220 | | if (normalVelocity < Fixed64.Zero) |
| | 44 | 221 | | return false; |
| | 2 | 222 | | impulseScalar = -accumulatedImpulse; |
| | | 223 | | } |
| | 145411 | 224 | | else if (!Fixed64.TryAdd( |
| | 145411 | 225 | | accumulatedImpulse, |
| | 145411 | 226 | | scaledImpulse, |
| | 145411 | 227 | | out Fixed64 accumulated) |
| | 145411 | 228 | | || !Fixed64.TrySubtract( |
| | 145411 | 229 | | FixedMath.Max(Fixed64.Zero, accumulated), |
| | 145411 | 230 | | accumulatedImpulse, |
| | 145411 | 231 | | out impulseScalar)) |
| | | 232 | | { |
| | 1 | 233 | | return false; |
| | | 234 | | } |
| | 145412 | 235 | | if (impulseScalar == Fixed64.Zero) |
| | | 236 | | { |
| | 127506 | 237 | | result = ZeroImpulse(normalVelocity); |
| | 127506 | 238 | | return true; |
| | | 239 | | } |
| | | 240 | | |
| | 17906 | 241 | | Vector2d planarNormal = normal.ToVector2d(); |
| | 17906 | 242 | | bool impulse3DResolved = ContactResponseArithmetic3D.TryScale( |
| | 17906 | 243 | | -normal, |
| | 17906 | 244 | | impulseScalar, |
| | 17906 | 245 | | out Vector3d impulse3D); |
| | 17906 | 246 | | bool linear3DResolved = |
| | 17906 | 247 | | ContactNormalImpulse3D.TryComputeLinearVelocityDelta( |
| | 17906 | 248 | | body3D, |
| | 17906 | 249 | | impulse3D, |
| | 17906 | 250 | | out Vector3d linearVelocityDelta3D); |
| | 17906 | 251 | | bool angular3DResolved = |
| | 17906 | 252 | | ContactNormalImpulse3D.TryComputeAngularVelocityDelta( |
| | 17906 | 253 | | body3D, |
| | 17906 | 254 | | relativeContactPoint3D, |
| | 17906 | 255 | | impulse3D, |
| | 17906 | 256 | | out Vector3d angularVelocityDelta3D); |
| | 17906 | 257 | | bool linear2DResolved = |
| | 17906 | 258 | | ContactNormalImpulse2D.TryComputeLinearVelocityDelta( |
| | 17906 | 259 | | body2D, |
| | 17906 | 260 | | planarNormal, |
| | 17906 | 261 | | impulseScalar, |
| | 17906 | 262 | | out Vector2d linearVelocityDelta2D); |
| | 17906 | 263 | | bool angular2DResolved = |
| | 17906 | 264 | | ContactNormalImpulse2D.TryComputeAngularVelocityDelta( |
| | 17906 | 265 | | body2D, |
| | 17906 | 266 | | relativeContactPoint2D, |
| | 17906 | 267 | | planarNormal, |
| | 17906 | 268 | | impulseScalar, |
| | 17906 | 269 | | out Fixed64 angularVelocityDelta2D); |
| | 17906 | 270 | | if (!(impulse3DResolved |
| | 17906 | 271 | | & linear3DResolved |
| | 17906 | 272 | | & angular3DResolved |
| | 17906 | 273 | | & linear2DResolved |
| | 17906 | 274 | | & angular2DResolved)) |
| | | 275 | | { |
| | 2224 | 276 | | return false; |
| | | 277 | | } |
| | | 278 | | |
| | 15682 | 279 | | result = new ContactNormalImpulseResultMixed( |
| | 15682 | 280 | | normalVelocity, |
| | 15682 | 281 | | impulseScalar, |
| | 15682 | 282 | | linearVelocityDelta3D, |
| | 15682 | 283 | | angularVelocityDelta3D, |
| | 15682 | 284 | | linearVelocityDelta2D, |
| | 15682 | 285 | | angularVelocityDelta2D); |
| | 15682 | 286 | | return true; |
| | | 287 | | } |
| | | 288 | | |
| | | 289 | | internal static bool TryCalculateAccumulatedDeltaExact( |
| | | 290 | | SolidBody? body3D, |
| | | 291 | | Vector3d linearVelocity3D, |
| | | 292 | | Vector3d angularVelocity3D, |
| | | 293 | | in ExactLever3D relativeContactPoint3D, |
| | | 294 | | SolidBody2D? body2D, |
| | | 295 | | Vector2d linearVelocity2D, |
| | | 296 | | Fixed64 angularVelocity2D, |
| | | 297 | | in ExactLever3D relativeContactPoint2D, |
| | | 298 | | Vector3d normal, |
| | | 299 | | Fixed64 restitution, |
| | | 300 | | Fixed64 restitutionVelocityThreshold, |
| | | 301 | | Fixed64 accumulatedImpulse, |
| | | 302 | | Fixed64 positiveImpulseScale, |
| | | 303 | | Fixed64 negativeImpulseScale, |
| | | 304 | | ref ExactContactImpulseState? exactImpulse, |
| | | 305 | | out ContactNormalImpulseResultMixed result, |
| | | 306 | | in ContactResponseSnapshot impact = default) |
| | | 307 | | { |
| | 3244 | 308 | | result = default; |
| | 3244 | 309 | | ExactContactResponseOperand3D first = |
| | 3244 | 310 | | ExactContactLever3D.CreateResponseOperand( |
| | 3244 | 311 | | body3D, |
| | 3244 | 312 | | linearVelocity3D, |
| | 3244 | 313 | | angularVelocity3D, |
| | 3244 | 314 | | relativeContactPoint3D, |
| | 3244 | 315 | | -normal); |
| | 3244 | 316 | | ExactContactResponseOperand3D second = |
| | 3244 | 317 | | ExactContactLever2D.CreateResponseOperand( |
| | 3244 | 318 | | body2D, |
| | 3244 | 319 | | linearVelocity2D, |
| | 3244 | 320 | | angularVelocity2D, |
| | 3244 | 321 | | relativeContactPoint2D, |
| | 3244 | 322 | | normal); |
| | | 323 | | ExactNormalResponse3D response; |
| | 3244 | 324 | | bool resolved = impact.IsValid |
| | 3244 | 325 | | ? ExactContactResponseKernel.TryGetAccumulatedNormalResponse( |
| | 3244 | 326 | | first, |
| | 3244 | 327 | | second, |
| | 3244 | 328 | | normal, |
| | 3244 | 329 | | restitution, |
| | 3244 | 330 | | restitutionVelocityThreshold, |
| | 3244 | 331 | | accumulatedImpulse, |
| | 3244 | 332 | | positiveImpulseScale, |
| | 3244 | 333 | | negativeImpulseScale, |
| | 3244 | 334 | | out response, impact, ref exactImpulse) |
| | 3244 | 335 | | : ExactContactResponseKernel.TryGetAccumulatedNormalResponse(first, second, normal, |
| | 3244 | 336 | | restitution, restitutionVelocityThreshold, accumulatedImpulse, positiveImpulseScale, |
| | 3244 | 337 | | negativeImpulseScale, out response); |
| | 3244 | 338 | | if (!resolved) |
| | | 339 | | { |
| | 12 | 340 | | return false; |
| | | 341 | | } |
| | | 342 | | |
| | 3232 | 343 | | bool hasNormalVelocity = |
| | 3232 | 344 | | response.TryGetNormalVelocity(out Fixed64 normalVelocity); |
| | 3232 | 345 | | bool hasAppliedImpulse = |
| | 3232 | 346 | | response.TryGetAppliedImpulse(out Fixed64 appliedImpulse); |
| | 3232 | 347 | | Fixed64 impulseScalar = response.TryGetAccumulatedImpulse( |
| | 3232 | 348 | | out Fixed64 newAccumulatedImpulse) |
| | 3232 | 349 | | ? newAccumulatedImpulse - accumulatedImpulse |
| | 3232 | 350 | | : -accumulatedImpulse; |
| | 3232 | 351 | | result = new ContactNormalImpulseResultMixed( |
| | 3232 | 352 | | normalVelocity, |
| | 3232 | 353 | | impulseScalar, |
| | 3232 | 354 | | appliedImpulse, |
| | 3232 | 355 | | response.FirstLinearVelocityDelta, |
| | 3232 | 356 | | response.FirstAngularVelocityDelta, |
| | 3232 | 357 | | ExactContactLever2D.ToPlanar(response.SecondLinearVelocityDelta), |
| | 3232 | 358 | | ExactContactLever2D.ToPlanarAngular(response.SecondAngularVelocityDelta), |
| | 3232 | 359 | | hasNormalVelocity, |
| | 3232 | 360 | | hasAppliedImpulse); |
| | 3232 | 361 | | return true; |
| | | 362 | | } |
| | | 363 | | |
| | | 364 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 365 | | private static bool TryComputeNormalVelocity( |
| | | 366 | | Vector3d linearVelocity3D, |
| | | 367 | | Vector3d angularVelocity3D, |
| | | 368 | | Vector3d relativeContactPoint3D, |
| | | 369 | | Vector2d linearVelocity2D, |
| | | 370 | | Fixed64 angularVelocity2D, |
| | | 371 | | Vector2d relativeContactPoint2D, |
| | | 372 | | Vector3d normal, |
| | | 373 | | out Fixed64 normalVelocity) => |
| | 184330 | 374 | | ContactNormalImpulse3D.TryComputeNormalVelocity( |
| | 184330 | 375 | | linearVelocity3D, |
| | 184330 | 376 | | angularVelocity3D, |
| | 184330 | 377 | | relativeContactPoint3D, |
| | 184330 | 378 | | ExactContactLever2D.ToSpatial(linearVelocity2D), |
| | 184330 | 379 | | new Vector3d(Fixed64.Zero, -angularVelocity2D, Fixed64.Zero), |
| | 184330 | 380 | | ExactContactLever2D.ToSpatial(relativeContactPoint2D), |
| | 184330 | 381 | | normal, |
| | 184330 | 382 | | out normalVelocity); |
| | | 383 | | |
| | | 384 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 385 | | private static bool TryComputeDenominator( |
| | | 386 | | SolidBody? body3D, |
| | | 387 | | Vector3d relativeContactPoint3D, |
| | | 388 | | SolidBody2D? body2D, |
| | | 389 | | Vector2d relativeContactPoint2D, |
| | | 390 | | Vector3d normal, |
| | | 391 | | out Fixed64 denominator) |
| | | 392 | | { |
| | 184327 | 393 | | Vector2d planarNormal = normal.ToVector2d(); |
| | 184327 | 394 | | bool angular3DResolved = |
| | 184327 | 395 | | ContactNormalImpulse3D.TryComputeAngularDenominator( |
| | 184327 | 396 | | body3D, |
| | 184327 | 397 | | relativeContactPoint3D, |
| | 184327 | 398 | | normal, |
| | 184327 | 399 | | out Fixed64 angular3D); |
| | 184327 | 400 | | bool angular2DResolved = |
| | 184327 | 401 | | ContactNormalImpulse2D.TryComputeAngularDenominator( |
| | 184327 | 402 | | body2D, |
| | 184327 | 403 | | relativeContactPoint2D, |
| | 184327 | 404 | | planarNormal, |
| | 184327 | 405 | | out Fixed64 angular2D); |
| | 184327 | 406 | | bool sumResolved = Fixed64.TryAdd( |
| | 184327 | 407 | | GetConstrainedInverseMass3D(body3D, normal), |
| | 184327 | 408 | | GetConstrainedInverseMass2D(body2D, normal), |
| | 184327 | 409 | | out Fixed64 linear) |
| | 184327 | 410 | | & Fixed64.TryAdd(linear, angular3D, out Fixed64 first) |
| | 184327 | 411 | | & Fixed64.TryAdd(first, angular2D, out denominator); |
| | 184327 | 412 | | if (!(angular3DResolved & angular2DResolved & sumResolved)) |
| | | 413 | | { |
| | 617 | 414 | | denominator = default; |
| | 617 | 415 | | return false; |
| | | 416 | | } |
| | | 417 | | |
| | 183710 | 418 | | return true; |
| | | 419 | | } |
| | | 420 | | |
| | | 421 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 422 | | private static Fixed64 GetConstrainedInverseMass3D(SolidBody? body, Vector3d axis) => |
| | 184327 | 423 | | body?.GetConstrainedInverseMass(axis) ?? Fixed64.Zero; |
| | | 424 | | |
| | | 425 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 426 | | private static Fixed64 GetConstrainedInverseMass2D(SolidBody2D? body, Vector3d axis) |
| | | 427 | | { |
| | 184327 | 428 | | if (body == null) |
| | 107522 | 429 | | return Fixed64.Zero; |
| | | 430 | | |
| | 76805 | 431 | | Vector2d planarAxis = axis.ToVector2d(); |
| | 76805 | 432 | | return planarAxis == Vector2d.Zero |
| | 76805 | 433 | | ? Fixed64.Zero |
| | 76805 | 434 | | : body.GetConstrainedInverseMass(planarAxis) * planarAxis.MagnitudeSquared; |
| | | 435 | | } |
| | | 436 | | |
| | | 437 | | private static bool TryResolveAngularVelocityDelta3D( |
| | | 438 | | SolidBody? body, |
| | | 439 | | Vector3d relativeContactPoint, |
| | | 440 | | Vector3d signedNormal, |
| | | 441 | | Fixed64 normalVelocity, |
| | | 442 | | Fixed64 responseFactor, |
| | | 443 | | Fixed64 denominator, |
| | | 444 | | out Vector3d velocityDelta) |
| | | 445 | | { |
| | 20 | 446 | | velocityDelta = Vector3d.Zero; |
| | 20 | 447 | | if (body?.CanRotate != true) |
| | 3 | 448 | | return true; |
| | | 449 | | |
| | 17 | 450 | | Vector3d response = body.ApplyConstrainedInverseInertia( |
| | 17 | 451 | | Vector3d.Cross(relativeContactPoint, signedNormal)); |
| | 17 | 452 | | bool xResolved = Fixed64.TryMultiplyDivide( |
| | 17 | 453 | | response.X, |
| | 17 | 454 | | normalVelocity, |
| | 17 | 455 | | responseFactor, |
| | 17 | 456 | | denominator, |
| | 17 | 457 | | out Fixed64 x); |
| | 17 | 458 | | bool yResolved = Fixed64.TryMultiplyDivide( |
| | 17 | 459 | | response.Y, |
| | 17 | 460 | | normalVelocity, |
| | 17 | 461 | | responseFactor, |
| | 17 | 462 | | denominator, |
| | 17 | 463 | | out Fixed64 y); |
| | 17 | 464 | | bool zResolved = Fixed64.TryMultiplyDivide( |
| | 17 | 465 | | response.Z, |
| | 17 | 466 | | normalVelocity, |
| | 17 | 467 | | responseFactor, |
| | 17 | 468 | | denominator, |
| | 17 | 469 | | out Fixed64 z); |
| | 17 | 470 | | velocityDelta = new Vector3d(x, y, z); |
| | 17 | 471 | | return xResolved & yResolved & zResolved; |
| | | 472 | | } |
| | | 473 | | |
| | | 474 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 475 | | private static bool TryResolveAngularVelocityDelta2D( |
| | | 476 | | SolidBody2D? body, |
| | | 477 | | Vector2d relativeContactPoint, |
| | | 478 | | Vector2d signedNormal, |
| | | 479 | | Fixed64 normalVelocity, |
| | | 480 | | Fixed64 responseFactor, |
| | | 481 | | Fixed64 denominator, |
| | | 482 | | out Fixed64 velocityDelta) |
| | | 483 | | { |
| | 20 | 484 | | velocityDelta = Fixed64.Zero; |
| | 20 | 485 | | if (body?.CanRotate != true) |
| | 5 | 486 | | return true; |
| | | 487 | | |
| | 15 | 488 | | Fixed64 torqueScale = Vector2d.CrossProduct(relativeContactPoint, signedNormal); |
| | 15 | 489 | | if (torqueScale == Fixed64.Zero) |
| | 13 | 490 | | return true; |
| | | 491 | | |
| | 2 | 492 | | bool angularScaleResolved = Fixed64.TryMultiplyDivide( |
| | 2 | 493 | | normalVelocity, |
| | 2 | 494 | | responseFactor, |
| | 2 | 495 | | body.EffectiveInverseMomentOfInertia, |
| | 2 | 496 | | denominator, |
| | 2 | 497 | | out Fixed64 angularScale); |
| | 2 | 498 | | bool velocityDeltaResolved = Fixed64.TryMultiplyDivide( |
| | 2 | 499 | | torqueScale, |
| | 2 | 500 | | angularScale, |
| | 2 | 501 | | Fixed64.One, |
| | 2 | 502 | | out velocityDelta); |
| | 2 | 503 | | return angularScaleResolved |
| | 2 | 504 | | & (angularScale != Fixed64.Zero | torqueScale.Abs() <= Fixed64.One) |
| | 2 | 505 | | & velocityDeltaResolved; |
| | | 506 | | } |
| | | 507 | | |
| | | 508 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 509 | | private static ContactNormalImpulseResultMixed ZeroImpulse(Fixed64 normalVelocity) => |
| | 165738 | 510 | | new( |
| | 165738 | 511 | | normalVelocity, |
| | 165738 | 512 | | Fixed64.Zero, |
| | 165738 | 513 | | Vector3d.Zero, |
| | 165738 | 514 | | Vector3d.Zero, |
| | 165738 | 515 | | Vector2d.Zero, |
| | 165738 | 516 | | Fixed64.Zero); |
| | | 517 | | |
| | | 518 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 519 | | private static ContactNormalVelocityDeltaResultMixed ZeroVelocityDelta(Fixed64 normalVelocity) => |
| | 1 | 520 | | new( |
| | 1 | 521 | | normalVelocity, |
| | 1 | 522 | | Vector3d.Zero, |
| | 1 | 523 | | Vector3d.Zero, |
| | 1 | 524 | | Vector2d.Zero, |
| | 1 | 525 | | Fixed64.Zero); |
| | | 526 | | } |