| | | 1 | | //======================================================================= |
| | | 2 | | // CollisionResponse.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 | | /// Solves deterministic contact response for one collision pair manifold. |
| | | 17 | | /// </summary> |
| | | 18 | | public static class CollisionResponse |
| | | 19 | | { |
| | | 20 | | /// <summary> |
| | | 21 | | /// Penetration depth below this value is treated as contact slop and does not |
| | | 22 | | /// produce positional correction. |
| | | 23 | | /// </summary> |
| | 1 | 24 | | public static readonly Fixed64 PenetrationSlop = (Fixed64)0.01f; |
| | | 25 | | |
| | | 26 | | /// <summary> |
| | | 27 | | /// Fraction of penetration above slop corrected per solver call. |
| | | 28 | | /// </summary> |
| | 1 | 29 | | public static readonly Fixed64 PenetrationCorrectionPercent = (Fixed64)0.8f; |
| | | 30 | | |
| | 1 | 31 | | private static readonly Fixed64 WarmStartNormalCompatibilityThreshold = Fixed64.FromFraction(63, 64); |
| | | 32 | | |
| | | 33 | | /// <summary> |
| | | 34 | | /// Applies positional correction, normal impulses, and Coulomb friction for |
| | | 35 | | /// the collision pair's current deterministic contact manifold. |
| | | 36 | | /// </summary> |
| | | 37 | | public static void CalculateImpulse(CollisionPair pair) => |
| | 155 | 38 | | CalculateImpulse( |
| | 155 | 39 | | pair, |
| | 155 | 40 | | applyCachedImpulse: true, |
| | 155 | 41 | | applyPositionCorrection: true); |
| | | 42 | | |
| | | 43 | | internal static void CalculateImpulse( |
| | | 44 | | CollisionPair pair, |
| | | 45 | | bool applyCachedImpulse, |
| | | 46 | | bool applyPositionCorrection) |
| | | 47 | | { |
| | 8842 | 48 | | if (!TryCreateBodyPair(pair, out ResponseBody bodyA, out ResponseBody bodyB)) |
| | 4 | 49 | | return; |
| | | 50 | | |
| | 8838 | 51 | | ContactAnchor responseCenterA = bodyA.Body.GetCenterOfMassAnchor(); |
| | 8838 | 52 | | ContactAnchor responseCenterB = bodyB.Body.GetCenterOfMassAnchor(); |
| | 8838 | 53 | | SolverContactBuffer contacts = BuildContactBuffer( |
| | 8838 | 54 | | pair, |
| | 8838 | 55 | | bodyA, |
| | 8838 | 56 | | bodyB, |
| | 8838 | 57 | | responseCenterA, |
| | 8838 | 58 | | responseCenterB); |
| | 8838 | 59 | | if (contacts.Count == 0) |
| | 1 | 60 | | return; |
| | | 61 | | |
| | 8837 | 62 | | Vector3d responsePositionA = bodyA.Body.Position3d; |
| | 8837 | 63 | | Vector3d responsePositionB = bodyB.Body.Position3d; |
| | 8837 | 64 | | byte failedResponseMask = 0; |
| | 8837 | 65 | | Fixed64 contactShare = Fixed64.One / (Fixed64)contacts.Count; |
| | 8837 | 66 | | if (applyPositionCorrection) |
| | | 67 | | { |
| | 3700 | 68 | | for (int i = 0; i < contacts.Count; i++) |
| | 970 | 69 | | ApplyPositionCorrection(contacts.GetContact(i), contactShare); |
| | | 70 | | } |
| | | 71 | | |
| | 8837 | 72 | | if (applyCachedImpulse) |
| | | 73 | | { |
| | 888 | 74 | | bool rebuildContacts = false; |
| | 3732 | 75 | | for (int i = 0; i < contacts.Count; i++) |
| | | 76 | | { |
| | 978 | 77 | | SolverContact contact = contacts.GetContact(i); |
| | 978 | 78 | | if (!TryApplyCachedImpulse( |
| | 978 | 79 | | pair, |
| | 978 | 80 | | contact, |
| | 978 | 81 | | responsePositionA, |
| | 978 | 82 | | responsePositionB)) |
| | | 83 | | { |
| | 7 | 84 | | ClearWarmStartImpulse(pair, contact); |
| | 7 | 85 | | rebuildContacts = true; |
| | | 86 | | } |
| | | 87 | | } |
| | | 88 | | |
| | 888 | 89 | | if (rebuildContacts) |
| | | 90 | | { |
| | 7 | 91 | | contacts = BuildContactBuffer( |
| | 7 | 92 | | pair, |
| | 7 | 93 | | bodyA, |
| | 7 | 94 | | bodyB, |
| | 7 | 95 | | responseCenterA, |
| | 7 | 96 | | responseCenterB); |
| | | 97 | | } |
| | | 98 | | } |
| | | 99 | | |
| | 8837 | 100 | | Fixed64 restitutionVelocityThreshold = pair.Context.Settings.RestitutionVelocityThreshold; |
| | 35828 | 101 | | for (int i = 0; i < contacts.Count; i++) |
| | | 102 | | { |
| | 9077 | 103 | | SolverContact contact = contacts.GetContact(i); |
| | | 104 | | bool normalResolved; |
| | | 105 | | ContactNormalImpulseResult3D normalResult; |
| | 9077 | 106 | | if (contact.RelativeA.IsExact || contact.RelativeB.IsExact) |
| | | 107 | | { |
| | 34 | 108 | | normalResolved = TryCalculateExactNormalResult( |
| | 34 | 109 | | pair, |
| | 34 | 110 | | contact, |
| | 34 | 111 | | responsePositionA, |
| | 34 | 112 | | responsePositionB, |
| | 34 | 113 | | restitutionVelocityThreshold, |
| | 34 | 114 | | contactShare, |
| | 34 | 115 | | out normalResult); |
| | | 116 | | } |
| | | 117 | | else |
| | | 118 | | { |
| | 9043 | 119 | | normalResolved = ContactNormalImpulse3D.TryCalculateAccumulatedDelta( |
| | 9043 | 120 | | contact.A.Body, |
| | 9043 | 121 | | ResolveLinearVelocity(contact.A.Body), |
| | 9043 | 122 | | ResolveAngularVelocity(contact.A.Body), |
| | 9043 | 123 | | contact.RelativeA.Vector, |
| | 9043 | 124 | | contact.B.Body, |
| | 9043 | 125 | | ResolveLinearVelocity(contact.B.Body), |
| | 9043 | 126 | | ResolveAngularVelocity(contact.B.Body), |
| | 9043 | 127 | | contact.RelativeB.Vector, |
| | 9043 | 128 | | contact.Normal, |
| | 9043 | 129 | | contact.Restitution, |
| | 9043 | 130 | | restitutionVelocityThreshold, |
| | 9043 | 131 | | contact.CachedNormalImpulse, |
| | 9043 | 132 | | contactShare, |
| | 9043 | 133 | | Fixed64.One, |
| | 9043 | 134 | | out normalResult); |
| | 9043 | 135 | | if (!normalResolved) |
| | | 136 | | { |
| | 1353 | 137 | | normalResolved = TryCalculateExactNormalResult( |
| | 1353 | 138 | | pair, |
| | 1353 | 139 | | contact, |
| | 1353 | 140 | | responsePositionA, |
| | 1353 | 141 | | responsePositionB, |
| | 1353 | 142 | | restitutionVelocityThreshold, |
| | 1353 | 143 | | contactShare, |
| | 1353 | 144 | | out normalResult); |
| | | 145 | | } |
| | | 146 | | } |
| | | 147 | | |
| | 9077 | 148 | | if (!normalResolved) |
| | | 149 | | { |
| | 2 | 150 | | RejectResponse(pair, contact, i, ref failedResponseMask); |
| | 2 | 151 | | continue; |
| | | 152 | | } |
| | | 153 | | |
| | | 154 | | // The kernel preflights this same cache-plus-delta sum. |
| | 9075 | 155 | | Fixed64 normalImpulse = |
| | 9075 | 156 | | contact.CachedNormalImpulse |
| | 9075 | 157 | | + normalResult.ImpulseScalar; |
| | 9075 | 158 | | contacts.SetNormalImpulse(i, normalImpulse, normalResult); |
| | | 159 | | } |
| | | 160 | | |
| | 35828 | 161 | | for (int i = 0; i < contacts.Count; i++) |
| | | 162 | | { |
| | 9077 | 163 | | if (HasFailedResponse(failedResponseMask, i)) |
| | | 164 | | continue; |
| | | 165 | | |
| | 9075 | 166 | | SolverContact contact = contacts.GetContact(i); |
| | 9075 | 167 | | if (!TryApplyNormalImpulse( |
| | 9075 | 168 | | pair, |
| | 9075 | 169 | | contact, |
| | 9075 | 170 | | contacts.GetNormalResult(i))) |
| | | 171 | | { |
| | 1 | 172 | | RejectResponse(pair, contact, i, ref failedResponseMask); |
| | | 173 | | } |
| | | 174 | | } |
| | | 175 | | |
| | 35828 | 176 | | for (int i = 0; i < contacts.Count; i++) |
| | | 177 | | { |
| | 9077 | 178 | | if (HasFailedResponse(failedResponseMask, i)) |
| | | 179 | | continue; |
| | | 180 | | |
| | 9074 | 181 | | SolverContact contact = contacts.GetContact(i); |
| | 9074 | 182 | | if (!TrySolveFrictionImpulse( |
| | 9074 | 183 | | pair, |
| | 9074 | 184 | | contact, |
| | 9074 | 185 | | responsePositionA, |
| | 9074 | 186 | | responsePositionB, |
| | 9074 | 187 | | FixedMath.Max(contacts.GetNormalImpulse(i), contact.CachedNormalImpulse), |
| | 9074 | 188 | | out Fixed64 tangentImpulse, |
| | 9074 | 189 | | out Fixed64 secondaryTangentImpulse)) |
| | | 190 | | { |
| | 1 | 191 | | RejectResponse(pair, contact, i, ref failedResponseMask); |
| | 1 | 192 | | continue; |
| | | 193 | | } |
| | | 194 | | |
| | 9073 | 195 | | contacts.SetTangentImpulse(i, tangentImpulse, secondaryTangentImpulse); |
| | | 196 | | } |
| | | 197 | | |
| | 35828 | 198 | | for (int i = 0; i < contacts.Count; i++) |
| | | 199 | | { |
| | 9077 | 200 | | if (HasFailedResponse(failedResponseMask, i)) |
| | | 201 | | continue; |
| | | 202 | | |
| | 9073 | 203 | | SolverContact contact = contacts.GetContact(i); |
| | 9073 | 204 | | pair.StoreWarmStartImpulse( |
| | 9073 | 205 | | contact.ContactId, |
| | 9073 | 206 | | contact.Normal, |
| | 9073 | 207 | | contacts.GetNormalImpulse(i), |
| | 9073 | 208 | | contacts.GetTangentImpulse(i), |
| | 9073 | 209 | | contacts.GetSecondaryTangentImpulse(i)); |
| | | 210 | | } |
| | 8837 | 211 | | } |
| | | 212 | | |
| | | 213 | | private static bool TryCreateBodyPair(CollisionPair pair, out ResponseBody bodyA, out ResponseBody bodyB) |
| | | 214 | | { |
| | 8842 | 215 | | bodyA = default; |
| | 8842 | 216 | | bodyB = default; |
| | | 217 | | |
| | 8842 | 218 | | if (pair.ColliderA.IsTrigger || pair.ColliderB.IsTrigger) |
| | 1 | 219 | | return false; |
| | | 220 | | |
| | 8841 | 221 | | if (pair.ColliderA.Body == null || pair.ColliderB.Body == null) |
| | 1 | 222 | | return false; |
| | | 223 | | |
| | 8840 | 224 | | if (!pair.Manifold.HasContact) |
| | 1 | 225 | | return false; |
| | | 226 | | |
| | 8839 | 227 | | bodyA = ResponseBody.Create(pair.ColliderA); |
| | 8839 | 228 | | bodyB = ResponseBody.Create(pair.ColliderB); |
| | 8839 | 229 | | return bodyA.HasSolverMobility || bodyB.HasSolverMobility; |
| | | 230 | | } |
| | | 231 | | |
| | | 232 | | private static SolverContactBuffer BuildContactBuffer( |
| | | 233 | | CollisionPair pair, |
| | | 234 | | ResponseBody bodyA, |
| | | 235 | | ResponseBody bodyB, |
| | | 236 | | in ContactAnchor responseCenterA, |
| | | 237 | | in ContactAnchor responseCenterB) |
| | | 238 | | { |
| | 8845 | 239 | | SolverContactBuffer contacts = default; |
| | 35860 | 240 | | for (int i = 0; i < pair.Manifold.Count; i++) |
| | | 241 | | { |
| | 9085 | 242 | | if (TryCreateContact( |
| | 9085 | 243 | | pair, |
| | 9085 | 244 | | bodyA, |
| | 9085 | 245 | | bodyB, |
| | 9085 | 246 | | responseCenterA, |
| | 9085 | 247 | | responseCenterB, |
| | 9085 | 248 | | i, |
| | 9085 | 249 | | out SolverContact contact)) |
| | | 250 | | { |
| | 9084 | 251 | | contacts.Add(contact); |
| | | 252 | | } |
| | | 253 | | } |
| | | 254 | | |
| | 8845 | 255 | | return contacts; |
| | | 256 | | } |
| | | 257 | | |
| | | 258 | | private static bool TryCreateContact( |
| | | 259 | | CollisionPair pair, |
| | | 260 | | ResponseBody bodyA, |
| | | 261 | | ResponseBody bodyB, |
| | | 262 | | in ContactAnchor responseCenterA, |
| | | 263 | | in ContactAnchor responseCenterB, |
| | | 264 | | int contactIndex, |
| | | 265 | | out SolverContact contact) |
| | | 266 | | { |
| | 9085 | 267 | | contact = default; |
| | 9085 | 268 | | ManifoldContact manifoldContact = pair.Manifold[contactIndex]; |
| | 9085 | 269 | | Vector3d normal = ResolveContactNormal(manifoldContact.Normal, pair.ColliderB.Center - pair.ColliderA.Center); |
| | 9085 | 270 | | if (normal == Vector3d.Zero) |
| | 1 | 271 | | return false; |
| | 9084 | 272 | | ContactLever3D relativeA = ContactLever3D.Create( |
| | 9084 | 273 | | manifoldContact.AnchorA, |
| | 9084 | 274 | | responseCenterA); |
| | 9084 | 275 | | ContactLever3D relativeB = ContactLever3D.Create( |
| | 9084 | 276 | | manifoldContact.AnchorB, |
| | 9084 | 277 | | responseCenterB); |
| | 9084 | 278 | | PhysicsMaterial materialA = manifoldContact.HasMaterialOverride |
| | 9084 | 279 | | ? manifoldContact.MaterialA |
| | 9084 | 280 | | : pair.ColliderA.Material; |
| | 9084 | 281 | | PhysicsMaterial materialB = manifoldContact.HasMaterialOverride |
| | 9084 | 282 | | ? manifoldContact.MaterialB |
| | 9084 | 283 | | : pair.ColliderB.Material; |
| | | 284 | | |
| | 9084 | 285 | | Fixed64 cachedNormalImpulse = Fixed64.Zero; |
| | 9084 | 286 | | Fixed64 cachedTangentImpulse = Fixed64.Zero; |
| | 9084 | 287 | | Fixed64 cachedSecondaryTangentImpulse = Fixed64.Zero; |
| | 9084 | 288 | | if (pair.TryGetWarmStartImpulse(manifoldContact.ContactId, out ContactWarmStartImpulse cached)) |
| | | 289 | | { |
| | 8053 | 290 | | if (IsWarmStartCompatible(cached.Normal, normal)) |
| | | 291 | | { |
| | 8050 | 292 | | cachedNormalImpulse = cached.NormalImpulse; |
| | 8050 | 293 | | cachedTangentImpulse = cached.TangentImpulse; |
| | 8050 | 294 | | cachedSecondaryTangentImpulse = cached.SecondaryTangentImpulse; |
| | | 295 | | } |
| | | 296 | | } |
| | | 297 | | |
| | 9084 | 298 | | contact = new SolverContact( |
| | 9084 | 299 | | contactIndex, |
| | 9084 | 300 | | manifoldContact.ContactId, |
| | 9084 | 301 | | bodyA, |
| | 9084 | 302 | | bodyB, |
| | 9084 | 303 | | relativeA, |
| | 9084 | 304 | | relativeB, |
| | 9084 | 305 | | manifoldContact.Depth, |
| | 9084 | 306 | | normal, |
| | 9084 | 307 | | materialA, |
| | 9084 | 308 | | materialB, |
| | 9084 | 309 | | cachedNormalImpulse, |
| | 9084 | 310 | | cachedTangentImpulse, |
| | 9084 | 311 | | cachedSecondaryTangentImpulse); |
| | 9084 | 312 | | return true; |
| | | 313 | | } |
| | | 314 | | |
| | | 315 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 316 | | private static bool TryCalculateExactNormalResult( |
| | | 317 | | CollisionPair pair, |
| | | 318 | | SolverContact contact, |
| | | 319 | | Vector3d responsePositionA, |
| | | 320 | | Vector3d responsePositionB, |
| | | 321 | | Fixed64 restitutionVelocityThreshold, |
| | | 322 | | Fixed64 contactShare, |
| | | 323 | | out ContactNormalImpulseResult3D result) |
| | | 324 | | { |
| | 1387 | 325 | | result = default; |
| | 1387 | 326 | | GetExactLevers( |
| | 1387 | 327 | | pair, |
| | 1387 | 328 | | contact, |
| | 1387 | 329 | | responsePositionA, |
| | 1387 | 330 | | responsePositionB, |
| | 1387 | 331 | | out ExactLever3D exactA, |
| | 1387 | 332 | | out ExactLever3D exactB); |
| | 1387 | 333 | | return ContactNormalImpulse3D.TryCalculateAccumulatedDeltaExact( |
| | 1387 | 334 | | contact.A.Body, |
| | 1387 | 335 | | ResolveLinearVelocity(contact.A.Body), |
| | 1387 | 336 | | ResolveAngularVelocity(contact.A.Body), |
| | 1387 | 337 | | exactA, |
| | 1387 | 338 | | contact.B.Body, |
| | 1387 | 339 | | ResolveLinearVelocity(contact.B.Body), |
| | 1387 | 340 | | ResolveAngularVelocity(contact.B.Body), |
| | 1387 | 341 | | exactB, |
| | 1387 | 342 | | contact.Normal, |
| | 1387 | 343 | | contact.Restitution, |
| | 1387 | 344 | | restitutionVelocityThreshold, |
| | 1387 | 345 | | contact.CachedNormalImpulse, |
| | 1387 | 346 | | contactShare, |
| | 1387 | 347 | | Fixed64.One, |
| | 1387 | 348 | | out result); |
| | | 349 | | } |
| | | 350 | | |
| | | 351 | | private static void ApplyPositionCorrection(SolverContact contact, Fixed64 contactShare) |
| | | 352 | | { |
| | 970 | 353 | | Fixed64 correctionDepth = contact.Depth - PenetrationSlop; |
| | 970 | 354 | | if (correctionDepth <= Fixed64.Zero) |
| | 436 | 355 | | return; |
| | | 356 | | |
| | 534 | 357 | | Fixed64 inverseMassA = contact.A.GetConstrainedInverseMass(contact.Normal); |
| | 534 | 358 | | Fixed64 inverseMassB = contact.B.GetConstrainedInverseMass(contact.Normal); |
| | 534 | 359 | | Fixed64 totalInverseMass = inverseMassA + inverseMassB; |
| | 534 | 360 | | if (totalInverseMass <= Fixed64.Zero) |
| | 1 | 361 | | return; |
| | | 362 | | |
| | 533 | 363 | | Vector3d correction = contact.Normal |
| | 533 | 364 | | * (correctionDepth * PenetrationCorrectionPercent * contactShare / totalInverseMass); |
| | 533 | 365 | | contact.A.Body.ApplyCollisionPositionCorrection(-correction * inverseMassA); |
| | 533 | 366 | | contact.B.Body.ApplyCollisionPositionCorrection(correction * inverseMassB); |
| | 533 | 367 | | } |
| | | 368 | | |
| | | 369 | | private static bool TryApplyCachedImpulse( |
| | | 370 | | CollisionPair pair, |
| | | 371 | | SolverContact contact, |
| | | 372 | | Vector3d responsePositionA, |
| | | 373 | | Vector3d responsePositionB) |
| | | 374 | | { |
| | 978 | 375 | | if (contact.CachedNormalImpulse == Fixed64.Zero |
| | 978 | 376 | | && contact.CachedTangentImpulse == Fixed64.Zero |
| | 978 | 377 | | && contact.CachedSecondaryTangentImpulse == Fixed64.Zero) |
| | | 378 | | { |
| | 956 | 379 | | return true; |
| | | 380 | | } |
| | | 381 | | |
| | 22 | 382 | | return TryApplyContactImpulseCombination( |
| | 22 | 383 | | pair, |
| | 22 | 384 | | contact, |
| | 22 | 385 | | responsePositionA, |
| | 22 | 386 | | responsePositionB, |
| | 22 | 387 | | contact.Normal, |
| | 22 | 388 | | contact.CachedNormalImpulse, |
| | 22 | 389 | | contact.Tangent, |
| | 22 | 390 | | contact.CachedTangentImpulse, |
| | 22 | 391 | | contact.SecondaryTangent, |
| | 22 | 392 | | contact.CachedSecondaryTangentImpulse); |
| | | 393 | | } |
| | | 394 | | |
| | | 395 | | private static bool TryApplyContactImpulseCombination( |
| | | 396 | | CollisionPair pair, |
| | | 397 | | SolverContact contact, |
| | | 398 | | Vector3d responsePositionA, |
| | | 399 | | Vector3d responsePositionB, |
| | | 400 | | Vector3d firstAxis, |
| | | 401 | | Fixed64 firstScale, |
| | | 402 | | Vector3d secondAxis, |
| | | 403 | | Fixed64 secondScale, |
| | | 404 | | Vector3d thirdAxis, |
| | | 405 | | Fixed64 thirdScale) |
| | | 406 | | { |
| | 6173 | 407 | | if (ContactResponseArithmetic3D.TryLinearCombination( |
| | 6173 | 408 | | firstAxis, |
| | 6173 | 409 | | firstScale, |
| | 6173 | 410 | | secondAxis, |
| | 6173 | 411 | | secondScale, |
| | 6173 | 412 | | thirdAxis, |
| | 6173 | 413 | | thirdScale, |
| | 6173 | 414 | | out Vector3d impulse) |
| | 6173 | 415 | | && CanNegate(impulse)) |
| | | 416 | | { |
| | 6168 | 417 | | return TryApplyContactImpulse( |
| | 6168 | 418 | | pair, |
| | 6168 | 419 | | contact, |
| | 6168 | 420 | | responsePositionA, |
| | 6168 | 421 | | responsePositionB, |
| | 6168 | 422 | | impulse); |
| | | 423 | | } |
| | | 424 | | |
| | 5 | 425 | | return TryApplyContactImpulseCombinationExact( |
| | 5 | 426 | | pair, |
| | 5 | 427 | | contact, |
| | 5 | 428 | | responsePositionA, |
| | 5 | 429 | | responsePositionB, |
| | 5 | 430 | | firstAxis, |
| | 5 | 431 | | firstScale, |
| | 5 | 432 | | secondAxis, |
| | 5 | 433 | | secondScale, |
| | 5 | 434 | | thirdAxis, |
| | 5 | 435 | | thirdScale); |
| | | 436 | | } |
| | | 437 | | |
| | | 438 | | private static bool TryApplyContactImpulse( |
| | | 439 | | CollisionPair pair, |
| | | 440 | | SolverContact contact, |
| | | 441 | | Vector3d responsePositionA, |
| | | 442 | | Vector3d responsePositionB, |
| | | 443 | | Vector3d impulse) |
| | | 444 | | { |
| | 6168 | 445 | | if (!contact.RelativeA.IsExact |
| | 6168 | 446 | | && !contact.RelativeB.IsExact |
| | 6168 | 447 | | && TryApplyCompactImpulse(contact, impulse)) |
| | | 448 | | { |
| | 6120 | 449 | | return true; |
| | | 450 | | } |
| | | 451 | | |
| | 48 | 452 | | return TryApplyContactImpulseExact( |
| | 48 | 453 | | pair, |
| | 48 | 454 | | contact, |
| | 48 | 455 | | responsePositionA, |
| | 48 | 456 | | responsePositionB, |
| | 48 | 457 | | impulse); |
| | | 458 | | } |
| | | 459 | | |
| | | 460 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 461 | | private static bool TryApplyContactImpulseExact( |
| | | 462 | | CollisionPair pair, |
| | | 463 | | SolverContact contact, |
| | | 464 | | Vector3d responsePositionA, |
| | | 465 | | Vector3d responsePositionB, |
| | | 466 | | Vector3d impulse) |
| | | 467 | | { |
| | 48 | 468 | | GetExactLevers( |
| | 48 | 469 | | pair, |
| | 48 | 470 | | contact, |
| | 48 | 471 | | responsePositionA, |
| | 48 | 472 | | responsePositionB, |
| | 48 | 473 | | out ExactLever3D exactA, |
| | 48 | 474 | | out ExactLever3D exactB); |
| | 48 | 475 | | return TryApplyExactImpulse( |
| | 48 | 476 | | contact, |
| | 48 | 477 | | exactA, |
| | 48 | 478 | | exactB, |
| | 48 | 479 | | impulse); |
| | | 480 | | } |
| | | 481 | | |
| | | 482 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 483 | | private static bool TryApplyContactImpulseCombinationExact( |
| | | 484 | | CollisionPair pair, |
| | | 485 | | SolverContact contact, |
| | | 486 | | Vector3d responsePositionA, |
| | | 487 | | Vector3d responsePositionB, |
| | | 488 | | Vector3d firstAxis, |
| | | 489 | | Fixed64 firstScale, |
| | | 490 | | Vector3d secondAxis, |
| | | 491 | | Fixed64 secondScale, |
| | | 492 | | Vector3d thirdAxis, |
| | | 493 | | Fixed64 thirdScale) |
| | | 494 | | { |
| | 5 | 495 | | GetExactLevers( |
| | 5 | 496 | | pair, |
| | 5 | 497 | | contact, |
| | 5 | 498 | | responsePositionA, |
| | 5 | 499 | | responsePositionB, |
| | 5 | 500 | | out ExactLever3D exactA, |
| | 5 | 501 | | out ExactLever3D exactB); |
| | 5 | 502 | | if (!ExactContactLever3D |
| | 5 | 503 | | .TryGetImpulseCombinationVelocityDeltas( |
| | 5 | 504 | | contact.A.Body, |
| | 5 | 505 | | exactA, |
| | 5 | 506 | | contact.B.Body, |
| | 5 | 507 | | exactB, |
| | 5 | 508 | | firstAxis, |
| | 5 | 509 | | firstScale, |
| | 5 | 510 | | secondAxis, |
| | 5 | 511 | | secondScale, |
| | 5 | 512 | | thirdAxis, |
| | 5 | 513 | | thirdScale, |
| | 5 | 514 | | out Vector3d linearA, |
| | 5 | 515 | | out Vector3d angularA, |
| | 5 | 516 | | out Vector3d linearB, |
| | 5 | 517 | | out Vector3d angularB)) |
| | | 518 | | { |
| | 3 | 519 | | return false; |
| | | 520 | | } |
| | | 521 | | |
| | 2 | 522 | | return TryApplyVelocityDeltas( |
| | 2 | 523 | | contact, |
| | 2 | 524 | | linearA, |
| | 2 | 525 | | angularA, |
| | 2 | 526 | | linearB, |
| | 2 | 527 | | angularB); |
| | | 528 | | } |
| | | 529 | | |
| | | 530 | | private static bool TryApplyNormalImpulse( |
| | | 531 | | CollisionPair pair, |
| | | 532 | | SolverContact contact, |
| | | 533 | | ContactNormalImpulseResult3D result) |
| | | 534 | | { |
| | 9075 | 535 | | if (result.LinearVelocityDeltaA == Vector3d.Zero |
| | 9075 | 536 | | && result.AngularVelocityDeltaA == Vector3d.Zero |
| | 9075 | 537 | | && result.LinearVelocityDeltaB == Vector3d.Zero |
| | 9075 | 538 | | && result.AngularVelocityDeltaB == Vector3d.Zero) |
| | | 539 | | { |
| | 2243 | 540 | | return true; |
| | | 541 | | } |
| | | 542 | | |
| | 6832 | 543 | | if (!TryPrepareVelocityStates( |
| | 6832 | 544 | | contact, |
| | 6832 | 545 | | result.LinearVelocityDeltaA, |
| | 6832 | 546 | | result.AngularVelocityDeltaA, |
| | 6832 | 547 | | result.LinearVelocityDeltaB, |
| | 6832 | 548 | | result.AngularVelocityDeltaB, |
| | 6832 | 549 | | out Vector3d linearA, |
| | 6832 | 550 | | out Vector3d angularA, |
| | 6832 | 551 | | out Vector3d linearB, |
| | 6832 | 552 | | out Vector3d angularB)) |
| | | 553 | | { |
| | 1 | 554 | | return false; |
| | | 555 | | } |
| | | 556 | | |
| | 6831 | 557 | | if (result.HasRepresentableAppliedImpulse |
| | 6831 | 558 | | && result.HasRepresentableNormalVelocity |
| | 6831 | 559 | | && result.AppliedImpulseScalar != Fixed64.Zero) |
| | | 560 | | { |
| | 6826 | 561 | | Vector3d impulse = |
| | 6826 | 562 | | contact.Normal * result.AppliedImpulseScalar; |
| | 6826 | 563 | | pair.Context.Diagnostics.EmitResponseImpulse( |
| | 6826 | 564 | | pair, |
| | 6826 | 565 | | impulse, |
| | 6826 | 566 | | result.NormalVelocity); |
| | | 567 | | } |
| | 6831 | 568 | | ApplyVelocityStates( |
| | 6831 | 569 | | contact, |
| | 6831 | 570 | | linearA, |
| | 6831 | 571 | | angularA, |
| | 6831 | 572 | | linearB, |
| | 6831 | 573 | | angularB); |
| | 6831 | 574 | | return true; |
| | | 575 | | } |
| | | 576 | | |
| | | 577 | | private static bool TrySolveFrictionImpulse( |
| | | 578 | | CollisionPair pair, |
| | | 579 | | SolverContact contact, |
| | | 580 | | Vector3d responsePositionA, |
| | | 581 | | Vector3d responsePositionB, |
| | | 582 | | Fixed64 normalImpulseScalar, |
| | | 583 | | out Fixed64 tangentImpulse, |
| | | 584 | | out Fixed64 secondaryTangentImpulse) |
| | | 585 | | { |
| | 9074 | 586 | | if (!contact.RelativeA.IsExact |
| | 9074 | 587 | | && !contact.RelativeB.IsExact |
| | 9074 | 588 | | && TryGetCompactFrictionResponse( |
| | 9074 | 589 | | contact, |
| | 9074 | 590 | | normalImpulseScalar, |
| | 9074 | 591 | | out tangentImpulse, |
| | 9074 | 592 | | out secondaryTangentImpulse, |
| | 9074 | 593 | | out Fixed64 tangentDelta, |
| | 9074 | 594 | | out Fixed64 secondaryTangentDelta) |
| | 9074 | 595 | | && ((tangentDelta == Fixed64.Zero |
| | 9074 | 596 | | && secondaryTangentDelta == Fixed64.Zero) |
| | 9074 | 597 | | || TryApplyContactImpulseCombination( |
| | 9074 | 598 | | pair, |
| | 9074 | 599 | | contact, |
| | 9074 | 600 | | responsePositionA, |
| | 9074 | 601 | | responsePositionB, |
| | 9074 | 602 | | contact.Normal, |
| | 9074 | 603 | | Fixed64.Zero, |
| | 9074 | 604 | | contact.Tangent, |
| | 9074 | 605 | | tangentDelta, |
| | 9074 | 606 | | contact.SecondaryTangent, |
| | 9074 | 607 | | secondaryTangentDelta))) |
| | | 608 | | { |
| | 8690 | 609 | | return true; |
| | | 610 | | } |
| | | 611 | | |
| | 384 | 612 | | return TrySolveFrictionImpulseExact( |
| | 384 | 613 | | pair, |
| | 384 | 614 | | contact, |
| | 384 | 615 | | responsePositionA, |
| | 384 | 616 | | responsePositionB, |
| | 384 | 617 | | normalImpulseScalar, |
| | 384 | 618 | | out tangentImpulse, |
| | 384 | 619 | | out secondaryTangentImpulse); |
| | | 620 | | } |
| | | 621 | | |
| | | 622 | | private static bool TryGetCompactFrictionResponse( |
| | | 623 | | SolverContact contact, |
| | | 624 | | Fixed64 normalImpulseScalar, |
| | | 625 | | out Fixed64 tangentImpulse, |
| | | 626 | | out Fixed64 secondaryTangentImpulse, |
| | | 627 | | out Fixed64 tangentDelta, |
| | | 628 | | out Fixed64 secondaryTangentDelta) |
| | | 629 | | { |
| | 9042 | 630 | | tangentImpulse = default; |
| | 9042 | 631 | | secondaryTangentImpulse = default; |
| | 9042 | 632 | | tangentDelta = default; |
| | 9042 | 633 | | secondaryTangentDelta = default; |
| | 9042 | 634 | | bool limitsResolved = TryGetFrictionLimit( |
| | 9042 | 635 | | normalImpulseScalar, |
| | 9042 | 636 | | contact.StaticFriction, |
| | 9042 | 637 | | out Fixed64 staticFrictionLimit); |
| | 9042 | 638 | | limitsResolved &= TryGetFrictionLimit( |
| | 9042 | 639 | | normalImpulseScalar, |
| | 9042 | 640 | | contact.DynamicFriction, |
| | 9042 | 641 | | out Fixed64 dynamicFrictionLimit); |
| | 9042 | 642 | | if (!limitsResolved) |
| | 2 | 643 | | return false; |
| | | 644 | | |
| | 9040 | 645 | | if (staticFrictionLimit == Fixed64.Zero |
| | 9040 | 646 | | && dynamicFrictionLimit == Fixed64.Zero) |
| | | 647 | | { |
| | 2207 | 648 | | return Fixed64.TrySubtract( |
| | 2207 | 649 | | Fixed64.Zero, |
| | 2207 | 650 | | contact.CachedTangentImpulse, |
| | 2207 | 651 | | out tangentDelta) |
| | 2207 | 652 | | & Fixed64.TrySubtract( |
| | 2207 | 653 | | Fixed64.Zero, |
| | 2207 | 654 | | contact.CachedSecondaryTangentImpulse, |
| | 2207 | 655 | | out secondaryTangentDelta); |
| | | 656 | | } |
| | | 657 | | |
| | 6833 | 658 | | Vector3d linearA = ResolveLinearVelocity(contact.A.Body); |
| | 6833 | 659 | | Vector3d angularA = ResolveAngularVelocity(contact.A.Body); |
| | 6833 | 660 | | Vector3d linearB = ResolveLinearVelocity(contact.B.Body); |
| | 6833 | 661 | | Vector3d angularB = ResolveAngularVelocity(contact.B.Body); |
| | 6833 | 662 | | if (!ContactResponseArithmetic3D.TryGetRelativePointVelocity( |
| | 6833 | 663 | | linearA, |
| | 6833 | 664 | | angularA, |
| | 6833 | 665 | | contact.RelativeA.Vector, |
| | 6833 | 666 | | linearB, |
| | 6833 | 667 | | angularB, |
| | 6833 | 668 | | contact.RelativeB.Vector, |
| | 6833 | 669 | | contact.Tangent, |
| | 6833 | 670 | | out Vector3d relativeVelocity)) |
| | | 671 | | { |
| | 338 | 672 | | return false; |
| | | 673 | | } |
| | | 674 | | |
| | 6495 | 675 | | bool deltasResolved = TryGetCompactTangentImpulseDelta( |
| | 6495 | 676 | | contact, |
| | 6495 | 677 | | relativeVelocity, |
| | 6495 | 678 | | contact.Tangent, |
| | 6495 | 679 | | out Fixed64 desiredTangentDelta); |
| | 6495 | 680 | | deltasResolved &= TryGetCompactTangentImpulseDelta( |
| | 6495 | 681 | | contact, |
| | 6495 | 682 | | relativeVelocity, |
| | 6495 | 683 | | contact.SecondaryTangent, |
| | 6495 | 684 | | out Fixed64 desiredSecondaryTangentDelta); |
| | 6495 | 685 | | bool diskResolved = Fixed64.TryAdd( |
| | 6495 | 686 | | contact.CachedTangentImpulse, |
| | 6495 | 687 | | desiredTangentDelta, |
| | 6495 | 688 | | out tangentImpulse) |
| | 6495 | 689 | | & Fixed64.TryAdd( |
| | 6495 | 690 | | contact.CachedSecondaryTangentImpulse, |
| | 6495 | 691 | | desiredSecondaryTangentDelta, |
| | 6495 | 692 | | out secondaryTangentImpulse) |
| | 6495 | 693 | | & TryGetMagnitudeSquared( |
| | 6495 | 694 | | tangentImpulse, |
| | 6495 | 695 | | secondaryTangentImpulse, |
| | 6495 | 696 | | out Fixed64 desiredMagnitudeSquared) |
| | 6495 | 697 | | & TryGetSquare( |
| | 6495 | 698 | | staticFrictionLimit, |
| | 6495 | 699 | | out Fixed64 staticLimitSquared); |
| | 6495 | 700 | | if (!(deltasResolved & diskResolved)) |
| | | 701 | | { |
| | 9 | 702 | | return false; |
| | | 703 | | } |
| | | 704 | | |
| | 6486 | 705 | | if (desiredMagnitudeSquared > staticLimitSquared) |
| | | 706 | | { |
| | 4358 | 707 | | Fixed64 magnitude = FixedMath.Sqrt(desiredMagnitudeSquared); |
| | 4358 | 708 | | Fixed64 scale = dynamicFrictionLimit / magnitude; |
| | 4358 | 709 | | if (dynamicFrictionLimit != Fixed64.Zero |
| | 4358 | 710 | | && scale == Fixed64.Zero) |
| | | 711 | | { |
| | 1 | 712 | | return false; |
| | | 713 | | } |
| | | 714 | | |
| | 4357 | 715 | | Fixed64 desiredTangentImpulse = tangentImpulse; |
| | 4357 | 716 | | Fixed64 desiredSecondaryTangentImpulse = |
| | 4357 | 717 | | secondaryTangentImpulse; |
| | 4357 | 718 | | tangentImpulse *= scale; |
| | 4357 | 719 | | secondaryTangentImpulse *= scale; |
| | 4357 | 720 | | if (dynamicFrictionLimit != Fixed64.Zero |
| | 4357 | 721 | | && ((desiredTangentImpulse != Fixed64.Zero |
| | 4357 | 722 | | && tangentImpulse == Fixed64.Zero) |
| | 4357 | 723 | | || (desiredSecondaryTangentImpulse != Fixed64.Zero |
| | 4357 | 724 | | && secondaryTangentImpulse == Fixed64.Zero))) |
| | | 725 | | { |
| | 1 | 726 | | return false; |
| | | 727 | | } |
| | | 728 | | } |
| | | 729 | | |
| | 6484 | 730 | | return Fixed64.TrySubtract( |
| | 6484 | 731 | | tangentImpulse, |
| | 6484 | 732 | | contact.CachedTangentImpulse, |
| | 6484 | 733 | | out tangentDelta) |
| | 6484 | 734 | | & Fixed64.TrySubtract( |
| | 6484 | 735 | | secondaryTangentImpulse, |
| | 6484 | 736 | | contact.CachedSecondaryTangentImpulse, |
| | 6484 | 737 | | out secondaryTangentDelta); |
| | | 738 | | } |
| | | 739 | | |
| | | 740 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 741 | | private static bool TrySolveFrictionImpulseExact( |
| | | 742 | | CollisionPair pair, |
| | | 743 | | SolverContact contact, |
| | | 744 | | Vector3d responsePositionA, |
| | | 745 | | Vector3d responsePositionB, |
| | | 746 | | Fixed64 normalImpulseScalar, |
| | | 747 | | out Fixed64 tangentImpulse, |
| | | 748 | | out Fixed64 secondaryTangentImpulse) |
| | | 749 | | { |
| | 384 | 750 | | GetExactLevers( |
| | 384 | 751 | | pair, |
| | 384 | 752 | | contact, |
| | 384 | 753 | | responsePositionA, |
| | 384 | 754 | | responsePositionB, |
| | 384 | 755 | | out ExactLever3D exactA, |
| | 384 | 756 | | out ExactLever3D exactB); |
| | 384 | 757 | | Vector3d linearA = ResolveLinearVelocity(contact.A.Body); |
| | 384 | 758 | | Vector3d angularA = ResolveAngularVelocity(contact.A.Body); |
| | 384 | 759 | | Vector3d linearB = ResolveLinearVelocity(contact.B.Body); |
| | 384 | 760 | | Vector3d angularB = ResolveAngularVelocity(contact.B.Body); |
| | 384 | 761 | | ExactContactResponseOperand3D primaryFirst = |
| | 384 | 762 | | ExactContactLever3D.CreateResponseOperand( |
| | 384 | 763 | | contact.A.Body, |
| | 384 | 764 | | linearA, |
| | 384 | 765 | | angularA, |
| | 384 | 766 | | exactA, |
| | 384 | 767 | | -contact.Tangent); |
| | 384 | 768 | | ExactContactResponseOperand3D primarySecond = |
| | 384 | 769 | | ExactContactLever3D.CreateResponseOperand( |
| | 384 | 770 | | contact.B.Body, |
| | 384 | 771 | | linearB, |
| | 384 | 772 | | angularB, |
| | 384 | 773 | | exactB, |
| | 384 | 774 | | contact.Tangent); |
| | 384 | 775 | | ExactContactResponseOperand3D secondaryFirst = |
| | 384 | 776 | | ExactContactLever3D.CreateResponseOperand( |
| | 384 | 777 | | contact.A.Body, |
| | 384 | 778 | | linearA, |
| | 384 | 779 | | angularA, |
| | 384 | 780 | | exactA, |
| | 384 | 781 | | -contact.SecondaryTangent); |
| | 384 | 782 | | ExactContactResponseOperand3D secondarySecond = |
| | 384 | 783 | | ExactContactLever3D.CreateResponseOperand( |
| | 384 | 784 | | contact.B.Body, |
| | 384 | 785 | | linearB, |
| | 384 | 786 | | angularB, |
| | 384 | 787 | | exactB, |
| | 384 | 788 | | contact.SecondaryTangent); |
| | 384 | 789 | | if (!ExactContactResponseKernel.TryGetCoulombDiskResponse( |
| | 384 | 790 | | contact.Normal, |
| | 384 | 791 | | normalImpulseScalar, |
| | 384 | 792 | | primaryFirst, |
| | 384 | 793 | | primarySecond, |
| | 384 | 794 | | contact.Tangent, |
| | 384 | 795 | | contact.CachedTangentImpulse, |
| | 384 | 796 | | secondaryFirst, |
| | 384 | 797 | | secondarySecond, |
| | 384 | 798 | | contact.SecondaryTangent, |
| | 384 | 799 | | contact.CachedSecondaryTangentImpulse, |
| | 384 | 800 | | contact.StaticFriction, |
| | 384 | 801 | | contact.DynamicFriction, |
| | 384 | 802 | | out ExactCoulombResponse3D response)) |
| | | 803 | | { |
| | 1 | 804 | | tangentImpulse = default; |
| | 1 | 805 | | secondaryTangentImpulse = default; |
| | 1 | 806 | | return false; |
| | | 807 | | } |
| | | 808 | | |
| | 383 | 809 | | _ = response.TryGetPrimaryAccumulatedImpulse(out tangentImpulse); |
| | 383 | 810 | | _ = response.TryGetSecondaryAccumulatedImpulse( |
| | 383 | 811 | | out secondaryTangentImpulse); |
| | 383 | 812 | | return !response.HasAppliedImpulse |
| | 383 | 813 | | || TryApplyVelocityDeltas( |
| | 383 | 814 | | contact, |
| | 383 | 815 | | response.FirstLinearVelocityDelta, |
| | 383 | 816 | | response.FirstAngularVelocityDelta, |
| | 383 | 817 | | response.SecondLinearVelocityDelta, |
| | 383 | 818 | | response.SecondAngularVelocityDelta); |
| | | 819 | | } |
| | | 820 | | |
| | | 821 | | private static bool TryGetCompactTangentImpulseDelta( |
| | | 822 | | SolverContact contact, |
| | | 823 | | Vector3d relativeVelocity, |
| | | 824 | | Vector3d tangent, |
| | | 825 | | out Fixed64 impulseDelta) |
| | | 826 | | { |
| | 12990 | 827 | | impulseDelta = Fixed64.Zero; |
| | 12990 | 828 | | if (!ContactResponseArithmetic3D.TryDot( |
| | 12990 | 829 | | relativeVelocity, |
| | 12990 | 830 | | tangent, |
| | 12990 | 831 | | out Fixed64 tangentVelocity)) |
| | | 832 | | { |
| | 1 | 833 | | return false; |
| | | 834 | | } |
| | | 835 | | |
| | 12989 | 836 | | if (tangentVelocity >= -Fixed64.Epsilon |
| | 12989 | 837 | | && tangentVelocity <= Fixed64.Epsilon) |
| | | 838 | | { |
| | 6768 | 839 | | return true; |
| | | 840 | | } |
| | | 841 | | |
| | 6221 | 842 | | bool denominatorsResolved = |
| | 6221 | 843 | | ContactNormalImpulse3D.TryComputeAngularDenominator( |
| | 6221 | 844 | | contact.A.Body, |
| | 6221 | 845 | | contact.RelativeA.Vector, |
| | 6221 | 846 | | tangent, |
| | 6221 | 847 | | out Fixed64 angularA); |
| | 6221 | 848 | | denominatorsResolved &= |
| | 6221 | 849 | | ContactNormalImpulse3D.TryComputeAngularDenominator( |
| | 6221 | 850 | | contact.B.Body, |
| | 6221 | 851 | | contact.RelativeB.Vector, |
| | 6221 | 852 | | tangent, |
| | 6221 | 853 | | out Fixed64 angularB); |
| | 6221 | 854 | | denominatorsResolved &= TryGetCompactConstrainedInverseMass( |
| | 6221 | 855 | | contact.A, |
| | 6221 | 856 | | tangent, |
| | 6221 | 857 | | out Fixed64 linearA); |
| | 6221 | 858 | | denominatorsResolved &= TryGetCompactConstrainedInverseMass( |
| | 6221 | 859 | | contact.B, |
| | 6221 | 860 | | tangent, |
| | 6221 | 861 | | out Fixed64 linearB); |
| | 6221 | 862 | | var denominatorTerms = new ContactEffectiveMassTerms3D( |
| | 6221 | 863 | | linearA, |
| | 6221 | 864 | | linearB, |
| | 6221 | 865 | | angularA, |
| | 6221 | 866 | | angularB); |
| | 6221 | 867 | | bool denominatorResolved = denominatorsResolved |
| | 6221 | 868 | | & denominatorTerms.TryGetValue(out Fixed64 denominator); |
| | 6221 | 869 | | if (!denominatorResolved) |
| | | 870 | | { |
| | 2 | 871 | | return false; |
| | | 872 | | } |
| | | 873 | | |
| | 6219 | 874 | | if (denominator <= Fixed64.Epsilon) |
| | 1 | 875 | | return true; |
| | | 876 | | |
| | 6218 | 877 | | return Fixed64.TryMultiplyDivide( |
| | 6218 | 878 | | tangentVelocity, |
| | 6218 | 879 | | -Fixed64.One, |
| | 6218 | 880 | | denominator, |
| | 6218 | 881 | | out impulseDelta) |
| | 6218 | 882 | | && impulseDelta != Fixed64.Zero; |
| | | 883 | | } |
| | | 884 | | |
| | | 885 | | private static bool TryGetCompactConstrainedInverseMass( |
| | | 886 | | ResponseBody body, |
| | | 887 | | Vector3d axis, |
| | | 888 | | out Fixed64 inverseMass) |
| | | 889 | | { |
| | 12442 | 890 | | inverseMass = body.GetConstrainedInverseMass(axis); |
| | 12442 | 891 | | return inverseMass != Fixed64.Zero |
| | 12442 | 892 | | || body.InverseMass == Fixed64.Zero |
| | 12442 | 893 | | || body.Body.ProjectLinearMotion(axis) == Vector3d.Zero; |
| | | 894 | | } |
| | | 895 | | |
| | | 896 | | private static bool TryGetFrictionLimit( |
| | | 897 | | Fixed64 normalImpulse, |
| | | 898 | | Fixed64 friction, |
| | | 899 | | out Fixed64 limit) |
| | | 900 | | { |
| | 18084 | 901 | | if (normalImpulse <= Fixed64.Zero || friction <= Fixed64.Zero) |
| | | 902 | | { |
| | 4415 | 903 | | limit = Fixed64.Zero; |
| | 4415 | 904 | | return true; |
| | | 905 | | } |
| | | 906 | | |
| | 13669 | 907 | | return Fixed64.TryMultiplyDivide( |
| | 13669 | 908 | | normalImpulse, |
| | 13669 | 909 | | friction, |
| | 13669 | 910 | | Fixed64.One, |
| | 13669 | 911 | | out limit) |
| | 13669 | 912 | | && limit != Fixed64.Zero; |
| | | 913 | | } |
| | | 914 | | |
| | | 915 | | private static bool TryGetMagnitudeSquared( |
| | | 916 | | Fixed64 first, |
| | | 917 | | Fixed64 second, |
| | | 918 | | out Fixed64 result) |
| | | 919 | | { |
| | 6495 | 920 | | bool resolved = TryGetSquare(first, out Fixed64 firstSquared) |
| | 6495 | 921 | | & TryGetSquare(second, out Fixed64 secondSquared); |
| | 6495 | 922 | | if (!resolved) |
| | | 923 | | { |
| | 3 | 924 | | result = default; |
| | 3 | 925 | | return false; |
| | | 926 | | } |
| | | 927 | | |
| | 6492 | 928 | | return Fixed64.TryAdd( |
| | 6492 | 929 | | firstSquared, |
| | 6492 | 930 | | secondSquared, |
| | 6492 | 931 | | out result); |
| | | 932 | | } |
| | | 933 | | |
| | | 934 | | private static bool TryGetSquare(Fixed64 value, out Fixed64 square) => |
| | 19485 | 935 | | Fixed64.TryMultiplyDivide( |
| | 19485 | 936 | | value, |
| | 19485 | 937 | | value, |
| | 19485 | 938 | | Fixed64.One, |
| | 19485 | 939 | | out square) |
| | 19485 | 940 | | && (value == Fixed64.Zero || square != Fixed64.Zero); |
| | | 941 | | |
| | | 942 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 943 | | private static Vector3d ResolveLinearVelocity(SolidBody body) => |
| | 35294 | 944 | | body.ProjectLinearMotion( |
| | 35294 | 945 | | body.IsKinematic |
| | 35294 | 946 | | ? body.SampleContinuousCollisionLinearVelocity(Fixed64.One) |
| | 35294 | 947 | | : body.LinearVelocity); |
| | | 948 | | |
| | | 949 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 950 | | private static Vector3d ResolveAngularVelocity(SolidBody body) => |
| | 35294 | 951 | | body.ProjectAngularMotion( |
| | 35294 | 952 | | body.IsKinematic |
| | 35294 | 953 | | ? body.SampleContinuousCollisionAngularVelocity(Fixed64.One) |
| | 35294 | 954 | | : body.AngularVelocity); |
| | | 955 | | |
| | | 956 | | private static bool TryApplyExactImpulse( |
| | | 957 | | SolverContact contact, |
| | | 958 | | in ExactLever3D exactA, |
| | | 959 | | in ExactLever3D exactB, |
| | | 960 | | Vector3d impulseB) |
| | | 961 | | { |
| | 48 | 962 | | Vector3d impulseA = -impulseB; |
| | 48 | 963 | | bool linearAResolved = TryGetLinearVelocityDelta( |
| | 48 | 964 | | contact.A, |
| | 48 | 965 | | impulseA, |
| | 48 | 966 | | out Vector3d linearA); |
| | 48 | 967 | | bool angularAResolved = |
| | 48 | 968 | | ExactContactLever3D.TryGetAngularVelocityDelta( |
| | 48 | 969 | | contact.A.Body, |
| | 48 | 970 | | exactA, |
| | 48 | 971 | | impulseA, |
| | 48 | 972 | | out Vector3d angularA); |
| | 48 | 973 | | bool linearBResolved = TryGetLinearVelocityDelta( |
| | 48 | 974 | | contact.B, |
| | 48 | 975 | | impulseB, |
| | 48 | 976 | | out Vector3d linearB); |
| | 48 | 977 | | bool angularBResolved = |
| | 48 | 978 | | ExactContactLever3D.TryGetAngularVelocityDelta( |
| | 48 | 979 | | contact.B.Body, |
| | 48 | 980 | | exactB, |
| | 48 | 981 | | impulseB, |
| | 48 | 982 | | out Vector3d angularB); |
| | 48 | 983 | | if (!(linearAResolved |
| | 48 | 984 | | & angularAResolved |
| | 48 | 985 | | & linearBResolved |
| | 48 | 986 | | & angularBResolved)) |
| | | 987 | | { |
| | 4 | 988 | | return false; |
| | | 989 | | } |
| | | 990 | | |
| | 44 | 991 | | return TryApplyVelocityDeltas( |
| | 44 | 992 | | contact, |
| | 44 | 993 | | linearA, |
| | 44 | 994 | | angularA, |
| | 44 | 995 | | linearB, |
| | 44 | 996 | | angularB); |
| | | 997 | | } |
| | | 998 | | |
| | | 999 | | private static bool TryApplyCompactImpulse( |
| | | 1000 | | SolverContact contact, |
| | | 1001 | | Vector3d impulseB) |
| | | 1002 | | { |
| | 6165 | 1003 | | Vector3d impulseA = -impulseB; |
| | 6165 | 1004 | | bool linearAResolved = TryGetLinearVelocityDelta( |
| | 6165 | 1005 | | contact.A, |
| | 6165 | 1006 | | impulseA, |
| | 6165 | 1007 | | out Vector3d linearA); |
| | 6165 | 1008 | | bool angularAResolved = TryGetAngularVelocityDelta( |
| | 6165 | 1009 | | contact.A, |
| | 6165 | 1010 | | contact.RelativeA.Vector, |
| | 6165 | 1011 | | impulseA, |
| | 6165 | 1012 | | out Vector3d angularA); |
| | 6165 | 1013 | | bool linearBResolved = TryGetLinearVelocityDelta( |
| | 6165 | 1014 | | contact.B, |
| | 6165 | 1015 | | impulseB, |
| | 6165 | 1016 | | out Vector3d linearB); |
| | 6165 | 1017 | | bool angularBResolved = TryGetAngularVelocityDelta( |
| | 6165 | 1018 | | contact.B, |
| | 6165 | 1019 | | contact.RelativeB.Vector, |
| | 6165 | 1020 | | impulseB, |
| | 6165 | 1021 | | out Vector3d angularB); |
| | 6165 | 1022 | | if (!(linearAResolved |
| | 6165 | 1023 | | & angularAResolved |
| | 6165 | 1024 | | & linearBResolved |
| | 6165 | 1025 | | & angularBResolved)) |
| | | 1026 | | { |
| | 45 | 1027 | | return false; |
| | | 1028 | | } |
| | | 1029 | | |
| | 6120 | 1030 | | return TryApplyVelocityDeltas( |
| | 6120 | 1031 | | contact, |
| | 6120 | 1032 | | linearA, |
| | 6120 | 1033 | | angularA, |
| | 6120 | 1034 | | linearB, |
| | 6120 | 1035 | | angularB); |
| | | 1036 | | } |
| | | 1037 | | |
| | | 1038 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1039 | | private static bool CanNegate(Vector3d value) => |
| | 6169 | 1040 | | value.X != Fixed64.MinValue |
| | 6169 | 1041 | | && value.Y != Fixed64.MinValue |
| | 6169 | 1042 | | && value.Z != Fixed64.MinValue; |
| | | 1043 | | |
| | | 1044 | | private static bool TryGetLinearVelocityDelta( |
| | | 1045 | | ResponseBody body, |
| | | 1046 | | Vector3d impulse, |
| | | 1047 | | out Vector3d velocityDelta) |
| | | 1048 | | { |
| | 12426 | 1049 | | if (!body.HasSolverMobility || !body.Body.CanTranslate) |
| | | 1050 | | { |
| | 30 | 1051 | | velocityDelta = Vector3d.Zero; |
| | 30 | 1052 | | return true; |
| | | 1053 | | } |
| | | 1054 | | |
| | 12396 | 1055 | | return ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta( |
| | 12396 | 1056 | | body.Body.ProjectLinearMotion(impulse), |
| | 12396 | 1057 | | Fixed64.One, |
| | 12396 | 1058 | | body.InverseMass, |
| | 12396 | 1059 | | Fixed64.One, |
| | 12396 | 1060 | | out velocityDelta); |
| | | 1061 | | } |
| | | 1062 | | |
| | | 1063 | | private static bool TryGetAngularVelocityDelta( |
| | | 1064 | | ResponseBody body, |
| | | 1065 | | Vector3d relativeContactPoint, |
| | | 1066 | | Vector3d impulse, |
| | | 1067 | | out Vector3d velocityDelta) |
| | | 1068 | | { |
| | 12330 | 1069 | | velocityDelta = Vector3d.Zero; |
| | 12330 | 1070 | | if (!body.HasSolverMobility || !body.CanRotate) |
| | 41 | 1071 | | return true; |
| | | 1072 | | |
| | 12289 | 1073 | | Fixed3x3 inverseInertia = |
| | 12289 | 1074 | | body.Body.GetConstrainedInverseInertiaTensor(); |
| | 12289 | 1075 | | if (ContactResponseArithmetic3D.CanUseFastAngularResponse( |
| | 12289 | 1076 | | relativeContactPoint, |
| | 12289 | 1077 | | impulse, |
| | 12289 | 1078 | | inverseInertia)) |
| | | 1079 | | { |
| | 12285 | 1080 | | Vector3d fastTorqueAxis = |
| | 12285 | 1081 | | Vector3d.Cross(relativeContactPoint, impulse); |
| | 12285 | 1082 | | velocityDelta = Fixed3x3.TransformDirection( |
| | 12285 | 1083 | | inverseInertia, |
| | 12285 | 1084 | | fastTorqueAxis); |
| | 12285 | 1085 | | return ContactResponseArithmetic3D |
| | 12285 | 1086 | | .PreservesNonzeroCrossProduct( |
| | 12285 | 1087 | | relativeContactPoint, |
| | 12285 | 1088 | | impulse, |
| | 12285 | 1089 | | fastTorqueAxis) |
| | 12285 | 1090 | | && ContactResponseArithmetic3D |
| | 12285 | 1091 | | .PreservesNonzeroTransformDirection( |
| | 12285 | 1092 | | inverseInertia, |
| | 12285 | 1093 | | fastTorqueAxis, |
| | 12285 | 1094 | | velocityDelta); |
| | | 1095 | | } |
| | | 1096 | | |
| | 4 | 1097 | | if (!ContactResponseArithmetic3D.TryCross( |
| | 4 | 1098 | | relativeContactPoint, |
| | 4 | 1099 | | impulse, |
| | 4 | 1100 | | out Vector3d torqueAxis)) |
| | | 1101 | | { |
| | 1 | 1102 | | return false; |
| | | 1103 | | } |
| | | 1104 | | |
| | 3 | 1105 | | return ContactResponseArithmetic3D.TryTransformDirection( |
| | 3 | 1106 | | inverseInertia, |
| | 3 | 1107 | | torqueAxis, |
| | 3 | 1108 | | out velocityDelta); |
| | | 1109 | | } |
| | | 1110 | | |
| | | 1111 | | private static bool TryPrepareVelocityStates( |
| | | 1112 | | SolverContact contact, |
| | | 1113 | | Vector3d linearA, |
| | | 1114 | | Vector3d angularA, |
| | | 1115 | | Vector3d linearB, |
| | | 1116 | | Vector3d angularB, |
| | | 1117 | | out Vector3d preparedLinearA, |
| | | 1118 | | out Vector3d preparedAngularA, |
| | | 1119 | | out Vector3d preparedLinearB, |
| | | 1120 | | out Vector3d preparedAngularB) |
| | | 1121 | | { |
| | 13350 | 1122 | | bool firstPrepared = |
| | 13350 | 1123 | | contact.A.Body.TryPrepareCollisionVelocityState( |
| | 13350 | 1124 | | linearA, |
| | 13350 | 1125 | | angularA, |
| | 13350 | 1126 | | out preparedLinearA, |
| | 13350 | 1127 | | out preparedAngularA); |
| | 13350 | 1128 | | bool secondPrepared = |
| | 13350 | 1129 | | contact.B.Body.TryPrepareCollisionVelocityState( |
| | 13350 | 1130 | | linearB, |
| | 13350 | 1131 | | angularB, |
| | 13350 | 1132 | | out preparedLinearB, |
| | 13350 | 1133 | | out preparedAngularB); |
| | 13350 | 1134 | | return firstPrepared & secondPrepared; |
| | | 1135 | | } |
| | | 1136 | | |
| | | 1137 | | private static bool TryApplyVelocityDeltas( |
| | | 1138 | | SolverContact contact, |
| | | 1139 | | Vector3d linearA, |
| | | 1140 | | Vector3d angularA, |
| | | 1141 | | Vector3d linearB, |
| | | 1142 | | Vector3d angularB) |
| | | 1143 | | { |
| | 6518 | 1144 | | if (!TryPrepareVelocityStates( |
| | 6518 | 1145 | | contact, |
| | 6518 | 1146 | | linearA, |
| | 6518 | 1147 | | angularA, |
| | 6518 | 1148 | | linearB, |
| | 6518 | 1149 | | angularB, |
| | 6518 | 1150 | | out Vector3d preparedLinearA, |
| | 6518 | 1151 | | out Vector3d preparedAngularA, |
| | 6518 | 1152 | | out Vector3d preparedLinearB, |
| | 6518 | 1153 | | out Vector3d preparedAngularB)) |
| | | 1154 | | { |
| | 1 | 1155 | | return false; |
| | | 1156 | | } |
| | | 1157 | | |
| | 6517 | 1158 | | ApplyVelocityStates( |
| | 6517 | 1159 | | contact, |
| | 6517 | 1160 | | preparedLinearA, |
| | 6517 | 1161 | | preparedAngularA, |
| | 6517 | 1162 | | preparedLinearB, |
| | 6517 | 1163 | | preparedAngularB); |
| | 6517 | 1164 | | return true; |
| | | 1165 | | } |
| | | 1166 | | |
| | | 1167 | | private static void ApplyVelocityStates( |
| | | 1168 | | SolverContact contact, |
| | | 1169 | | Vector3d linearA, |
| | | 1170 | | Vector3d angularA, |
| | | 1171 | | Vector3d linearB, |
| | | 1172 | | Vector3d angularB) |
| | | 1173 | | { |
| | 13348 | 1174 | | contact.A.Body.ApplyCollisionVelocityState(linearA, angularA); |
| | 13348 | 1175 | | contact.B.Body.ApplyCollisionVelocityState(linearB, angularB); |
| | 13348 | 1176 | | } |
| | | 1177 | | |
| | | 1178 | | private static void GetExactLevers( |
| | | 1179 | | CollisionPair pair, |
| | | 1180 | | SolverContact contact, |
| | | 1181 | | Vector3d responsePositionA, |
| | | 1182 | | Vector3d responsePositionB, |
| | | 1183 | | out ExactLever3D exactA, |
| | | 1184 | | out ExactLever3D exactB) |
| | | 1185 | | { |
| | 1824 | 1186 | | ManifoldContact manifoldContact = |
| | 1824 | 1187 | | pair.Manifold[contact.ManifoldIndex]; |
| | 1824 | 1188 | | var centerA = new ContactAnchor( |
| | 1824 | 1189 | | responsePositionA, |
| | 1824 | 1190 | | contact.A.Body.Rotation, |
| | 1824 | 1191 | | contact.A.Body.LocalCenterOfMassOffset); |
| | 1824 | 1192 | | var centerB = new ContactAnchor( |
| | 1824 | 1193 | | responsePositionB, |
| | 1824 | 1194 | | contact.B.Body.Rotation, |
| | 1824 | 1195 | | contact.B.Body.LocalCenterOfMassOffset); |
| | 1824 | 1196 | | exactA = manifoldContact.AnchorA.GetLeverFrom(centerA); |
| | 1824 | 1197 | | exactB = manifoldContact.AnchorB.GetLeverFrom(centerB); |
| | 1824 | 1198 | | } |
| | | 1199 | | |
| | | 1200 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1201 | | private static bool HasFailedResponse(byte failedResponseMask, int contactIndex) => |
| | 27231 | 1202 | | (failedResponseMask & (1 << contactIndex)) != 0; |
| | | 1203 | | |
| | | 1204 | | private static void RejectResponse( |
| | | 1205 | | CollisionPair pair, |
| | | 1206 | | SolverContact contact, |
| | | 1207 | | int contactIndex, |
| | | 1208 | | ref byte failedResponseMask) |
| | | 1209 | | { |
| | 4 | 1210 | | failedResponseMask |= (byte)(1 << contactIndex); |
| | 4 | 1211 | | ClearWarmStartImpulse(pair, contact); |
| | 4 | 1212 | | GravitasLogger.Channel.Error( |
| | 4 | 1213 | | $"Contact response is outside the representable velocity domain."); |
| | 4 | 1214 | | } |
| | | 1215 | | |
| | | 1216 | | private static void ClearWarmStartImpulse( |
| | | 1217 | | CollisionPair pair, |
| | | 1218 | | SolverContact contact) => |
| | 11 | 1219 | | pair.RemoveWarmStartImpulse(contact.ContactId); |
| | | 1220 | | |
| | | 1221 | | private static bool IsWarmStartCompatible(Vector3d cachedNormal, Vector3d normal) => |
| | 8053 | 1222 | | Vector3d.Dot(cachedNormal, normal) >= WarmStartNormalCompatibilityThreshold; |
| | | 1223 | | |
| | | 1224 | | private static Vector3d ResolveContactNormal(Vector3d normal, Vector3d fallbackDirection) |
| | | 1225 | | { |
| | 9085 | 1226 | | Vector3d resolved = normal.MagnitudeSquared > Fixed64.Epsilon |
| | 9085 | 1227 | | ? normal.Normalized |
| | 9085 | 1228 | | : fallbackDirection.MagnitudeSquared > Fixed64.Epsilon |
| | 9085 | 1229 | | ? fallbackDirection.Normalized |
| | 9085 | 1230 | | : Vector3d.Zero; |
| | | 1231 | | |
| | 9085 | 1232 | | if (resolved == Vector3d.Zero) |
| | 1 | 1233 | | return resolved; |
| | | 1234 | | |
| | 9084 | 1235 | | return fallbackDirection.MagnitudeSquared > Fixed64.Epsilon |
| | 9084 | 1236 | | && Vector3d.Dot(resolved, fallbackDirection) < Fixed64.Zero |
| | 9084 | 1237 | | ? -resolved |
| | 9084 | 1238 | | : resolved; |
| | | 1239 | | } |
| | | 1240 | | } |