| | | 1 | | //======================================================================= |
| | | 2 | | // CollisionResponseMixed.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 FixedMathSharp; |
| | | 9 | | using FixedMathSharp.Geometry; |
| | | 10 | | using Gravitas.Colliders; |
| | | 11 | | using Gravitas.Materials; |
| | | 12 | | using System.Runtime.CompilerServices; |
| | | 13 | | |
| | | 14 | | namespace Gravitas.CollisionHandling; |
| | | 15 | | |
| | | 16 | | /// <summary> |
| | | 17 | | /// Solves deterministic mixed 3D/2D contact response with the 2D body constrained to its X/Z plane. |
| | | 18 | | /// </summary> |
| | | 19 | | public static class CollisionResponseMixed |
| | | 20 | | { |
| | | 21 | | /// <summary>Penetration tolerated before positional correction is applied.</summary> |
| | 1 | 22 | | public static readonly Fixed64 PenetrationSlop = (Fixed64)0.01f; |
| | | 23 | | |
| | | 24 | | /// <summary>Fraction of excess penetration corrected by the mixed solver.</summary> |
| | 1 | 25 | | public static readonly Fixed64 PenetrationCorrectionPercent = (Fixed64)0.8f; |
| | | 26 | | |
| | | 27 | | internal static bool Resolve(CollisionPairMixed pair, MixedContact contact) => |
| | 135 | 28 | | Resolve(pair, contact, iteration: 0, iterationLimit: 1, applyPositionCorrection: true); |
| | | 29 | | |
| | | 30 | | internal static bool Resolve( |
| | | 31 | | CollisionPairMixed pair, |
| | | 32 | | MixedContact contact, |
| | | 33 | | int iteration, |
| | | 34 | | int iterationLimit, |
| | | 35 | | bool applyPositionCorrection) |
| | | 36 | | { |
| | 309 | 37 | | if (!contact.HasContact || pair.Collider3D.IsTrigger || pair.Collider2D.IsTrigger) |
| | 7 | 38 | | return false; |
| | | 39 | | |
| | 302 | 40 | | SolidBody? body3D = pair.Collider3D.Body; |
| | 302 | 41 | | SolidBody2D? body2D = pair.Collider2D.Body; |
| | 302 | 42 | | Vector3d normal = ResolveNormal(pair, contact); |
| | 302 | 43 | | bool hasPlanarResponseCoupling = normal.ToVector2d() != Vector2d.Zero; |
| | | 44 | | |
| | 302 | 45 | | Fixed64 correctionInverseMass = GetConstrainedInverseMass(body3D, normal) |
| | 302 | 46 | | + GetConstrainedPlanarInverseMass(body2D, normal); |
| | | 47 | | |
| | 302 | 48 | | Vector2d relative2D = default; |
| | 302 | 49 | | bool resolved3D = body3D != null |
| | 302 | 50 | | ? body3D.TryGetOffsetFromCenterOfMass(contact.Anchor3D, out Vector3d relative3D) |
| | 302 | 51 | | : contact.Anchor3D.TryGetOffsetFrom(pair.Collider3D.Center, out relative3D); |
| | 302 | 52 | | bool resolved2D = body2D == null |
| | 302 | 53 | | || contact.TryGetPlanarOffset2DFrom( |
| | 302 | 54 | | body2D.Position, |
| | 302 | 55 | | body2D.Rotation, |
| | 302 | 56 | | body2D.LocalCenterOfMassOffset, |
| | 302 | 57 | | out relative2D); |
| | 302 | 58 | | if (body2D == null) |
| | 29 | 59 | | relative2D = Vector2d.Zero; |
| | 302 | 60 | | Vector3d normalLinearVelocity3D = |
| | 302 | 61 | | body3D == null |
| | 302 | 62 | | ? Vector3d.Zero |
| | 302 | 63 | | : ResolveLinearVelocity(body3D); |
| | 302 | 64 | | Vector3d normalAngularVelocity3D = |
| | 302 | 65 | | body3D == null |
| | 302 | 66 | | ? Vector3d.Zero |
| | 302 | 67 | | : ResolveAngularVelocity(body3D); |
| | 302 | 68 | | Vector2d normalLinearVelocity2D = |
| | 302 | 69 | | body2D == null |
| | 302 | 70 | | ? Vector2d.Zero |
| | 302 | 71 | | : ResolveLinearVelocity(body2D); |
| | 302 | 72 | | Fixed64 normalAngularVelocity2D = |
| | 302 | 73 | | body2D == null |
| | 302 | 74 | | ? Fixed64.Zero |
| | 302 | 75 | | : ResolveAngularVelocity(body2D); |
| | | 76 | | |
| | 302 | 77 | | bool normalResolved = TryGetNormalImpulse( |
| | 302 | 78 | | pair, |
| | 302 | 79 | | contact, |
| | 302 | 80 | | body3D, |
| | 302 | 81 | | body2D, |
| | 302 | 82 | | normal, |
| | 302 | 83 | | relative3D, |
| | 302 | 84 | | relative2D, |
| | 302 | 85 | | resolved3D, |
| | 302 | 86 | | resolved2D, |
| | 302 | 87 | | normalLinearVelocity3D, |
| | 302 | 88 | | normalAngularVelocity3D, |
| | 302 | 89 | | normalLinearVelocity2D, |
| | 302 | 90 | | normalAngularVelocity2D, |
| | 302 | 91 | | out ContactNormalImpulseResultMixed normalResult); |
| | 302 | 92 | | if (!normalResolved |
| | 302 | 93 | | || !CanApplyNormalVelocityDeltas(body3D, body2D, normalResult)) |
| | | 94 | | { |
| | 3 | 95 | | GravitasLogger.Channel.Write( |
| | 3 | 96 | | DiagnosticLevel.Error, |
| | 3 | 97 | | "Mixed contact response is outside the representable velocity domain.", |
| | 3 | 98 | | nameof(CollisionResponseMixed)); |
| | 3 | 99 | | return false; |
| | | 100 | | } |
| | | 101 | | |
| | 299 | 102 | | if (applyPositionCorrection && correctionInverseMass > Fixed64.Zero) |
| | 151 | 103 | | ApplyPositionCorrection(body3D, body2D, normal, contact.Depth, correctionInverseMass); |
| | | 104 | | |
| | 299 | 105 | | bool appliedImpulse = HasVelocityDelta(normalResult); |
| | 299 | 106 | | if (appliedImpulse) |
| | | 107 | | { |
| | 113 | 108 | | if (normalResult.HasRepresentableAppliedImpulse |
| | 113 | 109 | | && normalResult.HasRepresentableNormalVelocity) |
| | | 110 | | { |
| | 109 | 111 | | pair.Context.Diagnostics.EmitMixedResponseImpulse( |
| | 109 | 112 | | pair, |
| | 109 | 113 | | contact, |
| | 109 | 114 | | normal * normalResult.AppliedImpulseScalar, |
| | 109 | 115 | | normalResult.NormalVelocity, |
| | 109 | 116 | | iteration, |
| | 109 | 117 | | iterationLimit); |
| | | 118 | | } |
| | 113 | 119 | | ApplyNormalVelocityDeltas(body3D, body2D, normalResult); |
| | | 120 | | } |
| | 299 | 121 | | PhysicsMaterial material3D = contact.HasMaterialOverride |
| | 299 | 122 | | ? contact.Material3D |
| | 299 | 123 | | : pair.Collider3D.Material; |
| | 299 | 124 | | PhysicsMaterial material2D = contact.HasMaterialOverride |
| | 299 | 125 | | ? contact.Material2D |
| | 299 | 126 | | : pair.Collider2D.Material; |
| | 299 | 127 | | bool frictionResolved = TryApplyFrictionImpulse( |
| | 299 | 128 | | pair, |
| | 299 | 129 | | contact, |
| | 299 | 130 | | body3D, |
| | 299 | 131 | | body2D, |
| | 299 | 132 | | normal, |
| | 299 | 133 | | relative3D, |
| | 299 | 134 | | relative2D, |
| | 299 | 135 | | material3D, |
| | 299 | 136 | | material2D, |
| | 299 | 137 | | normalResult, |
| | 299 | 138 | | normalLinearVelocity3D, |
| | 299 | 139 | | normalAngularVelocity3D, |
| | 299 | 140 | | normalLinearVelocity2D, |
| | 299 | 141 | | normalAngularVelocity2D, |
| | 299 | 142 | | pair.Context.Settings.RestitutionVelocityThreshold, |
| | 299 | 143 | | hasPlanarResponseCoupling, |
| | 299 | 144 | | resolved3D, |
| | 299 | 145 | | resolved2D, |
| | 299 | 146 | | out bool frictionApplied); |
| | 299 | 147 | | if (!frictionResolved) |
| | | 148 | | { |
| | 3 | 149 | | GravitasLogger.Channel.Write( |
| | 3 | 150 | | DiagnosticLevel.Error, |
| | 3 | 151 | | "Mixed contact friction is outside the representable velocity domain.", |
| | 3 | 152 | | nameof(CollisionResponseMixed)); |
| | | 153 | | } |
| | | 154 | | |
| | 299 | 155 | | return appliedImpulse | frictionApplied; |
| | | 156 | | } |
| | | 157 | | |
| | | 158 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 159 | | internal static bool HasPlanarResponseCoupling(CollisionPairMixed pair, MixedContact contact) => |
| | 144 | 160 | | ResolveNormal(pair, contact).ToVector2d() != Vector2d.Zero; |
| | | 161 | | |
| | | 162 | | private static void ApplyPositionCorrection( |
| | | 163 | | SolidBody? body3D, |
| | | 164 | | SolidBody2D? body2D, |
| | | 165 | | Vector3d normal, |
| | | 166 | | Fixed64 depth, |
| | | 167 | | Fixed64 effectiveInverseMass) |
| | | 168 | | { |
| | 151 | 169 | | Fixed64 correctionDepth = depth - PenetrationSlop; |
| | 151 | 170 | | if (correctionDepth <= Fixed64.Zero) |
| | 23 | 171 | | return; |
| | | 172 | | |
| | 128 | 173 | | Fixed64 correctionScalar = correctionDepth * PenetrationCorrectionPercent / effectiveInverseMass; |
| | 128 | 174 | | Fixed64 constrainedInverseMass3D = GetConstrainedInverseMass(body3D, normal); |
| | 128 | 175 | | if (constrainedInverseMass3D > Fixed64.Zero) |
| | 123 | 176 | | body3D!.ApplyCollisionPositionCorrection(-normal * (correctionScalar * constrainedInverseMass3D)); |
| | | 177 | | |
| | 128 | 178 | | Fixed64 constrainedInverseMass2D = GetConstrainedPlanarInverseMass(body2D, normal); |
| | 128 | 179 | | if (constrainedInverseMass2D <= Fixed64.Zero) |
| | 82 | 180 | | return; |
| | | 181 | | |
| | 46 | 182 | | Vector2d planarNormal = normal.ToVector2d(); |
| | 46 | 183 | | body2D!.ApplyCollisionPositionCorrection(planarNormal * (correctionScalar * constrainedInverseMass2D)); |
| | 46 | 184 | | } |
| | | 185 | | |
| | | 186 | | private static bool TryGetNormalImpulse( |
| | | 187 | | CollisionPairMixed pair, |
| | | 188 | | MixedContact contact, |
| | | 189 | | SolidBody? body3D, |
| | | 190 | | SolidBody2D? body2D, |
| | | 191 | | Vector3d normal, |
| | | 192 | | Vector3d relative3D, |
| | | 193 | | Vector2d relative2D, |
| | | 194 | | bool hasCompact3D, |
| | | 195 | | bool hasCompact2D, |
| | | 196 | | Vector3d linearVelocity3D, |
| | | 197 | | Vector3d angularVelocity3D, |
| | | 198 | | Vector2d linearVelocity2D, |
| | | 199 | | Fixed64 angularVelocity2D, |
| | | 200 | | out ContactNormalImpulseResultMixed result) |
| | | 201 | | { |
| | 302 | 202 | | PhysicsMaterial material3D = contact.HasMaterialOverride |
| | 302 | 203 | | ? contact.Material3D |
| | 302 | 204 | | : pair.Collider3D.Material; |
| | 302 | 205 | | PhysicsMaterial material2D = contact.HasMaterialOverride |
| | 302 | 206 | | ? contact.Material2D |
| | 302 | 207 | | : pair.Collider2D.Material; |
| | 302 | 208 | | Fixed64 restitution = |
| | 302 | 209 | | PhysicsMaterial.CombineRestitution(material3D, material2D); |
| | 302 | 210 | | Fixed64 restitutionVelocityThreshold = |
| | 302 | 211 | | pair.Context.Settings.RestitutionVelocityThreshold; |
| | 302 | 212 | | if (hasCompact3D |
| | 302 | 213 | | && hasCompact2D |
| | 302 | 214 | | && ContactNormalImpulseMixed.CanUseCompactResponse( |
| | 302 | 215 | | body3D, |
| | 302 | 216 | | linearVelocity3D, |
| | 302 | 217 | | angularVelocity3D, |
| | 302 | 218 | | relative3D, |
| | 302 | 219 | | body2D, |
| | 302 | 220 | | linearVelocity2D, |
| | 302 | 221 | | angularVelocity2D, |
| | 302 | 222 | | relative2D, |
| | 302 | 223 | | normal) |
| | 302 | 224 | | && ContactNormalImpulseMixed.TryCalculateAccumulatedDelta( |
| | 302 | 225 | | body3D, |
| | 302 | 226 | | linearVelocity3D, |
| | 302 | 227 | | angularVelocity3D, |
| | 302 | 228 | | relative3D, |
| | 302 | 229 | | body2D, |
| | 302 | 230 | | linearVelocity2D, |
| | 302 | 231 | | angularVelocity2D, |
| | 302 | 232 | | relative2D, |
| | 302 | 233 | | normal, |
| | 302 | 234 | | restitution, |
| | 302 | 235 | | restitutionVelocityThreshold, |
| | 302 | 236 | | Fixed64.Zero, |
| | 302 | 237 | | Fixed64.One, |
| | 302 | 238 | | Fixed64.One, |
| | 302 | 239 | | out result)) |
| | | 240 | | { |
| | 273 | 241 | | return true; |
| | | 242 | | } |
| | | 243 | | |
| | 29 | 244 | | GetExactLevers( |
| | 29 | 245 | | pair, |
| | 29 | 246 | | contact, |
| | 29 | 247 | | body3D, |
| | 29 | 248 | | body2D, |
| | 29 | 249 | | out ExactLever3D exact3D, |
| | 29 | 250 | | out ExactLever3D exact2D); |
| | 29 | 251 | | return ContactNormalImpulseMixed.TryCalculateAccumulatedDeltaExact( |
| | 29 | 252 | | body3D, |
| | 29 | 253 | | linearVelocity3D, |
| | 29 | 254 | | angularVelocity3D, |
| | 29 | 255 | | exact3D, |
| | 29 | 256 | | body2D, |
| | 29 | 257 | | linearVelocity2D, |
| | 29 | 258 | | angularVelocity2D, |
| | 29 | 259 | | exact2D, |
| | 29 | 260 | | normal, |
| | 29 | 261 | | restitution, |
| | 29 | 262 | | restitutionVelocityThreshold, |
| | 29 | 263 | | Fixed64.Zero, |
| | 29 | 264 | | Fixed64.One, |
| | 29 | 265 | | Fixed64.One, |
| | 29 | 266 | | out result); |
| | | 267 | | } |
| | | 268 | | |
| | | 269 | | private static bool CanApplyNormalVelocityDeltas( |
| | | 270 | | SolidBody? body3D, |
| | | 271 | | SolidBody2D? body2D, |
| | | 272 | | ContactNormalImpulseResultMixed result) => |
| | 300 | 273 | | CanApplyVelocityDeltas( |
| | 300 | 274 | | body3D, |
| | 300 | 275 | | body2D, |
| | 300 | 276 | | result.LinearVelocityDelta3D, |
| | 300 | 277 | | result.AngularVelocityDelta3D, |
| | 300 | 278 | | result.LinearVelocityDelta2D, |
| | 300 | 279 | | result.AngularVelocityDelta2D); |
| | | 280 | | |
| | | 281 | | private static bool HasVelocityDelta( |
| | | 282 | | ContactNormalImpulseResultMixed result) => |
| | 299 | 283 | | result.LinearVelocityDelta3D != Vector3d.Zero |
| | 299 | 284 | | || result.AngularVelocityDelta3D != Vector3d.Zero |
| | 299 | 285 | | || result.LinearVelocityDelta2D != Vector2d.Zero |
| | 299 | 286 | | || result.AngularVelocityDelta2D != Fixed64.Zero; |
| | | 287 | | |
| | | 288 | | private static void ApplyNormalVelocityDeltas( |
| | | 289 | | SolidBody? body3D, |
| | | 290 | | SolidBody2D? body2D, |
| | | 291 | | ContactNormalImpulseResultMixed result) => |
| | 113 | 292 | | ApplyVelocityDeltas( |
| | 113 | 293 | | body3D, |
| | 113 | 294 | | body2D, |
| | 113 | 295 | | result.LinearVelocityDelta3D, |
| | 113 | 296 | | result.AngularVelocityDelta3D, |
| | 113 | 297 | | result.LinearVelocityDelta2D, |
| | 113 | 298 | | result.AngularVelocityDelta2D); |
| | | 299 | | |
| | | 300 | | private static bool CanApplyVelocityDeltas( |
| | | 301 | | SolidBody? body3D, |
| | | 302 | | SolidBody2D? body2D, |
| | | 303 | | Vector3d linear3D, |
| | | 304 | | Vector3d angular3D, |
| | | 305 | | Vector2d linear2D, |
| | | 306 | | Fixed64 angular2D) |
| | | 307 | | { |
| | 343 | 308 | | bool response3DFits = body3D?.CanApplyCollisionVelocityDeltas( |
| | 343 | 309 | | linear3D, |
| | 343 | 310 | | angular3D) |
| | 343 | 311 | | ?? true; |
| | 343 | 312 | | bool response2DFits = body2D?.CanApplyCollisionVelocityDeltas( |
| | 343 | 313 | | linear2D, |
| | 343 | 314 | | angular2D) |
| | 343 | 315 | | ?? true; |
| | 343 | 316 | | return response3DFits & response2DFits; |
| | | 317 | | } |
| | | 318 | | |
| | | 319 | | private static void ApplyVelocityDeltas( |
| | | 320 | | SolidBody? body3D, |
| | | 321 | | SolidBody2D? body2D, |
| | | 322 | | Vector3d linear3D, |
| | | 323 | | Vector3d angular3D, |
| | | 324 | | Vector2d linear2D, |
| | | 325 | | Fixed64 angular2D) |
| | | 326 | | { |
| | 154 | 327 | | if (body3D != null) |
| | | 328 | | { |
| | 150 | 329 | | body3D.ApplyCollisionLinearVelocityDelta(linear3D); |
| | 150 | 330 | | body3D.ApplyCollisionAngularVelocityDelta(angular3D); |
| | | 331 | | } |
| | | 332 | | |
| | 154 | 333 | | if (body2D != null) |
| | | 334 | | { |
| | 146 | 335 | | body2D.ApplyCollisionLinearVelocityDelta(linear2D); |
| | 146 | 336 | | body2D.ApplyCollisionAngularVelocityDelta(angular2D); |
| | | 337 | | } |
| | 154 | 338 | | } |
| | | 339 | | |
| | | 340 | | private static bool TryApplyFrictionImpulse( |
| | | 341 | | CollisionPairMixed pair, |
| | | 342 | | MixedContact contact, |
| | | 343 | | SolidBody? body3D, |
| | | 344 | | SolidBody2D? body2D, |
| | | 345 | | Vector3d normal, |
| | | 346 | | Vector3d relative3D, |
| | | 347 | | Vector2d relative2D, |
| | | 348 | | PhysicsMaterial material3D, |
| | | 349 | | PhysicsMaterial material2D, |
| | | 350 | | ContactNormalImpulseResultMixed normalResult, |
| | | 351 | | Vector3d normalLinearVelocity3D, |
| | | 352 | | Vector3d normalAngularVelocity3D, |
| | | 353 | | Vector2d normalLinearVelocity2D, |
| | | 354 | | Fixed64 normalAngularVelocity2D, |
| | | 355 | | Fixed64 restitutionVelocityThreshold, |
| | | 356 | | bool applyTo2D, |
| | | 357 | | bool hasCompact3D, |
| | | 358 | | bool hasCompact2D, |
| | | 359 | | out bool applied) |
| | | 360 | | { |
| | 299 | 361 | | applied = false; |
| | 299 | 362 | | Fixed64 normalImpulse = normalResult.HasRepresentableAppliedImpulse |
| | 299 | 363 | | ? FixedMath.Max( |
| | 299 | 364 | | Fixed64.Zero, |
| | 299 | 365 | | normalResult.AppliedImpulseScalar) |
| | 299 | 366 | | : Fixed64.Zero; |
| | 299 | 367 | | if (normalResult.HasRepresentableAppliedImpulse |
| | 299 | 368 | | && normalImpulse <= Fixed64.Zero) |
| | 187 | 369 | | return true; |
| | | 370 | | |
| | 112 | 371 | | PhysicsMaterial.CombineFriction(material3D, material2D, out Fixed64 staticFriction, out Fixed64 dynamicFriction) |
| | 112 | 372 | | Fixed64 restitution = |
| | 112 | 373 | | PhysicsMaterial.CombineRestitution(material3D, material2D); |
| | 112 | 374 | | if (staticFriction <= Fixed64.Zero && dynamicFriction <= Fixed64.Zero) |
| | 8 | 375 | | return true; |
| | | 376 | | |
| | 104 | 377 | | if (normalResult.HasRepresentableAppliedImpulse |
| | 104 | 378 | | && hasCompact3D |
| | 104 | 379 | | && hasCompact2D |
| | 104 | 380 | | && TryGetCompactFrictionImpulse( |
| | 104 | 381 | | body3D, |
| | 104 | 382 | | body2D, |
| | 104 | 383 | | normal, |
| | 104 | 384 | | relative3D, |
| | 104 | 385 | | relative2D, |
| | 104 | 386 | | normalImpulse, |
| | 104 | 387 | | staticFriction, |
| | 104 | 388 | | dynamicFriction, |
| | 104 | 389 | | applyTo2D, |
| | 104 | 390 | | out Vector3d tangent, |
| | 104 | 391 | | out Fixed64 impulseScalar) |
| | 104 | 392 | | && (impulseScalar == Fixed64.Zero |
| | 104 | 393 | | || TryApplyImpulse( |
| | 104 | 394 | | body3D, |
| | 104 | 395 | | body2D, |
| | 104 | 396 | | tangent, |
| | 104 | 397 | | relative3D, |
| | 104 | 398 | | relative2D, |
| | 104 | 399 | | impulseScalar, |
| | 104 | 400 | | applyTo2D))) |
| | | 401 | | { |
| | 76 | 402 | | applied = impulseScalar != Fixed64.Zero; |
| | 76 | 403 | | return true; |
| | | 404 | | } |
| | | 405 | | |
| | 28 | 406 | | return TryApplyFrictionImpulseExact( |
| | 28 | 407 | | pair, |
| | 28 | 408 | | contact, |
| | 28 | 409 | | body3D, |
| | 28 | 410 | | body2D, |
| | 28 | 411 | | normal, |
| | 28 | 412 | | normalLinearVelocity3D, |
| | 28 | 413 | | normalAngularVelocity3D, |
| | 28 | 414 | | normalLinearVelocity2D, |
| | 28 | 415 | | normalAngularVelocity2D, |
| | 28 | 416 | | restitutionVelocityThreshold, |
| | 28 | 417 | | restitution, |
| | 28 | 418 | | staticFriction, |
| | 28 | 419 | | dynamicFriction, |
| | 28 | 420 | | applyTo2D, |
| | 28 | 421 | | out applied); |
| | | 422 | | } |
| | | 423 | | |
| | | 424 | | private static bool TryGetCompactFrictionImpulse( |
| | | 425 | | SolidBody? body3D, |
| | | 426 | | SolidBody2D? body2D, |
| | | 427 | | Vector3d normal, |
| | | 428 | | Vector3d relative3D, |
| | | 429 | | Vector2d relative2D, |
| | | 430 | | Fixed64 normalImpulse, |
| | | 431 | | Fixed64 staticFriction, |
| | | 432 | | Fixed64 dynamicFriction, |
| | | 433 | | bool applyTo2D, |
| | | 434 | | out Vector3d tangent, |
| | | 435 | | out Fixed64 impulseScalar) |
| | | 436 | | { |
| | 86 | 437 | | tangent = default; |
| | 86 | 438 | | impulseScalar = Fixed64.Zero; |
| | 86 | 439 | | Vector3d linearVelocity3D = body3D == null |
| | 86 | 440 | | ? Vector3d.Zero |
| | 86 | 441 | | : ResolveLinearVelocity(body3D); |
| | 86 | 442 | | Vector3d angularVelocity3D = body3D == null |
| | 86 | 443 | | ? Vector3d.Zero |
| | 86 | 444 | | : ResolveAngularVelocity(body3D); |
| | 86 | 445 | | Vector3d linearVelocity2D = body2D == null |
| | 86 | 446 | | ? Vector3d.Zero |
| | 86 | 447 | | : ExactContactLever2D.ToSpatial( |
| | 86 | 448 | | ResolveLinearVelocity(body2D)); |
| | 86 | 449 | | Vector3d angularVelocity2D = body2D == null |
| | 86 | 450 | | ? Vector3d.Zero |
| | 86 | 451 | | : new Vector3d( |
| | 86 | 452 | | Fixed64.Zero, |
| | 86 | 453 | | -ResolveAngularVelocity(body2D), |
| | 86 | 454 | | Fixed64.Zero); |
| | 86 | 455 | | if (!ContactResponseArithmetic3D.TryGetRelativePointVelocity( |
| | 86 | 456 | | linearVelocity3D, |
| | 86 | 457 | | angularVelocity3D, |
| | 86 | 458 | | relative3D, |
| | 86 | 459 | | linearVelocity2D, |
| | 86 | 460 | | angularVelocity2D, |
| | 86 | 461 | | ExactContactLever2D.ToSpatial(relative2D), |
| | 86 | 462 | | normal, |
| | 86 | 463 | | out Vector3d relativeVelocity) |
| | 86 | 464 | | || !TryGetCompactTangent( |
| | 86 | 465 | | relativeVelocity, |
| | 86 | 466 | | normal, |
| | 86 | 467 | | out tangent, |
| | 86 | 468 | | out bool hasTangentialMotion)) |
| | | 469 | | { |
| | 2 | 470 | | return false; |
| | | 471 | | } |
| | 84 | 472 | | if (!hasTangentialMotion) |
| | 57 | 473 | | return true; |
| | | 474 | | |
| | 27 | 475 | | bool denominatorResolved = |
| | 27 | 476 | | TryGetCompactConstrainedInverseMass( |
| | 27 | 477 | | body3D, |
| | 27 | 478 | | tangent, |
| | 27 | 479 | | out Fixed64 linear3D); |
| | 27 | 480 | | denominatorResolved &= |
| | 27 | 481 | | ContactNormalImpulse3D.TryComputeAngularDenominator( |
| | 27 | 482 | | body3D, |
| | 27 | 483 | | relative3D, |
| | 27 | 484 | | tangent, |
| | 27 | 485 | | out Fixed64 angular3D); |
| | 27 | 486 | | Fixed64 linear2D = Fixed64.Zero; |
| | 27 | 487 | | Fixed64 angular2D = Fixed64.Zero; |
| | 27 | 488 | | if (applyTo2D) |
| | | 489 | | { |
| | 21 | 490 | | denominatorResolved &= |
| | 21 | 491 | | TryGetCompactConstrainedPlanarInverseMass( |
| | 21 | 492 | | body2D, |
| | 21 | 493 | | tangent, |
| | 21 | 494 | | out linear2D); |
| | 21 | 495 | | denominatorResolved &= |
| | 21 | 496 | | ContactNormalImpulse2D.TryComputeAngularDenominator( |
| | 21 | 497 | | body2D, |
| | 21 | 498 | | relative2D, |
| | 21 | 499 | | tangent.ToVector2d(), |
| | 21 | 500 | | out angular2D); |
| | | 501 | | } |
| | | 502 | | |
| | 27 | 503 | | var denominatorTerms = new ContactEffectiveMassTerms3D( |
| | 27 | 504 | | linear3D, |
| | 27 | 505 | | linear2D, |
| | 27 | 506 | | angular3D, |
| | 27 | 507 | | angular2D); |
| | 27 | 508 | | if (!denominatorResolved |
| | 27 | 509 | | || !denominatorTerms.TryGetValue(out Fixed64 denominator)) |
| | | 510 | | { |
| | 3 | 511 | | return false; |
| | | 512 | | } |
| | 24 | 513 | | if (denominator <= Fixed64.Zero) |
| | 1 | 514 | | return true; |
| | | 515 | | |
| | 23 | 516 | | if (!ContactResponseArithmetic3D.TryDot( |
| | 23 | 517 | | relativeVelocity, |
| | 23 | 518 | | tangent, |
| | 23 | 519 | | out Fixed64 tangentVelocity) |
| | 23 | 520 | | || !Fixed64.TryMultiplyDivide( |
| | 23 | 521 | | -tangentVelocity, |
| | 23 | 522 | | Fixed64.One, |
| | 23 | 523 | | denominator, |
| | 23 | 524 | | out impulseScalar) |
| | 23 | 525 | | || !Fixed64.TryMultiplyDivide( |
| | 23 | 526 | | normalImpulse, |
| | 23 | 527 | | staticFriction, |
| | 23 | 528 | | Fixed64.One, |
| | 23 | 529 | | out Fixed64 staticLimit) |
| | 23 | 530 | | || !Fixed64.TryMultiplyDivide( |
| | 23 | 531 | | normalImpulse, |
| | 23 | 532 | | dynamicFriction, |
| | 23 | 533 | | Fixed64.One, |
| | 23 | 534 | | out Fixed64 dynamicLimit)) |
| | | 535 | | { |
| | 3 | 536 | | return false; |
| | | 537 | | } |
| | | 538 | | |
| | 20 | 539 | | if (impulseScalar < -staticLimit |
| | 20 | 540 | | || impulseScalar > staticLimit) |
| | | 541 | | { |
| | 10 | 542 | | impulseScalar = FixedMath.Clamp( |
| | 10 | 543 | | impulseScalar, |
| | 10 | 544 | | -dynamicLimit, |
| | 10 | 545 | | dynamicLimit); |
| | | 546 | | } |
| | 20 | 547 | | return true; |
| | | 548 | | } |
| | | 549 | | |
| | | 550 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 551 | | private static bool TryApplyFrictionImpulseExact( |
| | | 552 | | CollisionPairMixed pair, |
| | | 553 | | MixedContact contact, |
| | | 554 | | SolidBody? body3D, |
| | | 555 | | SolidBody2D? body2D, |
| | | 556 | | Vector3d normal, |
| | | 557 | | Vector3d normalLinearVelocity3D, |
| | | 558 | | Vector3d normalAngularVelocity3D, |
| | | 559 | | Vector2d normalLinearVelocity2D, |
| | | 560 | | Fixed64 normalAngularVelocity2D, |
| | | 561 | | Fixed64 restitutionVelocityThreshold, |
| | | 562 | | Fixed64 restitution, |
| | | 563 | | Fixed64 staticFriction, |
| | | 564 | | Fixed64 dynamicFriction, |
| | | 565 | | bool applyTo2D, |
| | | 566 | | out bool applied) |
| | | 567 | | { |
| | 28 | 568 | | applied = false; |
| | 28 | 569 | | GetExactLevers( |
| | 28 | 570 | | pair, |
| | 28 | 571 | | contact, |
| | 28 | 572 | | body3D, |
| | 28 | 573 | | body2D, |
| | 28 | 574 | | out ExactLever3D exact3D, |
| | 28 | 575 | | out ExactLever3D exact2D); |
| | 28 | 576 | | Vector3d tangent = SolverContact.CreateTangent(normal); |
| | 28 | 577 | | Vector3d secondaryTangent = |
| | 28 | 578 | | Vector3d.Cross(normal, tangent).Normalized; |
| | 28 | 579 | | SolidBody2D? responseBody2D = |
| | 28 | 580 | | applyTo2D ? body2D : null; |
| | 28 | 581 | | Vector2d responseLinearVelocity2D = |
| | 28 | 582 | | responseBody2D == null |
| | 28 | 583 | | ? Vector2d.Zero |
| | 28 | 584 | | : normalLinearVelocity2D; |
| | 28 | 585 | | Fixed64 responseAngularVelocity2D = |
| | 28 | 586 | | responseBody2D == null |
| | 28 | 587 | | ? Fixed64.Zero |
| | 28 | 588 | | : normalAngularVelocity2D; |
| | 28 | 589 | | var normalConstraint = new ExactNormalConstraint3D( |
| | 28 | 590 | | ExactContactLever3D.CreateResponseOperand( |
| | 28 | 591 | | body3D, |
| | 28 | 592 | | normalLinearVelocity3D, |
| | 28 | 593 | | normalAngularVelocity3D, |
| | 28 | 594 | | exact3D, |
| | 28 | 595 | | -normal), |
| | 28 | 596 | | ExactContactLever2D.CreateResponseOperand( |
| | 28 | 597 | | responseBody2D, |
| | 28 | 598 | | responseLinearVelocity2D, |
| | 28 | 599 | | responseAngularVelocity2D, |
| | 28 | 600 | | exact2D, |
| | 28 | 601 | | normal), |
| | 28 | 602 | | normal, |
| | 28 | 603 | | restitution, |
| | 28 | 604 | | restitutionVelocityThreshold, |
| | 28 | 605 | | Fixed64.Zero, |
| | 28 | 606 | | Fixed64.One, |
| | 28 | 607 | | Fixed64.One); |
| | 28 | 608 | | Vector3d linearVelocity3D = |
| | 28 | 609 | | body3D == null |
| | 28 | 610 | | ? Vector3d.Zero |
| | 28 | 611 | | : ResolveLinearVelocity(body3D); |
| | 28 | 612 | | Vector3d angularVelocity3D = |
| | 28 | 613 | | body3D == null |
| | 28 | 614 | | ? Vector3d.Zero |
| | 28 | 615 | | : ResolveAngularVelocity(body3D); |
| | 28 | 616 | | Vector2d linearVelocity2D = |
| | 28 | 617 | | responseBody2D == null |
| | 28 | 618 | | ? Vector2d.Zero |
| | 28 | 619 | | : ResolveLinearVelocity(responseBody2D); |
| | 28 | 620 | | Fixed64 angularVelocity2D = |
| | 28 | 621 | | responseBody2D == null |
| | 28 | 622 | | ? Fixed64.Zero |
| | 28 | 623 | | : ResolveAngularVelocity(responseBody2D); |
| | 28 | 624 | | if (!ExactContactResponseKernel.TryGetCoulombDiskResponse( |
| | 28 | 625 | | normalConstraint, |
| | 28 | 626 | | ExactContactLever3D.CreateResponseOperand( |
| | 28 | 627 | | body3D, |
| | 28 | 628 | | linearVelocity3D, |
| | 28 | 629 | | angularVelocity3D, |
| | 28 | 630 | | exact3D, |
| | 28 | 631 | | -tangent), |
| | 28 | 632 | | ExactContactLever2D.CreateResponseOperand( |
| | 28 | 633 | | responseBody2D, |
| | 28 | 634 | | linearVelocity2D, |
| | 28 | 635 | | angularVelocity2D, |
| | 28 | 636 | | exact2D, |
| | 28 | 637 | | tangent), |
| | 28 | 638 | | tangent, |
| | 28 | 639 | | Fixed64.Zero, |
| | 28 | 640 | | ExactContactLever3D.CreateResponseOperand( |
| | 28 | 641 | | body3D, |
| | 28 | 642 | | linearVelocity3D, |
| | 28 | 643 | | angularVelocity3D, |
| | 28 | 644 | | exact3D, |
| | 28 | 645 | | -secondaryTangent), |
| | 28 | 646 | | ExactContactLever2D.CreateResponseOperand( |
| | 28 | 647 | | responseBody2D, |
| | 28 | 648 | | linearVelocity2D, |
| | 28 | 649 | | angularVelocity2D, |
| | 28 | 650 | | exact2D, |
| | 28 | 651 | | secondaryTangent), |
| | 28 | 652 | | secondaryTangent, |
| | 28 | 653 | | Fixed64.Zero, |
| | 28 | 654 | | staticFriction, |
| | 28 | 655 | | dynamicFriction, |
| | 28 | 656 | | out ExactCoulombResponse3D response)) |
| | | 657 | | { |
| | 2 | 658 | | return false; |
| | | 659 | | } |
| | | 660 | | |
| | 26 | 661 | | Vector3d linear3D = |
| | 26 | 662 | | response.FirstLinearVelocityDelta; |
| | 26 | 663 | | Vector3d angular3D = |
| | 26 | 664 | | response.FirstAngularVelocityDelta; |
| | 26 | 665 | | Vector2d linear2D = ExactContactLever2D.ToPlanar( |
| | 26 | 666 | | response.SecondLinearVelocityDelta); |
| | 26 | 667 | | Fixed64 angular2D = ExactContactLever2D.ToPlanarAngular( |
| | 26 | 668 | | response.SecondAngularVelocityDelta); |
| | 26 | 669 | | if (!CanApplyVelocityDeltas( |
| | 26 | 670 | | body3D, |
| | 26 | 671 | | body2D, |
| | 26 | 672 | | linear3D, |
| | 26 | 673 | | angular3D, |
| | 26 | 674 | | linear2D, |
| | 26 | 675 | | angular2D)) |
| | | 676 | | { |
| | 1 | 677 | | return false; |
| | | 678 | | } |
| | | 679 | | |
| | 25 | 680 | | if (!response.HasAppliedImpulse) |
| | 1 | 681 | | return true; |
| | | 682 | | |
| | 24 | 683 | | ApplyVelocityDeltas( |
| | 24 | 684 | | body3D, |
| | 24 | 685 | | body2D, |
| | 24 | 686 | | linear3D, |
| | 24 | 687 | | angular3D, |
| | 24 | 688 | | linear2D, |
| | 24 | 689 | | angular2D); |
| | 24 | 690 | | applied = true; |
| | 24 | 691 | | return true; |
| | | 692 | | } |
| | | 693 | | |
| | | 694 | | private static bool TryApplyImpulse( |
| | | 695 | | SolidBody? body3D, |
| | | 696 | | SolidBody2D? body2D, |
| | | 697 | | Vector3d axis, |
| | | 698 | | Vector3d relative3D, |
| | | 699 | | Vector2d relative2D, |
| | | 700 | | Fixed64 impulseScalar, |
| | | 701 | | bool applyTo2D) |
| | | 702 | | { |
| | 19 | 703 | | Vector3d linear3D = Vector3d.Zero; |
| | 19 | 704 | | Vector3d angular3D = Vector3d.Zero; |
| | 19 | 705 | | Vector2d linear2D = Vector2d.Zero; |
| | 19 | 706 | | Fixed64 angular2D = Fixed64.Zero; |
| | 19 | 707 | | Vector3d impulse3D = -axis * impulseScalar; |
| | 19 | 708 | | bool impulse3DResolved = |
| | 19 | 709 | | (impulse3D.X != Fixed64.Zero |
| | 19 | 710 | | || axis.X == Fixed64.Zero) |
| | 19 | 711 | | && (impulse3D.Y != Fixed64.Zero |
| | 19 | 712 | | || axis.Y == Fixed64.Zero) |
| | 19 | 713 | | && (impulse3D.Z != Fixed64.Zero |
| | 19 | 714 | | || axis.Z == Fixed64.Zero); |
| | 19 | 715 | | bool linear3DResolved = impulse3DResolved |
| | 19 | 716 | | && ContactNormalImpulse3D.TryComputeLinearVelocityDelta( |
| | 19 | 717 | | body3D, |
| | 19 | 718 | | impulse3D, |
| | 19 | 719 | | out linear3D); |
| | 19 | 720 | | bool angular3DResolved = impulse3DResolved |
| | 19 | 721 | | && ContactNormalImpulse3D.TryComputeAngularVelocityDelta( |
| | 19 | 722 | | body3D, |
| | 19 | 723 | | relative3D, |
| | 19 | 724 | | impulse3D, |
| | 19 | 725 | | out angular3D); |
| | 19 | 726 | | Vector2d planarAxis = axis.ToVector2d(); |
| | 19 | 727 | | bool linear2DResolved = !applyTo2D |
| | 19 | 728 | | || ContactNormalImpulse2D.TryComputeLinearVelocityDelta( |
| | 19 | 729 | | body2D, |
| | 19 | 730 | | planarAxis, |
| | 19 | 731 | | impulseScalar, |
| | 19 | 732 | | out linear2D); |
| | 19 | 733 | | bool angular2DResolved = !applyTo2D |
| | 19 | 734 | | || ContactNormalImpulse2D.TryComputeAngularVelocityDelta( |
| | 19 | 735 | | body2D, |
| | 19 | 736 | | relative2D, |
| | 19 | 737 | | planarAxis, |
| | 19 | 738 | | impulseScalar, |
| | 19 | 739 | | out angular2D); |
| | 19 | 740 | | if (!(linear3DResolved |
| | 19 | 741 | | & angular3DResolved |
| | 19 | 742 | | & linear2DResolved |
| | 19 | 743 | | & angular2DResolved) |
| | 19 | 744 | | || !CanApplyVelocityDeltas( |
| | 19 | 745 | | body3D, |
| | 19 | 746 | | body2D, |
| | 19 | 747 | | linear3D, |
| | 19 | 748 | | angular3D, |
| | 19 | 749 | | linear2D, |
| | 19 | 750 | | angular2D)) |
| | | 751 | | { |
| | 2 | 752 | | return false; |
| | | 753 | | } |
| | | 754 | | |
| | 17 | 755 | | ApplyVelocityDeltas( |
| | 17 | 756 | | body3D, |
| | 17 | 757 | | body2D, |
| | 17 | 758 | | linear3D, |
| | 17 | 759 | | angular3D, |
| | 17 | 760 | | linear2D, |
| | 17 | 761 | | angular2D); |
| | 17 | 762 | | return true; |
| | | 763 | | } |
| | | 764 | | |
| | | 765 | | private static bool TryGetCompactTangent( |
| | | 766 | | Vector3d relativeVelocity, |
| | | 767 | | Vector3d normal, |
| | | 768 | | out Vector3d tangent, |
| | | 769 | | out bool hasTangentialMotion) |
| | | 770 | | { |
| | 85 | 771 | | tangent = default; |
| | 85 | 772 | | hasTangentialMotion = false; |
| | 85 | 773 | | if (!ContactResponseArithmetic3D.TryDot( |
| | 85 | 774 | | relativeVelocity, |
| | 85 | 775 | | normal, |
| | 85 | 776 | | out Fixed64 normalVelocity) |
| | 85 | 777 | | || !ContactResponseArithmetic3D.TryLinearCombination( |
| | 85 | 778 | | relativeVelocity, |
| | 85 | 779 | | Fixed64.One, |
| | 85 | 780 | | normal, |
| | 85 | 781 | | -normalVelocity, |
| | 85 | 782 | | Vector3d.Zero, |
| | 85 | 783 | | Fixed64.Zero, |
| | 85 | 784 | | out Vector3d tangentVelocity)) |
| | | 785 | | { |
| | 1 | 786 | | return false; |
| | | 787 | | } |
| | 84 | 788 | | if (tangentVelocity == Vector3d.Zero) |
| | 56 | 789 | | return true; |
| | | 790 | | |
| | 28 | 791 | | Fixed64 magnitudeSquared = |
| | 28 | 792 | | tangentVelocity.MagnitudeSquared; |
| | 28 | 793 | | if (magnitudeSquared <= Fixed64.Epsilon) |
| | 1 | 794 | | return true; |
| | | 795 | | |
| | 27 | 796 | | tangent = tangentVelocity.Normalized; |
| | 27 | 797 | | hasTangentialMotion = tangent.IsNormalized(); |
| | 27 | 798 | | return hasTangentialMotion; |
| | | 799 | | } |
| | | 800 | | |
| | | 801 | | private static bool TryGetCompactConstrainedInverseMass( |
| | | 802 | | SolidBody? body, |
| | | 803 | | Vector3d axis, |
| | | 804 | | out Fixed64 inverseMass) |
| | | 805 | | { |
| | 27 | 806 | | if (body == null) |
| | | 807 | | { |
| | 1 | 808 | | inverseMass = Fixed64.Zero; |
| | 1 | 809 | | return true; |
| | | 810 | | } |
| | | 811 | | |
| | 26 | 812 | | inverseMass = body.GetConstrainedInverseMass(axis); |
| | 26 | 813 | | return inverseMass != Fixed64.Zero |
| | 26 | 814 | | || body.EffectiveInverseMass == Fixed64.Zero |
| | 26 | 815 | | || body.ProjectLinearMotion(axis) == Vector3d.Zero; |
| | | 816 | | } |
| | | 817 | | |
| | | 818 | | private static bool TryGetCompactConstrainedPlanarInverseMass( |
| | | 819 | | SolidBody2D? body, |
| | | 820 | | Vector3d axis, |
| | | 821 | | out Fixed64 inverseMass) |
| | | 822 | | { |
| | 21 | 823 | | inverseMass = Fixed64.Zero; |
| | 21 | 824 | | if (body == null) |
| | 1 | 825 | | return true; |
| | | 826 | | |
| | 20 | 827 | | Vector2d planarAxis = axis.ToVector2d(); |
| | 20 | 828 | | if (!ContactNormalImpulse2D.TryGetConstrainedInverseMass( |
| | 20 | 829 | | body, |
| | 20 | 830 | | planarAxis, |
| | 20 | 831 | | out Fixed64 constrained)) |
| | 1 | 832 | | return false; |
| | 19 | 833 | | if (constrained == Fixed64.Zero) |
| | 9 | 834 | | return true; |
| | | 835 | | |
| | 10 | 836 | | Fixed64 magnitudeSquared = |
| | 10 | 837 | | planarAxis.MagnitudeSquared; |
| | 10 | 838 | | if (!Fixed64.TryMultiplyDivide( |
| | 10 | 839 | | constrained, |
| | 10 | 840 | | magnitudeSquared, |
| | 10 | 841 | | Fixed64.One, |
| | 10 | 842 | | out inverseMass)) |
| | | 843 | | { |
| | 1 | 844 | | inverseMass = default; |
| | 1 | 845 | | return false; |
| | | 846 | | } |
| | | 847 | | |
| | 9 | 848 | | return inverseMass != Fixed64.Zero; |
| | | 849 | | } |
| | | 850 | | |
| | | 851 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 852 | | private static Vector3d ResolveLinearVelocity(SolidBody body) => |
| | 406 | 853 | | body.ProjectLinearMotion( |
| | 406 | 854 | | body.IsKinematic |
| | 406 | 855 | | ? body.SampleContinuousCollisionLinearVelocity(Fixed64.One) |
| | 406 | 856 | | : body.LinearVelocity); |
| | | 857 | | |
| | | 858 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 859 | | private static Vector2d ResolveLinearVelocity(SolidBody2D body) => |
| | 380 | 860 | | body.ProjectLinearMotion( |
| | 380 | 861 | | body.IsKinematic |
| | 380 | 862 | | ? body.SampleContinuousCollisionLinearVelocity(Fixed64.One) |
| | 380 | 863 | | : body.LinearVelocity); |
| | | 864 | | |
| | | 865 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 866 | | private static Vector3d ResolveAngularVelocity(SolidBody body) => |
| | 406 | 867 | | body.ProjectAngularMotion( |
| | 406 | 868 | | body.IsKinematic |
| | 406 | 869 | | ? body.SampleContinuousCollisionAngularVelocity(Fixed64.One) |
| | 406 | 870 | | : body.AngularVelocity); |
| | | 871 | | |
| | | 872 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 873 | | private static Fixed64 ResolveAngularVelocity(SolidBody2D body) => |
| | 380 | 874 | | body.IsKinematic |
| | 380 | 875 | | ? body.SampleContinuousCollisionAngularVelocity(Fixed64.One) |
| | 380 | 876 | | : body.AngularVelocity; |
| | | 877 | | |
| | | 878 | | private static Vector3d ResolveNormal(CollisionPairMixed pair, MixedContact contact) |
| | | 879 | | { |
| | 446 | 880 | | Vector3d fallback = ResolveFallbackDirection(pair, contact); |
| | 446 | 881 | | if (contact.Normal3DTo2D.MagnitudeSquared > Fixed64.Epsilon) |
| | | 882 | | { |
| | 440 | 883 | | Vector3d normal = contact.Normal3DTo2D.Normalized; |
| | 440 | 884 | | return fallback.MagnitudeSquared > Fixed64.Epsilon && Vector3d.Dot(normal, fallback) < Fixed64.Zero |
| | 440 | 885 | | ? -normal |
| | 440 | 886 | | : normal; |
| | | 887 | | } |
| | | 888 | | |
| | 6 | 889 | | return fallback == Vector3d.Zero ? Vector3d.Up : fallback; |
| | | 890 | | } |
| | | 891 | | |
| | | 892 | | private static Vector3d ResolveFallbackDirection(CollisionPairMixed pair, MixedContact contact) |
| | | 893 | | { |
| | 446 | 894 | | LSCollider2D collider2D = pair.Collider2D; |
| | 446 | 895 | | Vector3d embeddedCenter = new( |
| | 446 | 896 | | collider2D.Center.X, |
| | 446 | 897 | | collider2D.MixedSlabCenterY, |
| | 446 | 898 | | collider2D.Center.Y); |
| | 446 | 899 | | Vector3d centerDirection = embeddedCenter - pair.Collider3D.Center; |
| | 446 | 900 | | if (centerDirection.MagnitudeSquared > Fixed64.Epsilon) |
| | 313 | 901 | | return centerDirection.Normalized; |
| | | 902 | | |
| | 133 | 903 | | if (contact.TryGetPoint2D(out Vector3d point2D) |
| | 133 | 904 | | && contact.TryGetPoint3D(out Vector3d point3D)) |
| | | 905 | | { |
| | 112 | 906 | | Vector3d pointDirection = point2D - point3D; |
| | 112 | 907 | | if (pointDirection.MagnitudeSquared > Fixed64.Epsilon) |
| | 3 | 908 | | return pointDirection.Normalized; |
| | | 909 | | } |
| | | 910 | | |
| | 130 | 911 | | return Vector3d.Zero; |
| | | 912 | | } |
| | | 913 | | |
| | | 914 | | private static void GetExactLevers( |
| | | 915 | | CollisionPairMixed pair, |
| | | 916 | | MixedContact contact, |
| | | 917 | | SolidBody? body3D, |
| | | 918 | | SolidBody2D? body2D, |
| | | 919 | | out ExactLever3D exact3D, |
| | | 920 | | out ExactLever3D exact2D) |
| | | 921 | | { |
| | 57 | 922 | | ContactAnchor center3D = body3D?.GetCenterOfMassAnchor() |
| | 57 | 923 | | ?? ContactAnchor.FromWorldPoint(pair.Collider3D.Center); |
| | 57 | 924 | | exact3D = contact.Anchor3D.GetLeverFrom(center3D); |
| | 57 | 925 | | exact2D = body2D == null |
| | 57 | 926 | | ? contact.GetPlanarXZLeverFrom( |
| | 57 | 927 | | pair.Collider2D.Center, |
| | 57 | 928 | | Fixed64.Zero, |
| | 57 | 929 | | Vector2d.Zero) |
| | 57 | 930 | | : contact.GetPlanarXZLeverFrom( |
| | 57 | 931 | | body2D.Position, |
| | 57 | 932 | | body2D.Rotation, |
| | 57 | 933 | | body2D.LocalCenterOfMassOffset); |
| | 57 | 934 | | } |
| | | 935 | | |
| | | 936 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 937 | | private static Fixed64 GetConstrainedInverseMass(SolidBody? body, Vector3d axis) => |
| | 430 | 938 | | body?.GetConstrainedInverseMass(axis) ?? Fixed64.Zero; |
| | | 939 | | |
| | | 940 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 941 | | private static Fixed64 GetConstrainedPlanarInverseMass(SolidBody2D? body, Vector3d axis) |
| | | 942 | | { |
| | 430 | 943 | | if (body == null) |
| | 41 | 944 | | return Fixed64.Zero; |
| | | 945 | | |
| | 389 | 946 | | Vector2d planarAxis = axis.ToVector2d(); |
| | 389 | 947 | | if (planarAxis == Vector2d.Zero) |
| | 157 | 948 | | return Fixed64.Zero; |
| | | 949 | | |
| | 232 | 950 | | return body.GetConstrainedInverseMass(planarAxis) * planarAxis.MagnitudeSquared; |
| | | 951 | | } |
| | | 952 | | |
| | | 953 | | } |