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