| | | 1 | | //======================================================================= |
| | | 2 | | // CollisionResponse2D.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.Materials; |
| | | 11 | | using System.Runtime.CompilerServices; |
| | | 12 | | |
| | | 13 | | namespace Gravitas.CollisionHandling; |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// Deterministic pure 2D manifold contact response. |
| | | 17 | | /// </summary> |
| | | 18 | | public static class CollisionResponse2D |
| | | 19 | | { |
| | | 20 | | /// <summary>Penetration tolerated before positional correction is applied.</summary> |
| | 1 | 21 | | public static readonly Fixed64 PenetrationSlop = (Fixed64)0.01f; |
| | | 22 | | |
| | | 23 | | /// <summary>Fraction of excess penetration corrected by the solver.</summary> |
| | 1 | 24 | | public static readonly Fixed64 PenetrationCorrectionPercent = Fixed64.One; |
| | | 25 | | |
| | | 26 | | internal static void Resolve(CollisionPair2D pair) => |
| | 454 | 27 | | Resolve(pair, applyCachedImpulse: true, applyPositionCorrection: true); |
| | | 28 | | |
| | | 29 | | internal static void Resolve( |
| | | 30 | | CollisionPair2D pair, |
| | | 31 | | bool applyCachedImpulse, |
| | | 32 | | bool applyPositionCorrection) |
| | | 33 | | { |
| | 995 | 34 | | if (!TryCreateBodyPair(pair, out ResponseBody2D bodyA, out ResponseBody2D bodyB)) |
| | 5 | 35 | | return; |
| | | 36 | | |
| | 990 | 37 | | ContactAnchor2D responseCenterA = bodyA.Body?.GetCenterOfMassAnchor() ?? default; |
| | 990 | 38 | | ContactAnchor2D responseCenterB = bodyB.Body?.GetCenterOfMassAnchor() ?? default; |
| | 990 | 39 | | SolverContactBuffer2D contacts = BuildContactBuffer( |
| | 990 | 40 | | pair, |
| | 990 | 41 | | bodyA, |
| | 990 | 42 | | bodyB, |
| | 990 | 43 | | responseCenterA, |
| | 990 | 44 | | responseCenterB); |
| | 990 | 45 | | if (contacts.Count == 0) |
| | 1 | 46 | | return; |
| | | 47 | | |
| | 989 | 48 | | Vector2d responsePositionA = bodyA.Body?.Position ?? Vector2d.Zero; |
| | 989 | 49 | | Vector2d responsePositionB = bodyB.Body?.Position ?? Vector2d.Zero; |
| | 989 | 50 | | byte failedResponseMask = 0; |
| | 989 | 51 | | Fixed64 contactShare = Fixed64.One / (Fixed64)contacts.Count; |
| | 989 | 52 | | if (applyPositionCorrection) |
| | | 53 | | { |
| | 2140 | 54 | | for (int i = 0; i < contacts.Count; i++) |
| | 539 | 55 | | ApplyPositionCorrection(contacts.GetContact(i), contactShare); |
| | | 56 | | } |
| | | 57 | | |
| | 989 | 58 | | if (applyCachedImpulse) |
| | | 59 | | { |
| | 536 | 60 | | bool rebuildContacts = false; |
| | 2162 | 61 | | for (int i = 0; i < contacts.Count; i++) |
| | | 62 | | { |
| | 545 | 63 | | SolverContact2D contact = contacts.GetContact(i); |
| | 545 | 64 | | if (TryApplyCachedImpulse( |
| | 545 | 65 | | pair, |
| | 545 | 66 | | contact, |
| | 545 | 67 | | responsePositionA, |
| | 545 | 68 | | responsePositionB)) |
| | | 69 | | { |
| | | 70 | | continue; |
| | | 71 | | } |
| | | 72 | | |
| | 2 | 73 | | pair.RemoveWarmStartImpulse(contact.ContactId); |
| | 2 | 74 | | rebuildContacts = true; |
| | | 75 | | } |
| | | 76 | | |
| | 536 | 77 | | if (rebuildContacts) |
| | | 78 | | { |
| | 2 | 79 | | contacts = BuildContactBuffer( |
| | 2 | 80 | | pair, |
| | 2 | 81 | | bodyA, |
| | 2 | 82 | | bodyB, |
| | 2 | 83 | | responseCenterA, |
| | 2 | 84 | | responseCenterB); |
| | | 85 | | } |
| | | 86 | | } |
| | | 87 | | |
| | 989 | 88 | | Vector2d normalLinearVelocityA = |
| | 989 | 89 | | ResolveLinearVelocity(bodyA.Body); |
| | 989 | 90 | | Fixed64 normalAngularVelocityA = |
| | 989 | 91 | | ResolveAngularVelocity(bodyA.Body); |
| | 989 | 92 | | Vector2d normalLinearVelocityB = |
| | 989 | 93 | | ResolveLinearVelocity(bodyB.Body); |
| | 989 | 94 | | Fixed64 normalAngularVelocityB = |
| | 989 | 95 | | ResolveAngularVelocity(bodyB.Body); |
| | 989 | 96 | | Fixed64 restitutionVelocityThreshold = pair.ColliderA.Context.Settings.RestitutionVelocityThreshold; |
| | 3994 | 97 | | for (int i = 0; i < contacts.Count; i++) |
| | | 98 | | { |
| | 1008 | 99 | | SolverContact2D contact = contacts.GetContact(i); |
| | 1008 | 100 | | SolidBody2D? contactBodyA = contact.A.Body; |
| | 1008 | 101 | | SolidBody2D? contactBodyB = contact.B.Body; |
| | | 102 | | bool normalResolved; |
| | 1008 | 103 | | ContactNormalImpulseResult2D normalResult = default; |
| | 1008 | 104 | | if (contact.RelativeA.IsExact || contact.RelativeB.IsExact) |
| | | 105 | | { |
| | 19 | 106 | | normalResolved = TryCalculateExactNormalResult( |
| | 19 | 107 | | pair, |
| | 19 | 108 | | contact, |
| | 19 | 109 | | responsePositionA, |
| | 19 | 110 | | responsePositionB, |
| | 19 | 111 | | normalLinearVelocityA, |
| | 19 | 112 | | normalAngularVelocityA, |
| | 19 | 113 | | normalLinearVelocityB, |
| | 19 | 114 | | normalAngularVelocityB, |
| | 19 | 115 | | restitutionVelocityThreshold, |
| | 19 | 116 | | contactShare, |
| | 19 | 117 | | out normalResult); |
| | | 118 | | } |
| | | 119 | | else |
| | | 120 | | { |
| | 989 | 121 | | normalResolved = CanUseCompactAxisResponse( |
| | 989 | 122 | | contact, |
| | 989 | 123 | | contact.Normal) |
| | 989 | 124 | | && ContactNormalImpulse2D.TryCalculateAccumulatedDelta( |
| | 989 | 125 | | contactBodyA, |
| | 989 | 126 | | normalLinearVelocityA, |
| | 989 | 127 | | normalAngularVelocityA, |
| | 989 | 128 | | contact.RelativeA.Vector, |
| | 989 | 129 | | contactBodyB, |
| | 989 | 130 | | normalLinearVelocityB, |
| | 989 | 131 | | normalAngularVelocityB, |
| | 989 | 132 | | contact.RelativeB.Vector, |
| | 989 | 133 | | contact.Normal, |
| | 989 | 134 | | contact.Restitution, |
| | 989 | 135 | | restitutionVelocityThreshold, |
| | 989 | 136 | | contact.CachedNormalImpulse, |
| | 989 | 137 | | contactShare, |
| | 989 | 138 | | contactShare, |
| | 989 | 139 | | out normalResult); |
| | 989 | 140 | | if (!normalResolved) |
| | | 141 | | { |
| | 17 | 142 | | normalResolved = TryCalculateExactNormalResult( |
| | 17 | 143 | | pair, |
| | 17 | 144 | | contact, |
| | 17 | 145 | | responsePositionA, |
| | 17 | 146 | | responsePositionB, |
| | 17 | 147 | | normalLinearVelocityA, |
| | 17 | 148 | | normalAngularVelocityA, |
| | 17 | 149 | | normalLinearVelocityB, |
| | 17 | 150 | | normalAngularVelocityB, |
| | 17 | 151 | | restitutionVelocityThreshold, |
| | 17 | 152 | | contactShare, |
| | 17 | 153 | | out normalResult); |
| | | 154 | | } |
| | | 155 | | } |
| | 1008 | 156 | | if (!normalResolved) |
| | | 157 | | { |
| | 2 | 158 | | RejectResponse(pair, contact, i, ref failedResponseMask); |
| | 2 | 159 | | continue; |
| | | 160 | | } |
| | 1006 | 161 | | Fixed64 normalImpulse = contact.CachedNormalImpulse + normalResult.ImpulseScalar; |
| | 1006 | 162 | | contacts.SetNormalImpulse( |
| | 1006 | 163 | | i, |
| | 1006 | 164 | | normalImpulse, |
| | 1006 | 165 | | normalResult); |
| | | 166 | | } |
| | | 167 | | |
| | 3994 | 168 | | for (int i = 0; i < contacts.Count; i++) |
| | | 169 | | { |
| | 1008 | 170 | | if (HasFailedResponse(failedResponseMask, i)) |
| | | 171 | | continue; |
| | | 172 | | |
| | 1006 | 173 | | if (!TryApplyNormalImpulse( |
| | 1006 | 174 | | contacts.GetContact(i), |
| | 1006 | 175 | | contacts.GetNormalResult(i))) |
| | | 176 | | { |
| | 1 | 177 | | RejectResponse( |
| | 1 | 178 | | pair, |
| | 1 | 179 | | contacts.GetContact(i), |
| | 1 | 180 | | i, |
| | 1 | 181 | | ref failedResponseMask); |
| | | 182 | | } |
| | | 183 | | } |
| | | 184 | | |
| | 3994 | 185 | | for (int i = 0; i < contacts.Count; i++) |
| | | 186 | | { |
| | 1008 | 187 | | if (HasFailedResponse(failedResponseMask, i)) |
| | | 188 | | continue; |
| | | 189 | | |
| | 1005 | 190 | | SolverContact2D contact = contacts.GetContact(i); |
| | 1005 | 191 | | if (!TrySolveFrictionImpulse( |
| | 1005 | 192 | | pair, |
| | 1005 | 193 | | contact, |
| | 1005 | 194 | | responsePositionA, |
| | 1005 | 195 | | responsePositionB, |
| | 1005 | 196 | | normalLinearVelocityA, |
| | 1005 | 197 | | normalAngularVelocityA, |
| | 1005 | 198 | | normalLinearVelocityB, |
| | 1005 | 199 | | normalAngularVelocityB, |
| | 1005 | 200 | | restitutionVelocityThreshold, |
| | 1005 | 201 | | contactShare, |
| | 1005 | 202 | | contacts.GetNormalResult(i), |
| | 1005 | 203 | | contacts.GetNormalImpulse(i), |
| | 1005 | 204 | | out Fixed64 tangentImpulse)) |
| | | 205 | | { |
| | 2 | 206 | | RejectResponse(pair, contact, i, ref failedResponseMask); |
| | 2 | 207 | | continue; |
| | | 208 | | } |
| | 1003 | 209 | | contacts.SetTangentImpulse( |
| | 1003 | 210 | | i, |
| | 1003 | 211 | | tangentImpulse); |
| | | 212 | | } |
| | | 213 | | |
| | 3994 | 214 | | for (int i = 0; i < contacts.Count; i++) |
| | | 215 | | { |
| | 1008 | 216 | | if (HasFailedResponse(failedResponseMask, i)) |
| | | 217 | | continue; |
| | | 218 | | |
| | 1003 | 219 | | SolverContact2D contact = contacts.GetContact(i); |
| | 1003 | 220 | | pair.StoreWarmStartImpulse( |
| | 1003 | 221 | | contact.ContactId, |
| | 1003 | 222 | | contacts.GetNormalImpulse(i), |
| | 1003 | 223 | | contacts.GetTangentImpulse(i)); |
| | | 224 | | } |
| | 989 | 225 | | } |
| | | 226 | | |
| | | 227 | | private static bool TryCreateBodyPair( |
| | | 228 | | CollisionPair2D pair, |
| | | 229 | | out ResponseBody2D bodyA, |
| | | 230 | | out ResponseBody2D bodyB) |
| | | 231 | | { |
| | 995 | 232 | | bodyA = default; |
| | 995 | 233 | | bodyB = default; |
| | | 234 | | |
| | 995 | 235 | | if (pair.ColliderA.IsTrigger || pair.ColliderB.IsTrigger || !pair.Manifold.HasContact) |
| | 3 | 236 | | return false; |
| | | 237 | | |
| | 992 | 238 | | bodyA = ResponseBody2D.Create(pair.ColliderA); |
| | 992 | 239 | | bodyB = ResponseBody2D.Create(pair.ColliderB); |
| | 992 | 240 | | return bodyA.HasSolverMobility || bodyB.HasSolverMobility; |
| | | 241 | | } |
| | | 242 | | |
| | | 243 | | private static SolverContactBuffer2D BuildContactBuffer( |
| | | 244 | | CollisionPair2D pair, |
| | | 245 | | ResponseBody2D bodyA, |
| | | 246 | | ResponseBody2D bodyB, |
| | | 247 | | in ContactAnchor2D responseCenterA, |
| | | 248 | | in ContactAnchor2D responseCenterB) |
| | | 249 | | { |
| | 992 | 250 | | SolverContactBuffer2D contacts = default; |
| | 4008 | 251 | | for (int i = 0; i < pair.Manifold.Count; i++) |
| | | 252 | | { |
| | 1012 | 253 | | if (TryCreateContact( |
| | 1012 | 254 | | pair, |
| | 1012 | 255 | | bodyA, |
| | 1012 | 256 | | bodyB, |
| | 1012 | 257 | | responseCenterA, |
| | 1012 | 258 | | responseCenterB, |
| | 1012 | 259 | | i, |
| | 1012 | 260 | | out SolverContact2D contact)) |
| | 1011 | 261 | | contacts.Add(contact); |
| | | 262 | | } |
| | | 263 | | |
| | 992 | 264 | | return contacts; |
| | | 265 | | } |
| | | 266 | | |
| | | 267 | | private static bool TryCreateContact( |
| | | 268 | | CollisionPair2D pair, |
| | | 269 | | ResponseBody2D bodyA, |
| | | 270 | | ResponseBody2D bodyB, |
| | | 271 | | in ContactAnchor2D responseCenterA, |
| | | 272 | | in ContactAnchor2D responseCenterB, |
| | | 273 | | int contactIndex, |
| | | 274 | | out SolverContact2D contact) |
| | | 275 | | { |
| | 1012 | 276 | | contact = default; |
| | 1012 | 277 | | ManifoldContact2D manifoldContact = pair.Manifold[contactIndex]; |
| | 1012 | 278 | | Vector2d normal = ResolveContactNormal( |
| | 1012 | 279 | | manifoldContact.Normal, |
| | 1012 | 280 | | pair.ColliderB.Center - pair.ColliderA.Center); |
| | 1012 | 281 | | if (normal == Vector2d.Zero) |
| | 1 | 282 | | return false; |
| | 1011 | 283 | | ContactLever2D relativeA = bodyA.Body == null |
| | 1011 | 284 | | ? ContactLever2D.Zero |
| | 1011 | 285 | | : ContactLever2D.Create( |
| | 1011 | 286 | | manifoldContact.AnchorA, |
| | 1011 | 287 | | responseCenterA); |
| | 1011 | 288 | | ContactLever2D relativeB = bodyB.Body == null |
| | 1011 | 289 | | ? ContactLever2D.Zero |
| | 1011 | 290 | | : ContactLever2D.Create( |
| | 1011 | 291 | | manifoldContact.AnchorB, |
| | 1011 | 292 | | responseCenterB); |
| | 1011 | 293 | | PhysicsMaterial materialA = manifoldContact.HasMaterialOverride |
| | 1011 | 294 | | ? manifoldContact.MaterialA |
| | 1011 | 295 | | : pair.ColliderA.Material; |
| | 1011 | 296 | | PhysicsMaterial materialB = manifoldContact.HasMaterialOverride |
| | 1011 | 297 | | ? manifoldContact.MaterialB |
| | 1011 | 298 | | : pair.ColliderB.Material; |
| | | 299 | | |
| | 1011 | 300 | | Fixed64 cachedNormalImpulse = Fixed64.Zero; |
| | 1011 | 301 | | Fixed64 cachedTangentImpulse = Fixed64.Zero; |
| | 1011 | 302 | | if (pair.TryGetWarmStartImpulse(manifoldContact.ContactId, out ContactWarmStartImpulse cached)) |
| | | 303 | | { |
| | 717 | 304 | | cachedNormalImpulse = cached.NormalImpulse; |
| | 717 | 305 | | cachedTangentImpulse = cached.TangentImpulse; |
| | | 306 | | } |
| | | 307 | | |
| | 1011 | 308 | | contact = new SolverContact2D( |
| | 1011 | 309 | | contactIndex, |
| | 1011 | 310 | | manifoldContact.ContactId, |
| | 1011 | 311 | | bodyA, |
| | 1011 | 312 | | bodyB, |
| | 1011 | 313 | | relativeA, |
| | 1011 | 314 | | relativeB, |
| | 1011 | 315 | | manifoldContact.Depth, |
| | 1011 | 316 | | normal, |
| | 1011 | 317 | | materialA, |
| | 1011 | 318 | | materialB, |
| | 1011 | 319 | | cachedNormalImpulse, |
| | 1011 | 320 | | cachedTangentImpulse); |
| | 1011 | 321 | | return true; |
| | | 322 | | } |
| | | 323 | | |
| | | 324 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 325 | | private static bool TryCalculateExactNormalResult( |
| | | 326 | | CollisionPair2D pair, |
| | | 327 | | SolverContact2D contact, |
| | | 328 | | Vector2d responsePositionA, |
| | | 329 | | Vector2d responsePositionB, |
| | | 330 | | Vector2d linearVelocityA, |
| | | 331 | | Fixed64 angularVelocityA, |
| | | 332 | | Vector2d linearVelocityB, |
| | | 333 | | Fixed64 angularVelocityB, |
| | | 334 | | Fixed64 restitutionVelocityThreshold, |
| | | 335 | | Fixed64 contactShare, |
| | | 336 | | out ContactNormalImpulseResult2D result) |
| | | 337 | | { |
| | 36 | 338 | | GetExactLevers( |
| | 36 | 339 | | pair, |
| | 36 | 340 | | contact, |
| | 36 | 341 | | responsePositionA, |
| | 36 | 342 | | responsePositionB, |
| | 36 | 343 | | out ExactLever3D exactA, |
| | 36 | 344 | | out ExactLever3D exactB); |
| | 36 | 345 | | return ContactNormalImpulse2D.TryCalculateAccumulatedDeltaExact( |
| | 36 | 346 | | contact.A.Body, |
| | 36 | 347 | | linearVelocityA, |
| | 36 | 348 | | angularVelocityA, |
| | 36 | 349 | | exactA, |
| | 36 | 350 | | contact.B.Body, |
| | 36 | 351 | | linearVelocityB, |
| | 36 | 352 | | angularVelocityB, |
| | 36 | 353 | | exactB, |
| | 36 | 354 | | contact.Normal, |
| | 36 | 355 | | contact.Restitution, |
| | 36 | 356 | | restitutionVelocityThreshold, |
| | 36 | 357 | | contact.CachedNormalImpulse, |
| | 36 | 358 | | contactShare, |
| | 36 | 359 | | contactShare, |
| | 36 | 360 | | out result); |
| | | 361 | | } |
| | | 362 | | |
| | | 363 | | private static void ApplyPositionCorrection(SolverContact2D contact, Fixed64 contactShare) |
| | | 364 | | { |
| | 539 | 365 | | Fixed64 correctionDepth = contact.Depth - PenetrationSlop; |
| | 539 | 366 | | if (correctionDepth <= Fixed64.Zero) |
| | 101 | 367 | | return; |
| | | 368 | | |
| | 438 | 369 | | Fixed64 inverseMassA = contact.A.GetConstrainedInverseMass(contact.Normal); |
| | 438 | 370 | | Fixed64 inverseMassB = contact.B.GetConstrainedInverseMass(contact.Normal); |
| | 438 | 371 | | Fixed64 totalInverseMass = inverseMassA + inverseMassB; |
| | 438 | 372 | | if (totalInverseMass <= Fixed64.Zero) |
| | 1 | 373 | | return; |
| | | 374 | | |
| | 437 | 375 | | Vector2d correction = contact.Normal |
| | 437 | 376 | | * (correctionDepth * PenetrationCorrectionPercent * contactShare / totalInverseMass); |
| | 437 | 377 | | ApplyPositionCorrection(contact.A, -correction * inverseMassA); |
| | 437 | 378 | | ApplyPositionCorrection(contact.B, correction * inverseMassB); |
| | 437 | 379 | | } |
| | | 380 | | |
| | | 381 | | private static bool TryApplyCachedImpulse( |
| | | 382 | | CollisionPair2D pair, |
| | | 383 | | SolverContact2D contact, |
| | | 384 | | Vector2d responsePositionA, |
| | | 385 | | Vector2d responsePositionB) |
| | | 386 | | { |
| | 545 | 387 | | if (contact.CachedNormalImpulse == Fixed64.Zero && contact.CachedTangentImpulse == Fixed64.Zero) |
| | 537 | 388 | | return true; |
| | | 389 | | |
| | 8 | 390 | | return TryApplyContactImpulseCombination( |
| | 8 | 391 | | pair, |
| | 8 | 392 | | contact, |
| | 8 | 393 | | responsePositionA, |
| | 8 | 394 | | responsePositionB, |
| | 8 | 395 | | contact.Normal, |
| | 8 | 396 | | contact.CachedNormalImpulse, |
| | 8 | 397 | | contact.Tangent, |
| | 8 | 398 | | contact.CachedTangentImpulse); |
| | | 399 | | } |
| | | 400 | | |
| | | 401 | | private static bool TryApplyNormalImpulse( |
| | | 402 | | SolverContact2D contact, |
| | | 403 | | ContactNormalImpulseResult2D result) |
| | | 404 | | { |
| | 1006 | 405 | | if (result.LinearVelocityDeltaA == Vector2d.Zero |
| | 1006 | 406 | | && result.AngularVelocityDeltaA == Fixed64.Zero |
| | 1006 | 407 | | && result.LinearVelocityDeltaB == Vector2d.Zero |
| | 1006 | 408 | | && result.AngularVelocityDeltaB == Fixed64.Zero) |
| | | 409 | | { |
| | 926 | 410 | | return true; |
| | | 411 | | } |
| | | 412 | | |
| | 80 | 413 | | return TryApplyVelocityDeltas( |
| | 80 | 414 | | contact, |
| | 80 | 415 | | result.LinearVelocityDeltaA, |
| | 80 | 416 | | result.AngularVelocityDeltaA, |
| | 80 | 417 | | result.LinearVelocityDeltaB, |
| | 80 | 418 | | result.AngularVelocityDeltaB); |
| | | 419 | | } |
| | | 420 | | |
| | | 421 | | private static void ApplyVelocityDelta( |
| | | 422 | | ResponseBody2D body, |
| | | 423 | | Vector2d linearVelocityDelta, |
| | | 424 | | Fixed64 angularVelocityDelta) |
| | | 425 | | { |
| | 236 | 426 | | if (!body.HasSolverMobility) |
| | 64 | 427 | | return; |
| | | 428 | | |
| | 172 | 429 | | if (body.CanTranslate) |
| | 168 | 430 | | body.Body!.ApplyCollisionLinearVelocityDelta(linearVelocityDelta); |
| | 172 | 431 | | if (body.CanRotate) |
| | 136 | 432 | | body.Body!.ApplyCollisionAngularVelocityDelta(angularVelocityDelta); |
| | 172 | 433 | | } |
| | | 434 | | |
| | | 435 | | private static bool TrySolveFrictionImpulse( |
| | | 436 | | CollisionPair2D pair, |
| | | 437 | | SolverContact2D contact, |
| | | 438 | | Vector2d responsePositionA, |
| | | 439 | | Vector2d responsePositionB, |
| | | 440 | | Vector2d normalLinearVelocityA, |
| | | 441 | | Fixed64 normalAngularVelocityA, |
| | | 442 | | Vector2d normalLinearVelocityB, |
| | | 443 | | Fixed64 normalAngularVelocityB, |
| | | 444 | | Fixed64 restitutionVelocityThreshold, |
| | | 445 | | Fixed64 contactShare, |
| | | 446 | | ContactNormalImpulseResult2D normalResult, |
| | | 447 | | Fixed64 normalImpulseScalar, |
| | | 448 | | out Fixed64 accumulated) |
| | | 449 | | { |
| | 1005 | 450 | | accumulated = default; |
| | 1005 | 451 | | if (contact.RelativeA.IsExact |
| | 1005 | 452 | | || contact.RelativeB.IsExact |
| | 1005 | 453 | | || !normalResult.HasRepresentableAccumulatedImpulse |
| | 1005 | 454 | | || !CanUseCompactAxisResponse(contact, contact.Tangent) |
| | 1005 | 455 | | || !Fixed64.TryMultiplyDivide( |
| | 1005 | 456 | | normalImpulseScalar, |
| | 1005 | 457 | | contact.StaticFriction, |
| | 1005 | 458 | | Fixed64.One, |
| | 1005 | 459 | | out Fixed64 staticFrictionLimit) |
| | 1005 | 460 | | || !Fixed64.TryMultiplyDivide( |
| | 1005 | 461 | | normalImpulseScalar, |
| | 1005 | 462 | | contact.DynamicFriction, |
| | 1005 | 463 | | Fixed64.One, |
| | 1005 | 464 | | out Fixed64 dynamicFrictionLimit)) |
| | | 465 | | { |
| | 24 | 466 | | return TrySolveFrictionImpulseExact( |
| | 24 | 467 | | pair, |
| | 24 | 468 | | contact, |
| | 24 | 469 | | responsePositionA, |
| | 24 | 470 | | responsePositionB, |
| | 24 | 471 | | normalLinearVelocityA, |
| | 24 | 472 | | normalAngularVelocityA, |
| | 24 | 473 | | normalLinearVelocityB, |
| | 24 | 474 | | normalAngularVelocityB, |
| | 24 | 475 | | restitutionVelocityThreshold, |
| | 24 | 476 | | contactShare, |
| | 24 | 477 | | out accumulated); |
| | | 478 | | } |
| | | 479 | | |
| | 981 | 480 | | Fixed64 impulseScalar = Fixed64.Zero; |
| | 981 | 481 | | if (staticFrictionLimit > Fixed64.Zero || dynamicFrictionLimit > Fixed64.Zero) |
| | | 482 | | { |
| | 55 | 483 | | Vector3d spatialTangent = |
| | 55 | 484 | | ExactContactLever2D.ToSpatial(contact.Tangent); |
| | 55 | 485 | | Fixed64 tangentVelocity = default; |
| | 55 | 486 | | bool tangentVelocityResolved = |
| | 55 | 487 | | ContactResponseArithmetic3D.TryGetRelativePointVelocity( |
| | 55 | 488 | | ExactContactLever2D.ToSpatial( |
| | 55 | 489 | | ResolveLinearVelocity(contact.A.Body)), |
| | 55 | 490 | | new Vector3d( |
| | 55 | 491 | | Fixed64.Zero, |
| | 55 | 492 | | -ResolveAngularVelocity(contact.A.Body), |
| | 55 | 493 | | Fixed64.Zero), |
| | 55 | 494 | | ExactContactLever2D.ToSpatial(contact.RelativeA.Vector), |
| | 55 | 495 | | ExactContactLever2D.ToSpatial( |
| | 55 | 496 | | ResolveLinearVelocity(contact.B.Body)), |
| | 55 | 497 | | new Vector3d( |
| | 55 | 498 | | Fixed64.Zero, |
| | 55 | 499 | | -ResolveAngularVelocity(contact.B.Body), |
| | 55 | 500 | | Fixed64.Zero), |
| | 55 | 501 | | ExactContactLever2D.ToSpatial(contact.RelativeB.Vector), |
| | 55 | 502 | | spatialTangent, |
| | 55 | 503 | | out Vector3d relativeVelocity) |
| | 55 | 504 | | && ContactResponseArithmetic3D.TryDot( |
| | 55 | 505 | | relativeVelocity, |
| | 55 | 506 | | spatialTangent, |
| | 55 | 507 | | out tangentVelocity); |
| | 55 | 508 | | bool denominatorResolved = |
| | 55 | 509 | | ContactNormalImpulse2D.TryComputeDenominator( |
| | 55 | 510 | | contact.A.Body, |
| | 55 | 511 | | contact.RelativeA.Vector, |
| | 55 | 512 | | contact.B.Body, |
| | 55 | 513 | | contact.RelativeB.Vector, |
| | 55 | 514 | | contact.Tangent, |
| | 55 | 515 | | out Fixed64 denominator); |
| | 55 | 516 | | if (!(tangentVelocityResolved & denominatorResolved)) |
| | | 517 | | { |
| | 3 | 518 | | return TrySolveFrictionImpulseExact( |
| | 3 | 519 | | pair, |
| | 3 | 520 | | contact, |
| | 3 | 521 | | responsePositionA, |
| | 3 | 522 | | responsePositionB, |
| | 3 | 523 | | normalLinearVelocityA, |
| | 3 | 524 | | normalAngularVelocityA, |
| | 3 | 525 | | normalLinearVelocityB, |
| | 3 | 526 | | normalAngularVelocityB, |
| | 3 | 527 | | restitutionVelocityThreshold, |
| | 3 | 528 | | contactShare, |
| | 3 | 529 | | out accumulated); |
| | | 530 | | } |
| | 52 | 531 | | if (tangentVelocity.Abs() > Fixed64.Epsilon |
| | 52 | 532 | | && denominator > Fixed64.Zero |
| | 52 | 533 | | && !Fixed64.TryMultiplyDivide( |
| | 52 | 534 | | -tangentVelocity, |
| | 52 | 535 | | Fixed64.One, |
| | 52 | 536 | | denominator, |
| | 52 | 537 | | out impulseScalar)) |
| | | 538 | | { |
| | 1 | 539 | | return TrySolveFrictionImpulseExact( |
| | 1 | 540 | | pair, |
| | 1 | 541 | | contact, |
| | 1 | 542 | | responsePositionA, |
| | 1 | 543 | | responsePositionB, |
| | 1 | 544 | | normalLinearVelocityA, |
| | 1 | 545 | | normalAngularVelocityA, |
| | 1 | 546 | | normalLinearVelocityB, |
| | 1 | 547 | | normalAngularVelocityB, |
| | 1 | 548 | | restitutionVelocityThreshold, |
| | 1 | 549 | | contactShare, |
| | 1 | 550 | | out accumulated); |
| | | 551 | | } |
| | | 552 | | } |
| | | 553 | | |
| | 977 | 554 | | if (!Fixed64.TryAdd( |
| | 977 | 555 | | contact.CachedTangentImpulse, |
| | 977 | 556 | | impulseScalar, |
| | 977 | 557 | | out Fixed64 desiredAccumulated)) |
| | | 558 | | { |
| | 2 | 559 | | return TrySolveFrictionImpulseExact( |
| | 2 | 560 | | pair, |
| | 2 | 561 | | contact, |
| | 2 | 562 | | responsePositionA, |
| | 2 | 563 | | responsePositionB, |
| | 2 | 564 | | normalLinearVelocityA, |
| | 2 | 565 | | normalAngularVelocityA, |
| | 2 | 566 | | normalLinearVelocityB, |
| | 2 | 567 | | normalAngularVelocityB, |
| | 2 | 568 | | restitutionVelocityThreshold, |
| | 2 | 569 | | contactShare, |
| | 2 | 570 | | out accumulated); |
| | | 571 | | } |
| | 975 | 572 | | accumulated = desiredAccumulated >= -staticFrictionLimit |
| | 975 | 573 | | && desiredAccumulated <= staticFrictionLimit |
| | 975 | 574 | | ? desiredAccumulated |
| | 975 | 575 | | : FixedMath.Clamp( |
| | 975 | 576 | | desiredAccumulated, |
| | 975 | 577 | | -dynamicFrictionLimit, |
| | 975 | 578 | | dynamicFrictionLimit); |
| | 975 | 579 | | if (!Fixed64.TrySubtract( |
| | 975 | 580 | | accumulated, |
| | 975 | 581 | | contact.CachedTangentImpulse, |
| | 975 | 582 | | out impulseScalar)) |
| | | 583 | | { |
| | 1 | 584 | | return TrySolveFrictionImpulseExact( |
| | 1 | 585 | | pair, |
| | 1 | 586 | | contact, |
| | 1 | 587 | | responsePositionA, |
| | 1 | 588 | | responsePositionB, |
| | 1 | 589 | | normalLinearVelocityA, |
| | 1 | 590 | | normalAngularVelocityA, |
| | 1 | 591 | | normalLinearVelocityB, |
| | 1 | 592 | | normalAngularVelocityB, |
| | 1 | 593 | | restitutionVelocityThreshold, |
| | 1 | 594 | | contactShare, |
| | 1 | 595 | | out accumulated); |
| | | 596 | | } |
| | 974 | 597 | | return impulseScalar == Fixed64.Zero |
| | 974 | 598 | | || TryApplyContactImpulseCombination( |
| | 974 | 599 | | pair, |
| | 974 | 600 | | contact, |
| | 974 | 601 | | responsePositionA, |
| | 974 | 602 | | responsePositionB, |
| | 974 | 603 | | contact.Normal, |
| | 974 | 604 | | Fixed64.Zero, |
| | 974 | 605 | | contact.Tangent, |
| | 974 | 606 | | impulseScalar); |
| | | 607 | | } |
| | | 608 | | |
| | | 609 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 610 | | private static bool TrySolveFrictionImpulseExact( |
| | | 611 | | CollisionPair2D pair, |
| | | 612 | | SolverContact2D contact, |
| | | 613 | | Vector2d responsePositionA, |
| | | 614 | | Vector2d responsePositionB, |
| | | 615 | | Vector2d normalLinearVelocityA, |
| | | 616 | | Fixed64 normalAngularVelocityA, |
| | | 617 | | Vector2d normalLinearVelocityB, |
| | | 618 | | Fixed64 normalAngularVelocityB, |
| | | 619 | | Fixed64 restitutionVelocityThreshold, |
| | | 620 | | Fixed64 contactShare, |
| | | 621 | | out Fixed64 accumulated) |
| | | 622 | | { |
| | 31 | 623 | | accumulated = default; |
| | 31 | 624 | | GetExactLevers( |
| | 31 | 625 | | pair, |
| | 31 | 626 | | contact, |
| | 31 | 627 | | responsePositionA, |
| | 31 | 628 | | responsePositionB, |
| | 31 | 629 | | out ExactLever3D exactA, |
| | 31 | 630 | | out ExactLever3D exactB); |
| | 31 | 631 | | Vector3d spatialNormal = |
| | 31 | 632 | | ExactContactLever2D.ToSpatial(contact.Normal); |
| | 31 | 633 | | var normalConstraint = new ExactNormalConstraint3D( |
| | 31 | 634 | | ExactContactLever2D.CreateResponseOperand( |
| | 31 | 635 | | contact.A.Body, |
| | 31 | 636 | | normalLinearVelocityA, |
| | 31 | 637 | | normalAngularVelocityA, |
| | 31 | 638 | | exactA, |
| | 31 | 639 | | -spatialNormal), |
| | 31 | 640 | | ExactContactLever2D.CreateResponseOperand( |
| | 31 | 641 | | contact.B.Body, |
| | 31 | 642 | | normalLinearVelocityB, |
| | 31 | 643 | | normalAngularVelocityB, |
| | 31 | 644 | | exactB, |
| | 31 | 645 | | spatialNormal), |
| | 31 | 646 | | spatialNormal, |
| | 31 | 647 | | contact.Restitution, |
| | 31 | 648 | | restitutionVelocityThreshold, |
| | 31 | 649 | | contact.CachedNormalImpulse, |
| | 31 | 650 | | contactShare, |
| | 31 | 651 | | contactShare); |
| | 31 | 652 | | Vector3d spatialTangent = |
| | 31 | 653 | | ExactContactLever2D.ToSpatial(contact.Tangent); |
| | 31 | 654 | | if (!ExactContactResponseKernel.TryGetCoulombLineResponse( |
| | 31 | 655 | | normalConstraint, |
| | 31 | 656 | | ExactContactLever2D.CreateResponseOperand( |
| | 31 | 657 | | contact.A.Body, |
| | 31 | 658 | | ResolveLinearVelocity(contact.A.Body), |
| | 31 | 659 | | ResolveAngularVelocity(contact.A.Body), |
| | 31 | 660 | | exactA, |
| | 31 | 661 | | -spatialTangent), |
| | 31 | 662 | | ExactContactLever2D.CreateResponseOperand( |
| | 31 | 663 | | contact.B.Body, |
| | 31 | 664 | | ResolveLinearVelocity(contact.B.Body), |
| | 31 | 665 | | ResolveAngularVelocity(contact.B.Body), |
| | 31 | 666 | | exactB, |
| | 31 | 667 | | spatialTangent), |
| | 31 | 668 | | spatialTangent, |
| | 31 | 669 | | contact.CachedTangentImpulse, |
| | 31 | 670 | | contact.StaticFriction, |
| | 31 | 671 | | contact.DynamicFriction, |
| | 31 | 672 | | out ExactCoulombResponse3D response)) |
| | | 673 | | { |
| | 2 | 674 | | return false; |
| | | 675 | | } |
| | | 676 | | |
| | 29 | 677 | | accumulated = response.TryGetPrimaryAccumulatedImpulse( |
| | 29 | 678 | | out Fixed64 projectedAccumulated) |
| | 29 | 679 | | ? projectedAccumulated |
| | 29 | 680 | | : Fixed64.Zero; |
| | 29 | 681 | | return !response.HasAppliedImpulse |
| | 29 | 682 | | || TryApplyVelocityDeltas( |
| | 29 | 683 | | contact, |
| | 29 | 684 | | ExactContactLever2D.ToPlanar( |
| | 29 | 685 | | response.FirstLinearVelocityDelta), |
| | 29 | 686 | | ExactContactLever2D.ToPlanarAngular( |
| | 29 | 687 | | response.FirstAngularVelocityDelta), |
| | 29 | 688 | | ExactContactLever2D.ToPlanar( |
| | 29 | 689 | | response.SecondLinearVelocityDelta), |
| | 29 | 690 | | ExactContactLever2D.ToPlanarAngular( |
| | 29 | 691 | | response.SecondAngularVelocityDelta)); |
| | | 692 | | } |
| | | 693 | | |
| | | 694 | | private static bool CanUseCompactAxisResponse( |
| | | 695 | | SolverContact2D contact, |
| | | 696 | | Vector2d axis) => |
| | 1973 | 697 | | ExactContactLever2D.CanUseCompactResponse( |
| | 1973 | 698 | | contact.A.Body, |
| | 1973 | 699 | | ResolveLinearVelocity(contact.A.Body), |
| | 1973 | 700 | | ResolveAngularVelocity(contact.A.Body), |
| | 1973 | 701 | | contact.RelativeA.Vector, |
| | 1973 | 702 | | contact.B.Body, |
| | 1973 | 703 | | ResolveLinearVelocity(contact.B.Body), |
| | 1973 | 704 | | ResolveAngularVelocity(contact.B.Body), |
| | 1973 | 705 | | contact.RelativeB.Vector, |
| | 1973 | 706 | | axis); |
| | | 707 | | |
| | | 708 | | private static void ApplyPositionCorrection(ResponseBody2D body, Vector2d correction) |
| | | 709 | | { |
| | 874 | 710 | | if (!body.CanTranslate || correction == Vector2d.Zero) |
| | 421 | 711 | | return; |
| | | 712 | | |
| | 453 | 713 | | body.Body!.ApplyCollisionPositionCorrection(correction); |
| | 453 | 714 | | } |
| | | 715 | | |
| | | 716 | | private static bool TryApplyContactImpulseCombination( |
| | | 717 | | CollisionPair2D pair, |
| | | 718 | | SolverContact2D contact, |
| | | 719 | | Vector2d responsePositionA, |
| | | 720 | | Vector2d responsePositionB, |
| | | 721 | | Vector2d firstAxis, |
| | | 722 | | Fixed64 firstScale, |
| | | 723 | | Vector2d secondAxis, |
| | | 724 | | Fixed64 secondScale) |
| | | 725 | | { |
| | 20 | 726 | | if (!contact.RelativeA.IsExact |
| | 20 | 727 | | && !contact.RelativeB.IsExact |
| | 20 | 728 | | && Vector3d.TryLinearCombination( |
| | 20 | 729 | | ExactContactLever2D.ToSpatial(firstAxis), |
| | 20 | 730 | | firstScale, |
| | 20 | 731 | | ExactContactLever2D.ToSpatial(secondAxis), |
| | 20 | 732 | | secondScale, |
| | 20 | 733 | | Vector3d.Zero, |
| | 20 | 734 | | Fixed64.Zero, |
| | 20 | 735 | | out Vector3d spatialImpulse) |
| | 20 | 736 | | && TryApplyCompactImpulse( |
| | 20 | 737 | | contact, |
| | 20 | 738 | | ExactContactLever2D.ToPlanar(spatialImpulse))) |
| | | 739 | | { |
| | 17 | 740 | | return true; |
| | | 741 | | } |
| | | 742 | | |
| | 3 | 743 | | GetExactLevers( |
| | 3 | 744 | | pair, |
| | 3 | 745 | | contact, |
| | 3 | 746 | | responsePositionA, |
| | 3 | 747 | | responsePositionB, |
| | 3 | 748 | | out ExactLever3D exactA, |
| | 3 | 749 | | out ExactLever3D exactB); |
| | 3 | 750 | | return ExactContactLever2D.TryGetImpulseVelocityDeltas( |
| | 3 | 751 | | contact.A.Body, |
| | 3 | 752 | | exactA, |
| | 3 | 753 | | contact.B.Body, |
| | 3 | 754 | | exactB, |
| | 3 | 755 | | firstAxis, |
| | 3 | 756 | | firstScale, |
| | 3 | 757 | | secondAxis, |
| | 3 | 758 | | secondScale, |
| | 3 | 759 | | out Vector2d linearA, |
| | 3 | 760 | | out Fixed64 angularA, |
| | 3 | 761 | | out Vector2d linearB, |
| | 3 | 762 | | out Fixed64 angularB) |
| | 3 | 763 | | && TryApplyVelocityDeltas( |
| | 3 | 764 | | contact, |
| | 3 | 765 | | linearA, |
| | 3 | 766 | | angularA, |
| | 3 | 767 | | linearB, |
| | 3 | 768 | | angularB); |
| | | 769 | | } |
| | | 770 | | |
| | | 771 | | private static bool TryApplyCompactImpulse( |
| | | 772 | | SolverContact2D contact, |
| | | 773 | | Vector2d impulseB) |
| | | 774 | | { |
| | 19 | 775 | | Vector2d impulseA = -impulseB; |
| | 19 | 776 | | bool linearAResolved = TryGetLinearVelocityDelta( |
| | 19 | 777 | | contact.A, |
| | 19 | 778 | | impulseA, |
| | 19 | 779 | | out Vector2d linearA); |
| | 19 | 780 | | bool angularAResolved = TryGetAngularVelocityDelta( |
| | 19 | 781 | | contact.A, |
| | 19 | 782 | | contact.RelativeA.Vector, |
| | 19 | 783 | | impulseA, |
| | 19 | 784 | | out Fixed64 angularA); |
| | 19 | 785 | | bool linearBResolved = TryGetLinearVelocityDelta( |
| | 19 | 786 | | contact.B, |
| | 19 | 787 | | impulseB, |
| | 19 | 788 | | out Vector2d linearB); |
| | 19 | 789 | | bool angularBResolved = TryGetAngularVelocityDelta( |
| | 19 | 790 | | contact.B, |
| | 19 | 791 | | contact.RelativeB.Vector, |
| | 19 | 792 | | impulseB, |
| | 19 | 793 | | out Fixed64 angularB); |
| | 19 | 794 | | return linearAResolved |
| | 19 | 795 | | & angularAResolved |
| | 19 | 796 | | & linearBResolved |
| | 19 | 797 | | & angularBResolved |
| | 19 | 798 | | && TryApplyVelocityDeltas( |
| | 19 | 799 | | contact, |
| | 19 | 800 | | linearA, |
| | 19 | 801 | | angularA, |
| | 19 | 802 | | linearB, |
| | 19 | 803 | | angularB); |
| | | 804 | | } |
| | | 805 | | |
| | | 806 | | private static bool TryGetLinearVelocityDelta( |
| | | 807 | | ResponseBody2D body, |
| | | 808 | | Vector2d impulse, |
| | | 809 | | out Vector2d velocityDelta) |
| | | 810 | | { |
| | 38 | 811 | | if (!body.CanTranslate) |
| | | 812 | | { |
| | 19 | 813 | | velocityDelta = Vector2d.Zero; |
| | 19 | 814 | | return true; |
| | | 815 | | } |
| | | 816 | | |
| | 19 | 817 | | return ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta( |
| | 19 | 818 | | body.Body!.ProjectLinearMotion(impulse), |
| | 19 | 819 | | Fixed64.One, |
| | 19 | 820 | | body.InverseMass, |
| | 19 | 821 | | Fixed64.One, |
| | 19 | 822 | | out velocityDelta); |
| | | 823 | | } |
| | | 824 | | |
| | | 825 | | private static bool TryGetAngularVelocityDelta( |
| | | 826 | | ResponseBody2D body, |
| | | 827 | | Vector2d relativeContactPoint, |
| | | 828 | | Vector2d impulse, |
| | | 829 | | out Fixed64 velocityDelta) |
| | | 830 | | { |
| | 38 | 831 | | velocityDelta = Fixed64.Zero; |
| | 38 | 832 | | if (!body.CanRotate) |
| | 25 | 833 | | return true; |
| | | 834 | | |
| | 13 | 835 | | return ContactResponseArithmetic3D.TryCross( |
| | 13 | 836 | | ExactContactLever2D.ToSpatial(relativeContactPoint), |
| | 13 | 837 | | ExactContactLever2D.ToSpatial(impulse), |
| | 13 | 838 | | out Vector3d torque) |
| | 13 | 839 | | && Fixed64.TryMultiplyDivide( |
| | 13 | 840 | | -torque.Y, |
| | 13 | 841 | | body.InverseMoment, |
| | 13 | 842 | | Fixed64.One, |
| | 13 | 843 | | out velocityDelta); |
| | | 844 | | } |
| | | 845 | | |
| | | 846 | | private static bool TryApplyVelocityDeltas( |
| | | 847 | | SolverContact2D contact, |
| | | 848 | | Vector2d linearA, |
| | | 849 | | Fixed64 angularA, |
| | | 850 | | Vector2d linearB, |
| | | 851 | | Fixed64 angularB) |
| | | 852 | | { |
| | 119 | 853 | | bool firstFits = contact.A.Body?.CanApplyCollisionVelocityDeltas( |
| | 119 | 854 | | linearA, |
| | 119 | 855 | | angularA) |
| | 119 | 856 | | ?? true; |
| | 119 | 857 | | bool secondFits = contact.B.Body?.CanApplyCollisionVelocityDeltas( |
| | 119 | 858 | | linearB, |
| | 119 | 859 | | angularB) |
| | 119 | 860 | | ?? true; |
| | 119 | 861 | | if (!(firstFits & secondFits)) |
| | 1 | 862 | | return false; |
| | | 863 | | |
| | 118 | 864 | | ApplyVelocityDelta(contact.A, linearA, angularA); |
| | 118 | 865 | | ApplyVelocityDelta(contact.B, linearB, angularB); |
| | 118 | 866 | | return true; |
| | | 867 | | } |
| | | 868 | | |
| | | 869 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 870 | | private static Vector2d ResolveLinearVelocity(SolidBody2D? body) => |
| | 6096 | 871 | | body == null |
| | 6096 | 872 | | ? Vector2d.Zero |
| | 6096 | 873 | | : body.ProjectLinearMotion( |
| | 6096 | 874 | | body.IsKinematic |
| | 6096 | 875 | | ? body.SampleContinuousCollisionLinearVelocity(Fixed64.One) |
| | 6096 | 876 | | : body.LinearVelocity); |
| | | 877 | | |
| | | 878 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 879 | | private static Fixed64 ResolveAngularVelocity(SolidBody2D? body) => |
| | 6096 | 880 | | body == null |
| | 6096 | 881 | | ? Fixed64.Zero |
| | 6096 | 882 | | : body.IsKinematic |
| | 6096 | 883 | | ? body.SampleContinuousCollisionAngularVelocity(Fixed64.One) |
| | 6096 | 884 | | : body.AngularVelocity; |
| | | 885 | | |
| | | 886 | | private static Vector2d ResolveContactNormal(Vector2d normal, Vector2d fallbackDirection) |
| | | 887 | | { |
| | 1012 | 888 | | Vector2d resolved = normal.MagnitudeSquared > Fixed64.Epsilon |
| | 1012 | 889 | | ? normal.Normalized |
| | 1012 | 890 | | : fallbackDirection.MagnitudeSquared > Fixed64.Epsilon |
| | 1012 | 891 | | ? fallbackDirection.Normalized |
| | 1012 | 892 | | : Vector2d.Zero; |
| | | 893 | | |
| | 1012 | 894 | | if (resolved == Vector2d.Zero) |
| | 1 | 895 | | return resolved; |
| | | 896 | | |
| | 1011 | 897 | | return fallbackDirection.MagnitudeSquared > Fixed64.Epsilon |
| | 1011 | 898 | | && Vector2d.Dot(resolved, fallbackDirection) < Fixed64.Zero |
| | 1011 | 899 | | ? -resolved |
| | 1011 | 900 | | : resolved; |
| | | 901 | | } |
| | | 902 | | |
| | | 903 | | private static void GetExactLevers( |
| | | 904 | | CollisionPair2D pair, |
| | | 905 | | SolverContact2D contact, |
| | | 906 | | Vector2d responsePositionA, |
| | | 907 | | Vector2d responsePositionB, |
| | | 908 | | out ExactLever3D exactA, |
| | | 909 | | out ExactLever3D exactB) |
| | | 910 | | { |
| | 70 | 911 | | ManifoldContact2D manifoldContact = |
| | 70 | 912 | | pair.Manifold[contact.ManifoldIndex]; |
| | 70 | 913 | | ContactAnchor2D zero = |
| | 70 | 914 | | ContactAnchor2D.FromWorldPoint(Vector2d.Zero); |
| | 70 | 915 | | exactA = contact.A.Body == null |
| | 70 | 916 | | ? zero.GetXZLeverFrom(zero) |
| | 70 | 917 | | : manifoldContact.AnchorA.GetXZLeverFrom( |
| | 70 | 918 | | new ContactAnchor2D( |
| | 70 | 919 | | responsePositionA, |
| | 70 | 920 | | contact.A.Body.Rotation, |
| | 70 | 921 | | contact.A.Body.LocalCenterOfMassOffset)); |
| | 70 | 922 | | exactB = contact.B.Body == null |
| | 70 | 923 | | ? zero.GetXZLeverFrom(zero) |
| | 70 | 924 | | : manifoldContact.AnchorB.GetXZLeverFrom( |
| | 70 | 925 | | new ContactAnchor2D( |
| | 70 | 926 | | responsePositionB, |
| | 70 | 927 | | contact.B.Body.Rotation, |
| | 70 | 928 | | contact.B.Body.LocalCenterOfMassOffset)); |
| | 70 | 929 | | } |
| | | 930 | | |
| | | 931 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 932 | | private static bool HasFailedResponse( |
| | | 933 | | byte failedResponseMask, |
| | | 934 | | int contactIndex) => |
| | 3024 | 935 | | (failedResponseMask & (1 << contactIndex)) != 0; |
| | | 936 | | |
| | | 937 | | private static void RejectResponse( |
| | | 938 | | CollisionPair2D pair, |
| | | 939 | | SolverContact2D contact, |
| | | 940 | | int contactIndex, |
| | | 941 | | ref byte failedResponseMask) |
| | | 942 | | { |
| | 5 | 943 | | failedResponseMask |= (byte)(1 << contactIndex); |
| | 5 | 944 | | pair.RemoveWarmStartImpulse(contact.ContactId); |
| | 5 | 945 | | GravitasLogger.Channel.Error( |
| | 5 | 946 | | $"2D contact response is outside the representable velocity domain."); |
| | 5 | 947 | | } |
| | | 948 | | } |