| | | 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 System.Runtime.CompilerServices; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using Gravitas.Materials; |
| | | 11 | | |
| | | 12 | | namespace Gravitas.CollisionHandling; |
| | | 13 | | |
| | | 14 | | /// <summary> |
| | | 15 | | /// Deterministic pure 2D manifold contact response. |
| | | 16 | | /// </summary> |
| | | 17 | | public static partial class CollisionResponse2D |
| | | 18 | | { |
| | | 19 | | /// <summary>Penetration tolerated before positional correction is applied.</summary> |
| | 1 | 20 | | public static readonly Fixed64 PenetrationSlop = (Fixed64)0.01f; |
| | | 21 | | |
| | | 22 | | /// <summary>Fraction of excess penetration corrected by the solver.</summary> |
| | 1 | 23 | | public static readonly Fixed64 PenetrationCorrectionPercent = Fixed64.One; |
| | | 24 | | |
| | | 25 | | |
| | | 26 | | |
| | | 27 | | internal static void PrepareSolve(CollisionPair2D pair) |
| | | 28 | | { |
| | 118 | 29 | | pair.ClearResponseSnapshot(); |
| | 118 | 30 | | if (pair.Manifold.GroupCount <= 1 |
| | 122 | 31 | | || !TryCreateBodyPair(pair, out ResponseBody2D bodyA, out ResponseBody2D bodyB)) return; |
| | 114 | 32 | | pair.RetainWarmStart(); |
| | 114 | 33 | | GroupedSolveState state = pair.ResponseState ??= new GroupedSolveState(); |
| | 114 | 34 | | state.CenterA = bodyA.Body?.GetCenterOfMassAnchor() ?? default; |
| | 114 | 35 | | state.CenterB = bodyB.Body?.GetCenterOfMassAnchor() ?? default; |
| | 114 | 36 | | state.Impact = new ContactResponseSnapshot( |
| | 114 | 37 | | ExactContactLever2D.ToSpatial(bodyA.Body?.Position ?? Vector2d.Zero), |
| | 114 | 38 | | ExactContactLever2D.ToSpatial(bodyB.Body?.Position ?? Vector2d.Zero), default, default, |
| | 114 | 39 | | ExactContactLever2D.ToSpatial(ResolveLinearVelocity(bodyA.Body)), Vector3d.Zero, |
| | 114 | 40 | | ExactContactLever2D.ToSpatial(ResolveLinearVelocity(bodyB.Body)), Vector3d.Zero, |
| | 114 | 41 | | planarAngularB: ResolveAngularVelocity(bodyB.Body), |
| | 114 | 42 | | planarAngularA: ResolveAngularVelocity(bodyA.Body)); |
| | 114 | 43 | | } |
| | | 44 | | |
| | | 45 | | internal static void Resolve(CollisionPair2D pair) |
| | | 46 | | { |
| | 5073 | 47 | | bool grouped = pair.Manifold.GroupCount > 1; |
| | 5177 | 48 | | if (grouped) PrepareSolve(pair); |
| | 5073 | 49 | | Resolve(pair, applyCachedImpulse: true, applyPositionCorrection: true, iterateGroups: true); |
| | 5167 | 50 | | if (grouped) pair.ClearResponseSnapshot(); |
| | 5063 | 51 | | } |
| | | 52 | | |
| | | 53 | | internal static void Resolve(CollisionPair2D pair, bool applyCachedImpulse, bool applyPositionCorrection) => |
| | 4603 | 54 | | Resolve(pair, applyCachedImpulse, applyPositionCorrection, iterateGroups: false); |
| | | 55 | | |
| | | 56 | | private static void Resolve(CollisionPair2D pair, bool applyCachedImpulse, |
| | | 57 | | bool applyPositionCorrection, bool iterateGroups) |
| | | 58 | | { |
| | 15341 | 59 | | if (applyPositionCorrection) pair.ApplyRecovery(); |
| | 9668 | 60 | | if (!TryCreateBodyPair(pair, out ResponseBody2D bodyA, out ResponseBody2D bodyB)) |
| | 15 | 61 | | return; |
| | | 62 | | |
| | 9653 | 63 | | if (pair.Manifold.GroupCount == 1) |
| | | 64 | | { |
| | 9529 | 65 | | pair.RetainWarmStart(); |
| | 9529 | 66 | | ResolveGroup(pair, bodyA, bodyB, 0, applyCachedImpulse, applyPositionCorrection, |
| | 9529 | 67 | | bodyA.Body?.GetCenterOfMassAnchor() ?? default, |
| | 9529 | 68 | | bodyB.Body?.GetCenterOfMassAnchor() ?? default, |
| | 9529 | 69 | | bodyA.Body?.Position ?? Vector2d.Zero, |
| | 9529 | 70 | | bodyB.Body?.Position ?? Vector2d.Zero, default); |
| | 9527 | 71 | | return; |
| | | 72 | | } |
| | 124 | 73 | | ResolveGroups(pair, bodyA, bodyB, applyCachedImpulse, applyPositionCorrection, iterateGroups); |
| | 124 | 74 | | } |
| | | 75 | | |
| | | 76 | | private static void ResolveGroups(CollisionPair2D pair, ResponseBody2D bodyA, ResponseBody2D bodyB, |
| | | 77 | | bool applyCachedImpulse, bool applyPositionCorrection, bool iterateGroups) |
| | | 78 | | { |
| | | 79 | | // Standalone and island owners prepare the grouped incoming state |
| | | 80 | | // before any row or joint changes a participating body's velocity. |
| | 124 | 81 | | GroupedSolveState state = pair.ResponseState!; |
| | 248 | 82 | | ContactAnchor2D centerA = state.CenterA, centerB = state.CenterB; |
| | 124 | 83 | | ref readonly ContactResponseSnapshot impact = ref state.Impact; |
| | 124 | 84 | | Vector2d positionA = ExactContactLever2D.ToPlanar(impact.PositionA); |
| | 124 | 85 | | Vector2d positionB = ExactContactLever2D.ToPlanar(impact.PositionB); |
| | 124 | 86 | | if (applyCachedImpulse || applyPositionCorrection) |
| | | 87 | | { |
| | | 88 | | // Warm all groups before solving any row: later warm starts must |
| | | 89 | | // not overwrite the state against which an earlier group was solved. |
| | 998 | 90 | | for (int g = 0; g < pair.Manifold.GroupCount; g++) |
| | | 91 | | { |
| | 389 | 92 | | ref ContactGroup2D group = ref pair.Manifold.GetGroup(g); |
| | 1790 | 93 | | for (int point = 0; point < group.Count; point++) |
| | | 94 | | { |
| | 506 | 95 | | if (!TryCreateContact(pair, bodyA, bodyB, centerA, centerB, |
| | 506 | 96 | | g, point, out SolverContact2D contact)) continue; |
| | 505 | 97 | | if (applyPositionCorrection && IsCorrectionOwner(pair, contact)) |
| | 131 | 98 | | ApplyPositionCorrection(contact, Fixed64.One); |
| | 505 | 99 | | if (applyCachedImpulse && !TryApplyCachedImpulse(pair, contact, positionA, positionB)) |
| | 1 | 100 | | pair.RemoveWarmStartImpulse(contact.Group, contact.ContactId); |
| | | 101 | | } |
| | | 102 | | } |
| | | 103 | | } |
| | 124 | 104 | | int iterations = iterateGroups |
| | 124 | 105 | | ? pair.ColliderA.Context.Settings.DiscreteSolverIterations : 1; |
| | 1496 | 106 | | for (int iteration = 0; iteration < iterations; iteration++) |
| | | 107 | | { |
| | 5774 | 108 | | for (int group = 0; group < pair.Manifold.GroupCount; group++) |
| | | 109 | | { |
| | 2263 | 110 | | ResolveGroup(pair, bodyA, bodyB, group, |
| | 2263 | 111 | | applyCachedImpulse: false, applyPositionCorrection: false, |
| | 2263 | 112 | | centerA, centerB, positionA, positionB, impact); |
| | | 113 | | } |
| | | 114 | | } |
| | 124 | 115 | | } |
| | | 116 | | |
| | | 117 | | private static void ResolveGroup( |
| | | 118 | | CollisionPair2D pair, |
| | | 119 | | ResponseBody2D bodyA, |
| | | 120 | | ResponseBody2D bodyB, |
| | | 121 | | int groupIndex, |
| | | 122 | | bool applyCachedImpulse, |
| | | 123 | | bool applyPositionCorrection, |
| | | 124 | | in ContactAnchor2D responseCenterA, |
| | | 125 | | in ContactAnchor2D responseCenterB, |
| | | 126 | | Vector2d responsePositionA, |
| | | 127 | | Vector2d responsePositionB, |
| | | 128 | | in ContactResponseSnapshot impact) |
| | | 129 | | { |
| | 11792 | 130 | | SolverContactBuffer2D contacts = BuildContactBuffer( |
| | 11792 | 131 | | pair, |
| | 11792 | 132 | | bodyA, |
| | 11792 | 133 | | bodyB, |
| | 11792 | 134 | | responseCenterA, |
| | 11792 | 135 | | responseCenterB, groupIndex); |
| | 11792 | 136 | | if (contacts.Count == 0) |
| | 5 | 137 | | return; |
| | | 138 | | |
| | 11787 | 139 | | byte failedResponseMask = 0; |
| | 11787 | 140 | | Fixed64 contactShare = impact.IsValid ? Fixed64.One : Fixed64.One / (Fixed64)contacts.Count; |
| | 11787 | 141 | | if (applyPositionCorrection) |
| | 5532 | 142 | | ApplyPositionCorrections(ref contacts, contactShare); |
| | | 143 | | |
| | 11785 | 144 | | if (applyCachedImpulse) |
| | | 145 | | { |
| | 5535 | 146 | | bool rebuildContacts = false; |
| | 23210 | 147 | | for (int i = 0; i < contacts.Count; i++) |
| | | 148 | | { |
| | 6070 | 149 | | SolverContact2D contact = contacts.GetContact(i); |
| | 6070 | 150 | | if (TryApplyCachedImpulse( |
| | 6070 | 151 | | pair, |
| | 6070 | 152 | | contact, |
| | 6070 | 153 | | responsePositionA, |
| | 6070 | 154 | | responsePositionB)) |
| | | 155 | | { |
| | | 156 | | continue; |
| | | 157 | | } |
| | | 158 | | |
| | 2 | 159 | | pair.RemoveWarmStartImpulse(contact.Group, contact.ContactId); |
| | 2 | 160 | | rebuildContacts = true; |
| | | 161 | | } |
| | | 162 | | |
| | 5535 | 163 | | if (rebuildContacts) |
| | | 164 | | { |
| | 2 | 165 | | contacts = BuildContactBuffer( |
| | 2 | 166 | | pair, |
| | 2 | 167 | | bodyA, |
| | 2 | 168 | | bodyB, |
| | 2 | 169 | | responseCenterA, |
| | 2 | 170 | | responseCenterB, groupIndex); |
| | | 171 | | } |
| | | 172 | | } |
| | | 173 | | |
| | 11785 | 174 | | Vector2d normalLinearVelocityA = |
| | 11785 | 175 | | ResolveLinearVelocity(bodyA.Body); |
| | 11785 | 176 | | Fixed64 normalAngularVelocityA = |
| | 11785 | 177 | | ResolveAngularVelocity(bodyA.Body); |
| | 11785 | 178 | | Vector2d normalLinearVelocityB = |
| | 11785 | 179 | | ResolveLinearVelocity(bodyB.Body); |
| | 11785 | 180 | | Fixed64 normalAngularVelocityB = |
| | 11785 | 181 | | ResolveAngularVelocity(bodyB.Body); |
| | 11785 | 182 | | Fixed64 restitutionVelocityThreshold = pair.ColliderA.Context.Settings.RestitutionVelocityThreshold; |
| | 56382 | 183 | | for (int i = 0; i < contacts.Count; i++) |
| | | 184 | | { |
| | 16406 | 185 | | SolverContact2D contact = contacts.GetContact(i); |
| | 16406 | 186 | | if (impact.IsValid) |
| | | 187 | | { |
| | 2939 | 188 | | normalLinearVelocityA = ResolveLinearVelocity(bodyA.Body); |
| | 2939 | 189 | | normalAngularVelocityA = ResolveAngularVelocity(bodyA.Body); |
| | 2939 | 190 | | normalLinearVelocityB = ResolveLinearVelocity(bodyB.Body); |
| | 2939 | 191 | | normalAngularVelocityB = ResolveAngularVelocity(bodyB.Body); |
| | | 192 | | } |
| | 16406 | 193 | | SolidBody2D? contactBodyA = contact.A.Body; |
| | 16406 | 194 | | SolidBody2D? contactBodyB = contact.B.Body; |
| | | 195 | | bool normalResolved; |
| | 16406 | 196 | | ContactNormalImpulseResult2D normalResult = default; |
| | 16406 | 197 | | if (contact.RelativeA.IsExact || contact.RelativeB.IsExact |
| | 16406 | 198 | | || impact.IsValid && contact.Restitution != Fixed64.Zero) |
| | | 199 | | { |
| | 296 | 200 | | normalResolved = TryCalculateExactNormalResult( |
| | 296 | 201 | | pair, |
| | 296 | 202 | | contact, |
| | 296 | 203 | | responsePositionA, |
| | 296 | 204 | | responsePositionB, |
| | 296 | 205 | | normalLinearVelocityA, |
| | 296 | 206 | | normalAngularVelocityA, |
| | 296 | 207 | | normalLinearVelocityB, |
| | 296 | 208 | | normalAngularVelocityB, |
| | 296 | 209 | | restitutionVelocityThreshold, |
| | 296 | 210 | | contactShare, |
| | 296 | 211 | | out normalResult, impact); |
| | | 212 | | } |
| | | 213 | | else |
| | | 214 | | { |
| | 16110 | 215 | | normalResolved = CanUseCompactAxisResponse( |
| | 16110 | 216 | | contact, |
| | 16110 | 217 | | contact.Normal) |
| | 16110 | 218 | | && ContactNormalImpulse2D.TryCalculateAccumulatedDelta( |
| | 16110 | 219 | | contactBodyA, |
| | 16110 | 220 | | normalLinearVelocityA, |
| | 16110 | 221 | | normalAngularVelocityA, |
| | 16110 | 222 | | contact.RelativeA.Vector, |
| | 16110 | 223 | | contactBodyB, |
| | 16110 | 224 | | normalLinearVelocityB, |
| | 16110 | 225 | | normalAngularVelocityB, |
| | 16110 | 226 | | contact.RelativeB.Vector, |
| | 16110 | 227 | | contact.Normal, |
| | 16110 | 228 | | contact.Restitution, |
| | 16110 | 229 | | restitutionVelocityThreshold, |
| | 16110 | 230 | | contact.CachedNormalImpulse, |
| | 16110 | 231 | | contactShare, |
| | 16110 | 232 | | contactShare, |
| | 16110 | 233 | | out normalResult); |
| | 16110 | 234 | | if (!normalResolved) |
| | | 235 | | { |
| | 72 | 236 | | normalResolved = TryCalculateExactNormalResult( |
| | 72 | 237 | | pair, |
| | 72 | 238 | | contact, |
| | 72 | 239 | | responsePositionA, |
| | 72 | 240 | | responsePositionB, |
| | 72 | 241 | | normalLinearVelocityA, |
| | 72 | 242 | | normalAngularVelocityA, |
| | 72 | 243 | | normalLinearVelocityB, |
| | 72 | 244 | | normalAngularVelocityB, |
| | 72 | 245 | | restitutionVelocityThreshold, |
| | 72 | 246 | | contactShare, |
| | 72 | 247 | | out normalResult, impact); |
| | | 248 | | } |
| | | 249 | | } |
| | 16406 | 250 | | if (!normalResolved) |
| | | 251 | | { |
| | 2 | 252 | | RejectResponse(pair, contact, i, ref failedResponseMask); |
| | 2 | 253 | | continue; |
| | | 254 | | } |
| | 16404 | 255 | | if (impact.IsValid && !TryApplyNormalImpulse(contact, normalResult)) |
| | | 256 | | { |
| | 1 | 257 | | RejectResponse(pair, contact, i, ref failedResponseMask); |
| | 1 | 258 | | continue; |
| | | 259 | | } |
| | 16403 | 260 | | Fixed64 normalImpulse = contact.CachedNormalImpulse + normalResult.ImpulseScalar; |
| | 16403 | 261 | | contacts.SetNormalImpulse( |
| | 16403 | 262 | | i, |
| | 16403 | 263 | | normalImpulse, |
| | 16403 | 264 | | normalResult); |
| | 16403 | 265 | | if (impact.IsValid) |
| | | 266 | | { |
| | 2938 | 267 | | if (!TrySolveFrictionImpulse(pair, contact, responsePositionA, responsePositionB, |
| | 2938 | 268 | | normalLinearVelocityA, normalAngularVelocityA, normalLinearVelocityB, |
| | 2938 | 269 | | normalAngularVelocityB, restitutionVelocityThreshold, Fixed64.One, |
| | 2938 | 270 | | normalResult, normalImpulse, out Fixed64 tangentImpulse, impact)) |
| | | 271 | | { |
| | 2 | 272 | | RejectResponse(pair, contact, i, ref failedResponseMask); |
| | 2 | 273 | | continue; |
| | | 274 | | } |
| | 2936 | 275 | | pair.StoreWarmStartImpulse(contact.Group, contact.ContactId, normalImpulse, tangentImpulse); |
| | | 276 | | } |
| | | 277 | | } |
| | 14044 | 278 | | if (impact.IsValid) return; |
| | | 279 | | |
| | 45986 | 280 | | for (int i = 0; i < contacts.Count; i++) |
| | | 281 | | { |
| | 13467 | 282 | | if (HasFailedResponse(failedResponseMask, i)) |
| | | 283 | | continue; |
| | | 284 | | |
| | 13465 | 285 | | if (!TryApplyNormalImpulse( |
| | 13465 | 286 | | contacts.GetContact(i), |
| | 13465 | 287 | | contacts.GetNormalResult(i))) |
| | | 288 | | { |
| | 1 | 289 | | RejectResponse( |
| | 1 | 290 | | pair, |
| | 1 | 291 | | contacts.GetContact(i), |
| | 1 | 292 | | i, |
| | 1 | 293 | | ref failedResponseMask); |
| | | 294 | | } |
| | | 295 | | } |
| | | 296 | | |
| | 45986 | 297 | | for (int i = 0; i < contacts.Count; i++) |
| | | 298 | | { |
| | 13467 | 299 | | if (HasFailedResponse(failedResponseMask, i)) |
| | | 300 | | continue; |
| | | 301 | | |
| | 13464 | 302 | | SolverContact2D contact = contacts.GetContact(i); |
| | 13464 | 303 | | if (!TrySolveFrictionImpulse( |
| | 13464 | 304 | | pair, |
| | 13464 | 305 | | contact, |
| | 13464 | 306 | | responsePositionA, |
| | 13464 | 307 | | responsePositionB, |
| | 13464 | 308 | | normalLinearVelocityA, |
| | 13464 | 309 | | normalAngularVelocityA, |
| | 13464 | 310 | | normalLinearVelocityB, |
| | 13464 | 311 | | normalAngularVelocityB, |
| | 13464 | 312 | | restitutionVelocityThreshold, |
| | 13464 | 313 | | contactShare, |
| | 13464 | 314 | | contacts.GetNormalResult(i), |
| | 13464 | 315 | | contacts.GetNormalImpulse(i), |
| | 13464 | 316 | | out Fixed64 tangentImpulse)) |
| | | 317 | | { |
| | 2 | 318 | | RejectResponse(pair, contact, i, ref failedResponseMask); |
| | 2 | 319 | | continue; |
| | | 320 | | } |
| | 13462 | 321 | | contacts.SetTangentImpulse( |
| | 13462 | 322 | | i, |
| | 13462 | 323 | | tangentImpulse); |
| | | 324 | | } |
| | | 325 | | |
| | 45986 | 326 | | for (int i = 0; i < contacts.Count; i++) |
| | | 327 | | { |
| | 13467 | 328 | | if (HasFailedResponse(failedResponseMask, i)) |
| | | 329 | | continue; |
| | | 330 | | |
| | 13462 | 331 | | SolverContact2D contact = contacts.GetContact(i); |
| | 13462 | 332 | | pair.StoreWarmStartImpulse( |
| | 13462 | 333 | | contact.Group, contact.ContactId, |
| | 13462 | 334 | | contacts.GetNormalImpulse(i), |
| | 13462 | 335 | | contacts.GetTangentImpulse(i)); |
| | | 336 | | } |
| | 9526 | 337 | | } |
| | | 338 | | |
| | | 339 | | private static bool IsCorrectionOwner(CollisionPair2D pair, SolverContact2D contact) |
| | | 340 | | { |
| | | 341 | | // Translation is shared by equal-normal samples; retain every velocity |
| | | 342 | | // row, but apply the deepest directional correction only once. |
| | 1930 | 343 | | for (int g = 0; g < pair.Manifold.GroupCount; g++) |
| | | 344 | | { |
| | 834 | 345 | | ref ContactGroup2D group = ref pair.Manifold.GetGroup(g); |
| | 2866 | 346 | | for (int point = 0; point < group.Count; point++) |
| | | 347 | | { |
| | 971 | 348 | | ManifoldContact2D other = group[point]; |
| | 971 | 349 | | if (ResolveContactNormal(pair, group.Key, other.Normal) != contact.Normal) continue; |
| | 885 | 350 | | if (other.Depth > contact.Depth |
| | 885 | 351 | | || other.Depth == contact.Depth |
| | 1257 | 352 | | && (g < contact.GroupIndex || g == contact.GroupIndex && point < contact.PointIndex)) return false; |
| | | 353 | | } |
| | | 354 | | } |
| | 131 | 355 | | return true; |
| | | 356 | | } |
| | | 357 | | |
| | | 358 | | private static bool TryCreateBodyPair( |
| | | 359 | | CollisionPair2D pair, |
| | | 360 | | out ResponseBody2D bodyA, |
| | | 361 | | out ResponseBody2D bodyB) |
| | | 362 | | { |
| | 9782 | 363 | | bodyA = default; |
| | 9782 | 364 | | bodyB = default; |
| | | 365 | | |
| | 9782 | 366 | | if (pair.ColliderA.IsTrigger || pair.ColliderB.IsTrigger || !pair.Manifold.HasContact) |
| | 13 | 367 | | return false; |
| | | 368 | | |
| | 9769 | 369 | | bodyA = ResponseBody2D.Create(pair.ColliderA); |
| | 9769 | 370 | | bodyB = ResponseBody2D.Create(pair.ColliderB); |
| | 9769 | 371 | | return bodyA.HasSolverMobility || bodyB.HasSolverMobility; |
| | | 372 | | } |
| | | 373 | | |
| | | 374 | | private static SolverContactBuffer2D BuildContactBuffer( |
| | | 375 | | CollisionPair2D pair, |
| | | 376 | | ResponseBody2D bodyA, |
| | | 377 | | ResponseBody2D bodyB, |
| | | 378 | | in ContactAnchor2D responseCenterA, |
| | | 379 | | in ContactAnchor2D responseCenterB, |
| | | 380 | | int groupIndex) |
| | | 381 | | { |
| | 11798 | 382 | | SolverContactBuffer2D contacts = default; |
| | 56440 | 383 | | for (int i = 0; i < pair.Manifold.GetGroup(groupIndex).Count; i++) |
| | | 384 | | { |
| | 16422 | 385 | | if (TryCreateContact( |
| | 16422 | 386 | | pair, |
| | 16422 | 387 | | bodyA, |
| | 16422 | 388 | | bodyB, |
| | 16422 | 389 | | responseCenterA, |
| | 16422 | 390 | | responseCenterB, |
| | 16422 | 391 | | groupIndex, i, |
| | 16422 | 392 | | out SolverContact2D contact)) |
| | 16417 | 393 | | contacts.Add(contact); |
| | | 394 | | } |
| | | 395 | | |
| | 11798 | 396 | | return contacts; |
| | | 397 | | } |
| | | 398 | | |
| | | 399 | | private static bool TryCreateContact( |
| | | 400 | | CollisionPair2D pair, |
| | | 401 | | ResponseBody2D bodyA, |
| | | 402 | | ResponseBody2D bodyB, |
| | | 403 | | in ContactAnchor2D responseCenterA, |
| | | 404 | | in ContactAnchor2D responseCenterB, |
| | | 405 | | int groupIndex, |
| | | 406 | | int pointIndex, |
| | | 407 | | out SolverContact2D contact) |
| | | 408 | | { |
| | 17754 | 409 | | contact = default; |
| | 17754 | 410 | | ref ContactGroup2D surface = ref pair.Manifold.GetGroup(groupIndex); |
| | 17754 | 411 | | ManifoldContact2D manifoldContact = surface[pointIndex]; |
| | 17754 | 412 | | ContactGroupKey group = surface.Key; |
| | 17754 | 413 | | Vector2d normal = ResolveContactNormal(pair, group, manifoldContact.Normal); |
| | 17754 | 414 | | if (normal == Vector2d.Zero) |
| | 6 | 415 | | return false; |
| | 17748 | 416 | | ContactLever2D relativeA = bodyA.Body == null |
| | 17748 | 417 | | ? ContactLever2D.Zero |
| | 17748 | 418 | | : ContactLever2D.Create( |
| | 17748 | 419 | | manifoldContact.AnchorA, |
| | 17748 | 420 | | responseCenterA); |
| | 17748 | 421 | | ContactLever2D relativeB = bodyB.Body == null |
| | 17748 | 422 | | ? ContactLever2D.Zero |
| | 17748 | 423 | | : ContactLever2D.Create( |
| | 17748 | 424 | | manifoldContact.AnchorB, |
| | 17748 | 425 | | responseCenterB); |
| | 17748 | 426 | | PhysicsMaterial materialA = manifoldContact.HasMaterialOverride |
| | 17748 | 427 | | ? manifoldContact.MaterialA |
| | 17748 | 428 | | : pair.ColliderA.Material; |
| | 17748 | 429 | | PhysicsMaterial materialB = manifoldContact.HasMaterialOverride |
| | 17748 | 430 | | ? manifoldContact.MaterialB |
| | 17748 | 431 | | : pair.ColliderB.Material; |
| | | 432 | | |
| | 17748 | 433 | | Fixed64 cachedNormalImpulse = Fixed64.Zero; |
| | 17748 | 434 | | Fixed64 cachedTangentImpulse = Fixed64.Zero; |
| | 17748 | 435 | | if (pair.TryGetWarmStartImpulse(group, manifoldContact.ContactId, out ContactWarmStartImpulse cached)) |
| | | 436 | | { |
| | 13489 | 437 | | cachedNormalImpulse = cached.NormalImpulse; |
| | 13489 | 438 | | cachedTangentImpulse = cached.TangentImpulse; |
| | | 439 | | } |
| | | 440 | | |
| | 17748 | 441 | | contact = new SolverContact2D( |
| | 17748 | 442 | | groupIndex, pointIndex, |
| | 17748 | 443 | | manifoldContact.ContactId, |
| | 17748 | 444 | | bodyA, |
| | 17748 | 445 | | bodyB, |
| | 17748 | 446 | | relativeA, |
| | 17748 | 447 | | relativeB, |
| | 17748 | 448 | | manifoldContact.Depth, |
| | 17748 | 449 | | normal, |
| | 17748 | 450 | | materialA, |
| | 17748 | 451 | | materialB, |
| | 17748 | 452 | | cachedNormalImpulse, |
| | 17748 | 453 | | cachedTangentImpulse, group); |
| | 17748 | 454 | | return true; |
| | | 455 | | } |
| | | 456 | | |
| | | 457 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 458 | | private static bool TryCalculateExactNormalResult( |
| | | 459 | | CollisionPair2D pair, |
| | | 460 | | SolverContact2D contact, |
| | | 461 | | Vector2d responsePositionA, |
| | | 462 | | Vector2d responsePositionB, |
| | | 463 | | Vector2d linearVelocityA, |
| | | 464 | | Fixed64 angularVelocityA, |
| | | 465 | | Vector2d linearVelocityB, |
| | | 466 | | Fixed64 angularVelocityB, |
| | | 467 | | Fixed64 restitutionVelocityThreshold, |
| | | 468 | | Fixed64 contactShare, |
| | | 469 | | out ContactNormalImpulseResult2D result, |
| | | 470 | | in ContactResponseSnapshot impact = default) |
| | | 471 | | { |
| | 368 | 472 | | GetExactLevers( |
| | 368 | 473 | | pair, |
| | 368 | 474 | | contact, |
| | 368 | 475 | | responsePositionA, |
| | 368 | 476 | | responsePositionB, |
| | 368 | 477 | | out ExactLever3D exactA, |
| | 368 | 478 | | out ExactLever3D exactB); |
| | 368 | 479 | | return ContactNormalImpulse2D.TryCalculateAccumulatedDeltaExact( |
| | 368 | 480 | | contact.A.Body, |
| | 368 | 481 | | linearVelocityA, |
| | 368 | 482 | | angularVelocityA, |
| | 368 | 483 | | exactA, |
| | 368 | 484 | | contact.B.Body, |
| | 368 | 485 | | linearVelocityB, |
| | 368 | 486 | | angularVelocityB, |
| | 368 | 487 | | exactB, |
| | 368 | 488 | | contact.Normal, |
| | 368 | 489 | | contact.Restitution, |
| | 368 | 490 | | restitutionVelocityThreshold, |
| | 368 | 491 | | contact.CachedNormalImpulse, |
| | 368 | 492 | | contactShare, |
| | 368 | 493 | | contactShare, |
| | 368 | 494 | | out result, impact); |
| | | 495 | | } |
| | | 496 | | |
| | | 497 | | private static void ApplyPositionCorrections(ref SolverContactBuffer2D contacts, Fixed64 contactShare) |
| | | 498 | | { |
| | 23212 | 499 | | for (int i = 0; i < contacts.Count; i++) |
| | 6072 | 500 | | ApplyPositionCorrection(contacts.GetContact(i), contactShare); |
| | 5534 | 501 | | } |
| | | 502 | | |
| | | 503 | | private static void ApplyPositionCorrection(SolverContact2D contact, Fixed64 contactShare) |
| | | 504 | | { |
| | 6203 | 505 | | Fixed64 correctionDepth = contact.Depth - PenetrationSlop; |
| | 6203 | 506 | | if (correctionDepth <= Fixed64.Zero) |
| | 5120 | 507 | | return; |
| | | 508 | | |
| | 1083 | 509 | | Fixed64 inverseMassA = contact.A.GetConstrainedInverseMass(contact.Normal); |
| | 1083 | 510 | | Fixed64 inverseMassB = contact.B.GetConstrainedInverseMass(contact.Normal); |
| | 1083 | 511 | | Fixed64 totalInverseMass = inverseMassA + inverseMassB; |
| | 1083 | 512 | | if (totalInverseMass <= Fixed64.Zero) |
| | 11 | 513 | | return; |
| | | 514 | | |
| | 1072 | 515 | | Fixed64 weightedDepth = correctionDepth * PenetrationCorrectionPercent * contactShare; |
| | | 516 | | Vector2d correctionA, correctionB; |
| | 1072 | 517 | | if (CollisionPositionCorrection.TryGetCompactScale( |
| | 1072 | 518 | | weightedDepth, inverseMassA, inverseMassB, out Fixed64 scale)) |
| | | 519 | | { |
| | 1032 | 520 | | Vector2d correction = contact.Normal * scale; |
| | 1032 | 521 | | correctionA = -correction * inverseMassA; |
| | 1032 | 522 | | correctionB = correction * inverseMassB; |
| | | 523 | | } |
| | | 524 | | else |
| | | 525 | | { |
| | 40 | 526 | | correctionA = -contact.Normal * CollisionPositionCorrection.GetWeightedDistance( |
| | 40 | 527 | | weightedDepth, inverseMassA, inverseMassA, inverseMassB); |
| | 40 | 528 | | correctionB = contact.Normal * CollisionPositionCorrection.GetWeightedDistance( |
| | 40 | 529 | | weightedDepth, inverseMassB, inverseMassA, inverseMassB); |
| | | 530 | | } |
| | | 531 | | // Prepare both endpoints before publishing either participant's correction. |
| | 1072 | 532 | | Vector2d positionA = contact.A.Body?.PrepareCollisionPositionCorrection(correctionA) ?? default; |
| | 1072 | 533 | | Vector2d positionB = contact.B.Body?.PrepareCollisionPositionCorrection(correctionB) ?? default; |
| | 1070 | 534 | | contact.A.Body?.ApplyCollisionPositionState(positionA); |
| | 1070 | 535 | | contact.B.Body?.ApplyCollisionPositionState(positionB); |
| | 1062 | 536 | | } |
| | | 537 | | |
| | | 538 | | private static bool TryApplyCachedImpulse( |
| | | 539 | | CollisionPair2D pair, |
| | | 540 | | SolverContact2D contact, |
| | | 541 | | Vector2d responsePositionA, |
| | | 542 | | Vector2d responsePositionB) |
| | | 543 | | { |
| | 6573 | 544 | | if (contact.CachedNormalImpulse == Fixed64.Zero && contact.CachedTangentImpulse == Fixed64.Zero) |
| | 6316 | 545 | | return true; |
| | | 546 | | |
| | 257 | 547 | | return TryApplyContactImpulseCombination( |
| | 257 | 548 | | pair, |
| | 257 | 549 | | contact, |
| | 257 | 550 | | responsePositionA, |
| | 257 | 551 | | responsePositionB, |
| | 257 | 552 | | contact.Normal, |
| | 257 | 553 | | contact.CachedNormalImpulse, |
| | 257 | 554 | | contact.Tangent, |
| | 257 | 555 | | contact.CachedTangentImpulse); |
| | | 556 | | } |
| | | 557 | | |
| | | 558 | | private static bool TryApplyNormalImpulse( |
| | | 559 | | SolverContact2D contact, |
| | | 560 | | ContactNormalImpulseResult2D result) |
| | | 561 | | { |
| | 16404 | 562 | | if (result.LinearVelocityDeltaA == Vector2d.Zero |
| | 16404 | 563 | | && result.AngularVelocityDeltaA == Fixed64.Zero |
| | 16404 | 564 | | && result.LinearVelocityDeltaB == Vector2d.Zero |
| | 16404 | 565 | | && result.AngularVelocityDeltaB == Fixed64.Zero) |
| | | 566 | | { |
| | 15966 | 567 | | return true; |
| | | 568 | | } |
| | | 569 | | |
| | 438 | 570 | | return TryApplyVelocityDeltas( |
| | 438 | 571 | | contact, |
| | 438 | 572 | | result.LinearVelocityDeltaA, |
| | 438 | 573 | | result.AngularVelocityDeltaA, |
| | 438 | 574 | | result.LinearVelocityDeltaB, |
| | 438 | 575 | | result.AngularVelocityDeltaB); |
| | | 576 | | } |
| | | 577 | | |
| | | 578 | | private static void ApplyVelocityDelta( |
| | | 579 | | ResponseBody2D body, |
| | | 580 | | Vector2d linearVelocityDelta, |
| | | 581 | | Fixed64 angularVelocityDelta) |
| | | 582 | | { |
| | 1534 | 583 | | if (!body.HasSolverMobility) |
| | 559 | 584 | | return; |
| | | 585 | | |
| | 975 | 586 | | if (body.CanTranslate) |
| | 971 | 587 | | body.Body!.ApplyCollisionLinearVelocityDelta(linearVelocityDelta); |
| | 975 | 588 | | if (body.CanRotate) |
| | 780 | 589 | | body.Body!.ApplyCollisionAngularVelocityDelta(angularVelocityDelta); |
| | 975 | 590 | | } |
| | | 591 | | |
| | | 592 | | private static bool TrySolveFrictionImpulse( |
| | | 593 | | CollisionPair2D pair, |
| | | 594 | | SolverContact2D contact, |
| | | 595 | | Vector2d responsePositionA, |
| | | 596 | | Vector2d responsePositionB, |
| | | 597 | | Vector2d normalLinearVelocityA, |
| | | 598 | | Fixed64 normalAngularVelocityA, |
| | | 599 | | Vector2d normalLinearVelocityB, |
| | | 600 | | Fixed64 normalAngularVelocityB, |
| | | 601 | | Fixed64 restitutionVelocityThreshold, |
| | | 602 | | Fixed64 contactShare, |
| | | 603 | | ContactNormalImpulseResult2D normalResult, |
| | | 604 | | Fixed64 normalImpulseScalar, |
| | | 605 | | out Fixed64 accumulated, |
| | | 606 | | in ContactResponseSnapshot impact = default) |
| | | 607 | | { |
| | 16402 | 608 | | accumulated = default; |
| | 16402 | 609 | | if (contact.RelativeA.IsExact |
| | 16402 | 610 | | || contact.RelativeB.IsExact |
| | 16402 | 611 | | || !normalResult.HasRepresentableAccumulatedImpulse |
| | 16402 | 612 | | || !CanUseCompactAxisResponse(contact, contact.Tangent) |
| | 16402 | 613 | | || !Fixed64.TryMultiplyDivide( |
| | 16402 | 614 | | normalImpulseScalar, |
| | 16402 | 615 | | contact.StaticFriction, |
| | 16402 | 616 | | Fixed64.One, |
| | 16402 | 617 | | out Fixed64 staticFrictionLimit) |
| | 16402 | 618 | | || !Fixed64.TryMultiplyDivide( |
| | 16402 | 619 | | normalImpulseScalar, |
| | 16402 | 620 | | contact.DynamicFriction, |
| | 16402 | 621 | | Fixed64.One, |
| | 16402 | 622 | | out Fixed64 dynamicFrictionLimit)) |
| | | 623 | | { |
| | 52 | 624 | | return TrySolveFrictionImpulseExact( |
| | 52 | 625 | | pair, |
| | 52 | 626 | | contact, |
| | 52 | 627 | | responsePositionA, |
| | 52 | 628 | | responsePositionB, |
| | 52 | 629 | | normalLinearVelocityA, |
| | 52 | 630 | | normalAngularVelocityA, |
| | 52 | 631 | | normalLinearVelocityB, |
| | 52 | 632 | | normalAngularVelocityB, |
| | 52 | 633 | | restitutionVelocityThreshold, |
| | 52 | 634 | | contactShare, |
| | 52 | 635 | | out accumulated, impact); |
| | | 636 | | } |
| | | 637 | | |
| | 16350 | 638 | | Fixed64 impulseScalar = Fixed64.Zero; |
| | 16350 | 639 | | if (staticFrictionLimit > Fixed64.Zero || dynamicFrictionLimit > Fixed64.Zero) |
| | | 640 | | { |
| | 2300 | 641 | | Vector3d spatialTangent = |
| | 2300 | 642 | | ExactContactLever2D.ToSpatial(contact.Tangent); |
| | 2300 | 643 | | Fixed64 tangentVelocity = default; |
| | 2300 | 644 | | bool tangentVelocityResolved = |
| | 2300 | 645 | | ContactResponseArithmetic3D.TryGetRelativePointVelocity( |
| | 2300 | 646 | | ExactContactLever2D.ToSpatial( |
| | 2300 | 647 | | ResolveLinearVelocity(contact.A.Body)), |
| | 2300 | 648 | | new Vector3d( |
| | 2300 | 649 | | Fixed64.Zero, |
| | 2300 | 650 | | -ResolveAngularVelocity(contact.A.Body), |
| | 2300 | 651 | | Fixed64.Zero), |
| | 2300 | 652 | | ExactContactLever2D.ToSpatial(contact.RelativeA.Vector), |
| | 2300 | 653 | | ExactContactLever2D.ToSpatial( |
| | 2300 | 654 | | ResolveLinearVelocity(contact.B.Body)), |
| | 2300 | 655 | | new Vector3d( |
| | 2300 | 656 | | Fixed64.Zero, |
| | 2300 | 657 | | -ResolveAngularVelocity(contact.B.Body), |
| | 2300 | 658 | | Fixed64.Zero), |
| | 2300 | 659 | | ExactContactLever2D.ToSpatial(contact.RelativeB.Vector), |
| | 2300 | 660 | | spatialTangent, |
| | 2300 | 661 | | out Vector3d relativeVelocity) |
| | 2300 | 662 | | && ContactResponseArithmetic3D.TryDot( |
| | 2300 | 663 | | relativeVelocity, |
| | 2300 | 664 | | spatialTangent, |
| | 2300 | 665 | | out tangentVelocity); |
| | 2300 | 666 | | bool denominatorResolved = |
| | 2300 | 667 | | ContactNormalImpulse2D.TryComputeDenominator( |
| | 2300 | 668 | | contact.A.Body, |
| | 2300 | 669 | | contact.RelativeA.Vector, |
| | 2300 | 670 | | contact.B.Body, |
| | 2300 | 671 | | contact.RelativeB.Vector, |
| | 2300 | 672 | | contact.Tangent, |
| | 2300 | 673 | | out Fixed64 denominator); |
| | 2300 | 674 | | if (!(tangentVelocityResolved & denominatorResolved)) |
| | | 675 | | { |
| | 3 | 676 | | return TrySolveFrictionImpulseExact( |
| | 3 | 677 | | pair, |
| | 3 | 678 | | contact, |
| | 3 | 679 | | responsePositionA, |
| | 3 | 680 | | responsePositionB, |
| | 3 | 681 | | normalLinearVelocityA, |
| | 3 | 682 | | normalAngularVelocityA, |
| | 3 | 683 | | normalLinearVelocityB, |
| | 3 | 684 | | normalAngularVelocityB, |
| | 3 | 685 | | restitutionVelocityThreshold, |
| | 3 | 686 | | contactShare, |
| | 3 | 687 | | out accumulated, impact); |
| | | 688 | | } |
| | 2297 | 689 | | if (tangentVelocity.Abs() > Fixed64.Epsilon |
| | 2297 | 690 | | && denominator > Fixed64.Zero |
| | 2297 | 691 | | && !Fixed64.TryMultiplyDivide( |
| | 2297 | 692 | | -tangentVelocity, |
| | 2297 | 693 | | Fixed64.One, |
| | 2297 | 694 | | denominator, |
| | 2297 | 695 | | out impulseScalar)) |
| | | 696 | | { |
| | 1 | 697 | | return TrySolveFrictionImpulseExact( |
| | 1 | 698 | | pair, |
| | 1 | 699 | | contact, |
| | 1 | 700 | | responsePositionA, |
| | 1 | 701 | | responsePositionB, |
| | 1 | 702 | | normalLinearVelocityA, |
| | 1 | 703 | | normalAngularVelocityA, |
| | 1 | 704 | | normalLinearVelocityB, |
| | 1 | 705 | | normalAngularVelocityB, |
| | 1 | 706 | | restitutionVelocityThreshold, |
| | 1 | 707 | | contactShare, |
| | 1 | 708 | | out accumulated, impact); |
| | | 709 | | } |
| | | 710 | | } |
| | | 711 | | |
| | 16346 | 712 | | if (!Fixed64.TryAdd( |
| | 16346 | 713 | | contact.CachedTangentImpulse, |
| | 16346 | 714 | | impulseScalar, |
| | 16346 | 715 | | out Fixed64 desiredAccumulated)) |
| | | 716 | | { |
| | 3 | 717 | | return TrySolveFrictionImpulseExact( |
| | 3 | 718 | | pair, |
| | 3 | 719 | | contact, |
| | 3 | 720 | | responsePositionA, |
| | 3 | 721 | | responsePositionB, |
| | 3 | 722 | | normalLinearVelocityA, |
| | 3 | 723 | | normalAngularVelocityA, |
| | 3 | 724 | | normalLinearVelocityB, |
| | 3 | 725 | | normalAngularVelocityB, |
| | 3 | 726 | | restitutionVelocityThreshold, |
| | 3 | 727 | | contactShare, |
| | 3 | 728 | | out accumulated, impact); |
| | | 729 | | } |
| | 16343 | 730 | | accumulated = desiredAccumulated >= -staticFrictionLimit |
| | 16343 | 731 | | && desiredAccumulated <= staticFrictionLimit |
| | 16343 | 732 | | ? desiredAccumulated |
| | 16343 | 733 | | : FixedMath.Clamp( |
| | 16343 | 734 | | desiredAccumulated, |
| | 16343 | 735 | | -dynamicFrictionLimit, |
| | 16343 | 736 | | dynamicFrictionLimit); |
| | 16343 | 737 | | if (!Fixed64.TrySubtract( |
| | 16343 | 738 | | accumulated, |
| | 16343 | 739 | | contact.CachedTangentImpulse, |
| | 16343 | 740 | | out impulseScalar)) |
| | | 741 | | { |
| | 2 | 742 | | return TrySolveFrictionImpulseExact( |
| | 2 | 743 | | pair, |
| | 2 | 744 | | contact, |
| | 2 | 745 | | responsePositionA, |
| | 2 | 746 | | responsePositionB, |
| | 2 | 747 | | normalLinearVelocityA, |
| | 2 | 748 | | normalAngularVelocityA, |
| | 2 | 749 | | normalLinearVelocityB, |
| | 2 | 750 | | normalAngularVelocityB, |
| | 2 | 751 | | restitutionVelocityThreshold, |
| | 2 | 752 | | contactShare, |
| | 2 | 753 | | out accumulated, impact); |
| | | 754 | | } |
| | 16341 | 755 | | return impulseScalar == Fixed64.Zero |
| | 16341 | 756 | | || TryApplyContactImpulseCombination( |
| | 16341 | 757 | | pair, |
| | 16341 | 758 | | contact, |
| | 16341 | 759 | | responsePositionA, |
| | 16341 | 760 | | responsePositionB, |
| | 16341 | 761 | | contact.Normal, |
| | 16341 | 762 | | Fixed64.Zero, |
| | 16341 | 763 | | contact.Tangent, |
| | 16341 | 764 | | impulseScalar); |
| | | 765 | | } |
| | | 766 | | |
| | | 767 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 768 | | private static bool TrySolveFrictionImpulseExact( |
| | | 769 | | CollisionPair2D pair, |
| | | 770 | | SolverContact2D contact, |
| | | 771 | | Vector2d responsePositionA, |
| | | 772 | | Vector2d responsePositionB, |
| | | 773 | | Vector2d normalLinearVelocityA, |
| | | 774 | | Fixed64 normalAngularVelocityA, |
| | | 775 | | Vector2d normalLinearVelocityB, |
| | | 776 | | Fixed64 normalAngularVelocityB, |
| | | 777 | | Fixed64 restitutionVelocityThreshold, |
| | | 778 | | Fixed64 contactShare, |
| | | 779 | | out Fixed64 accumulated, |
| | | 780 | | in ContactResponseSnapshot impact = default) |
| | | 781 | | { |
| | 61 | 782 | | accumulated = default; |
| | 61 | 783 | | GetExactLevers( |
| | 61 | 784 | | pair, |
| | 61 | 785 | | contact, |
| | 61 | 786 | | responsePositionA, |
| | 61 | 787 | | responsePositionB, |
| | 61 | 788 | | out ExactLever3D exactA, |
| | 61 | 789 | | out ExactLever3D exactB); |
| | 61 | 790 | | Vector3d spatialNormal = |
| | 61 | 791 | | ExactContactLever2D.ToSpatial(contact.Normal); |
| | 61 | 792 | | var normalConstraint = new ExactNormalConstraint3D( |
| | 61 | 793 | | ExactContactLever2D.CreateResponseOperand( |
| | 61 | 794 | | contact.A.Body, |
| | 61 | 795 | | normalLinearVelocityA, |
| | 61 | 796 | | normalAngularVelocityA, |
| | 61 | 797 | | exactA, |
| | 61 | 798 | | -spatialNormal), |
| | 61 | 799 | | ExactContactLever2D.CreateResponseOperand( |
| | 61 | 800 | | contact.B.Body, |
| | 61 | 801 | | normalLinearVelocityB, |
| | 61 | 802 | | normalAngularVelocityB, |
| | 61 | 803 | | exactB, |
| | 61 | 804 | | spatialNormal), |
| | 61 | 805 | | spatialNormal, |
| | 61 | 806 | | contact.Restitution, |
| | 61 | 807 | | restitutionVelocityThreshold, |
| | 61 | 808 | | contact.CachedNormalImpulse, |
| | 61 | 809 | | contactShare, |
| | 61 | 810 | | contactShare); |
| | 61 | 811 | | Vector3d spatialTangent = |
| | 61 | 812 | | ExactContactLever2D.ToSpatial(contact.Tangent); |
| | 61 | 813 | | if (!ExactContactResponseKernel.TryGetCoulombLineResponse( |
| | 61 | 814 | | normalConstraint, |
| | 61 | 815 | | ExactContactLever2D.CreateResponseOperand( |
| | 61 | 816 | | contact.A.Body, |
| | 61 | 817 | | ResolveLinearVelocity(contact.A.Body), |
| | 61 | 818 | | ResolveAngularVelocity(contact.A.Body), |
| | 61 | 819 | | exactA, |
| | 61 | 820 | | -spatialTangent), |
| | 61 | 821 | | ExactContactLever2D.CreateResponseOperand( |
| | 61 | 822 | | contact.B.Body, |
| | 61 | 823 | | ResolveLinearVelocity(contact.B.Body), |
| | 61 | 824 | | ResolveAngularVelocity(contact.B.Body), |
| | 61 | 825 | | exactB, |
| | 61 | 826 | | spatialTangent), |
| | 61 | 827 | | spatialTangent, |
| | 61 | 828 | | contact.CachedTangentImpulse, |
| | 61 | 829 | | contact.StaticFriction, |
| | 61 | 830 | | contact.DynamicFriction, |
| | 61 | 831 | | out ExactCoulombResponse3D response, impact)) |
| | | 832 | | { |
| | 4 | 833 | | return false; |
| | | 834 | | } |
| | | 835 | | |
| | 57 | 836 | | accumulated = response.TryGetPrimaryAccumulatedImpulse( |
| | 57 | 837 | | out Fixed64 projectedAccumulated) |
| | 57 | 838 | | ? projectedAccumulated |
| | 57 | 839 | | : Fixed64.Zero; |
| | 57 | 840 | | return !response.HasAppliedImpulse |
| | 57 | 841 | | || TryApplyVelocityDeltas( |
| | 57 | 842 | | contact, |
| | 57 | 843 | | ExactContactLever2D.ToPlanar( |
| | 57 | 844 | | response.FirstLinearVelocityDelta), |
| | 57 | 845 | | ExactContactLever2D.ToPlanarAngular( |
| | 57 | 846 | | response.FirstAngularVelocityDelta), |
| | 57 | 847 | | ExactContactLever2D.ToPlanar( |
| | 57 | 848 | | response.SecondLinearVelocityDelta), |
| | 57 | 849 | | ExactContactLever2D.ToPlanarAngular( |
| | 57 | 850 | | response.SecondAngularVelocityDelta)); |
| | | 851 | | } |
| | | 852 | | |
| | | 853 | | private static bool CanUseCompactAxisResponse( |
| | | 854 | | SolverContact2D contact, |
| | | 855 | | Vector2d axis) => |
| | 32491 | 856 | | ExactContactLever2D.CanUseCompactResponse( |
| | 32491 | 857 | | contact.A.Body, |
| | 32491 | 858 | | ResolveLinearVelocity(contact.A.Body), |
| | 32491 | 859 | | ResolveAngularVelocity(contact.A.Body), |
| | 32491 | 860 | | contact.RelativeA.Vector, |
| | 32491 | 861 | | contact.B.Body, |
| | 32491 | 862 | | ResolveLinearVelocity(contact.B.Body), |
| | 32491 | 863 | | ResolveAngularVelocity(contact.B.Body), |
| | 32491 | 864 | | contact.RelativeB.Vector, |
| | 32491 | 865 | | axis); |
| | | 866 | | |
| | | 867 | | private static bool TryApplyContactImpulseCombination( |
| | | 868 | | CollisionPair2D pair, |
| | | 869 | | SolverContact2D contact, |
| | | 870 | | Vector2d responsePositionA, |
| | | 871 | | Vector2d responsePositionB, |
| | | 872 | | Vector2d firstAxis, |
| | | 873 | | Fixed64 firstScale, |
| | | 874 | | Vector2d secondAxis, |
| | | 875 | | Fixed64 secondScale) |
| | | 876 | | { |
| | 311 | 877 | | if (!contact.RelativeA.IsExact |
| | 311 | 878 | | && !contact.RelativeB.IsExact |
| | 311 | 879 | | && Vector3d.TryLinearCombination( |
| | 311 | 880 | | ExactContactLever2D.ToSpatial(firstAxis), |
| | 311 | 881 | | firstScale, |
| | 311 | 882 | | ExactContactLever2D.ToSpatial(secondAxis), |
| | 311 | 883 | | secondScale, |
| | 311 | 884 | | Vector3d.Zero, |
| | 311 | 885 | | Fixed64.Zero, |
| | 311 | 886 | | out Vector3d spatialImpulse) |
| | 311 | 887 | | && TryApplyCompactImpulse( |
| | 311 | 888 | | contact, |
| | 311 | 889 | | ExactContactLever2D.ToPlanar(spatialImpulse))) |
| | | 890 | | { |
| | 307 | 891 | | return true; |
| | | 892 | | } |
| | | 893 | | |
| | 4 | 894 | | GetExactLevers( |
| | 4 | 895 | | pair, |
| | 4 | 896 | | contact, |
| | 4 | 897 | | responsePositionA, |
| | 4 | 898 | | responsePositionB, |
| | 4 | 899 | | out ExactLever3D exactA, |
| | 4 | 900 | | out ExactLever3D exactB); |
| | 4 | 901 | | return ExactContactLever2D.TryGetImpulseVelocityDeltas( |
| | 4 | 902 | | contact.A.Body, |
| | 4 | 903 | | exactA, |
| | 4 | 904 | | contact.B.Body, |
| | 4 | 905 | | exactB, |
| | 4 | 906 | | firstAxis, |
| | 4 | 907 | | firstScale, |
| | 4 | 908 | | secondAxis, |
| | 4 | 909 | | secondScale, |
| | 4 | 910 | | out Vector2d linearA, |
| | 4 | 911 | | out Fixed64 angularA, |
| | 4 | 912 | | out Vector2d linearB, |
| | 4 | 913 | | out Fixed64 angularB) |
| | 4 | 914 | | && TryApplyVelocityDeltas( |
| | 4 | 915 | | contact, |
| | 4 | 916 | | linearA, |
| | 4 | 917 | | angularA, |
| | 4 | 918 | | linearB, |
| | 4 | 919 | | angularB); |
| | | 920 | | } |
| | | 921 | | |
| | | 922 | | private static bool TryApplyCompactImpulse( |
| | | 923 | | SolverContact2D contact, |
| | | 924 | | Vector2d impulseB) |
| | | 925 | | { |
| | 310 | 926 | | Vector2d impulseA = -impulseB; |
| | 310 | 927 | | bool linearAResolved = TryGetLinearVelocityDelta( |
| | 310 | 928 | | contact.A, |
| | 310 | 929 | | impulseA, |
| | 310 | 930 | | out Vector2d linearA); |
| | 310 | 931 | | bool angularAResolved = TryGetAngularVelocityDelta( |
| | 310 | 932 | | contact.A, |
| | 310 | 933 | | contact.RelativeA.Vector, |
| | 310 | 934 | | impulseA, |
| | 310 | 935 | | out Fixed64 angularA); |
| | 310 | 936 | | bool linearBResolved = TryGetLinearVelocityDelta( |
| | 310 | 937 | | contact.B, |
| | 310 | 938 | | impulseB, |
| | 310 | 939 | | out Vector2d linearB); |
| | 310 | 940 | | bool angularBResolved = TryGetAngularVelocityDelta( |
| | 310 | 941 | | contact.B, |
| | 310 | 942 | | contact.RelativeB.Vector, |
| | 310 | 943 | | impulseB, |
| | 310 | 944 | | out Fixed64 angularB); |
| | 310 | 945 | | return linearAResolved |
| | 310 | 946 | | & angularAResolved |
| | 310 | 947 | | & linearBResolved |
| | 310 | 948 | | & angularBResolved |
| | 310 | 949 | | && TryApplyVelocityDeltas( |
| | 310 | 950 | | contact, |
| | 310 | 951 | | linearA, |
| | 310 | 952 | | angularA, |
| | 310 | 953 | | linearB, |
| | 310 | 954 | | angularB); |
| | | 955 | | } |
| | | 956 | | |
| | | 957 | | private static bool TryGetLinearVelocityDelta( |
| | | 958 | | ResponseBody2D body, |
| | | 959 | | Vector2d impulse, |
| | | 960 | | out Vector2d velocityDelta) |
| | | 961 | | { |
| | 620 | 962 | | if (!body.CanTranslate) |
| | | 963 | | { |
| | 296 | 964 | | velocityDelta = Vector2d.Zero; |
| | 296 | 965 | | return true; |
| | | 966 | | } |
| | | 967 | | |
| | 324 | 968 | | return ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta( |
| | 324 | 969 | | body.Body!.ProjectLinearMotion(impulse), |
| | 324 | 970 | | Fixed64.One, |
| | 324 | 971 | | body.InverseMass, |
| | 324 | 972 | | Fixed64.One, |
| | 324 | 973 | | out velocityDelta); |
| | | 974 | | } |
| | | 975 | | |
| | | 976 | | private static bool TryGetAngularVelocityDelta( |
| | | 977 | | ResponseBody2D body, |
| | | 978 | | Vector2d relativeContactPoint, |
| | | 979 | | Vector2d impulse, |
| | | 980 | | out Fixed64 velocityDelta) |
| | | 981 | | { |
| | 620 | 982 | | velocityDelta = Fixed64.Zero; |
| | 620 | 983 | | if (!body.CanRotate) |
| | 353 | 984 | | return true; |
| | | 985 | | |
| | 267 | 986 | | return ContactResponseArithmetic3D.TryCross( |
| | 267 | 987 | | ExactContactLever2D.ToSpatial(relativeContactPoint), |
| | 267 | 988 | | ExactContactLever2D.ToSpatial(impulse), |
| | 267 | 989 | | out Vector3d torque) |
| | 267 | 990 | | && Fixed64.TryMultiplyDivide( |
| | 267 | 991 | | -torque.Y, |
| | 267 | 992 | | body.InverseMoment, |
| | 267 | 993 | | Fixed64.One, |
| | 267 | 994 | | out velocityDelta); |
| | | 995 | | } |
| | | 996 | | |
| | | 997 | | private static bool TryApplyVelocityDeltas( |
| | | 998 | | SolverContact2D contact, |
| | | 999 | | Vector2d linearA, |
| | | 1000 | | Fixed64 angularA, |
| | | 1001 | | Vector2d linearB, |
| | | 1002 | | Fixed64 angularB) |
| | | 1003 | | { |
| | 769 | 1004 | | bool firstFits = contact.A.Body?.CanApplyCollisionVelocityDeltas( |
| | 769 | 1005 | | linearA, |
| | 769 | 1006 | | angularA) |
| | 769 | 1007 | | ?? true; |
| | 769 | 1008 | | bool secondFits = contact.B.Body?.CanApplyCollisionVelocityDeltas( |
| | 769 | 1009 | | linearB, |
| | 769 | 1010 | | angularB) |
| | 769 | 1011 | | ?? true; |
| | 769 | 1012 | | if (!(firstFits & secondFits)) |
| | 2 | 1013 | | return false; |
| | | 1014 | | |
| | 767 | 1015 | | ApplyVelocityDelta(contact.A, linearA, angularA); |
| | 767 | 1016 | | ApplyVelocityDelta(contact.B, linearB, angularB); |
| | 767 | 1017 | | return true; |
| | | 1018 | | } |
| | | 1019 | | |
| | | 1020 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1021 | | private static Vector2d ResolveLinearVelocity(SolidBody2D? body) => |
| | 99380 | 1022 | | body == null |
| | 99380 | 1023 | | ? Vector2d.Zero |
| | 99380 | 1024 | | : body.ProjectLinearMotion( |
| | 99380 | 1025 | | body.IsKinematic |
| | 99380 | 1026 | | ? body.SampleContinuousCollisionLinearVelocity(Fixed64.One) |
| | 99380 | 1027 | | : body.LinearVelocity); |
| | | 1028 | | |
| | | 1029 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1030 | | private static Fixed64 ResolveAngularVelocity(SolidBody2D? body) => |
| | 99380 | 1031 | | body == null |
| | 99380 | 1032 | | ? Fixed64.Zero |
| | 99380 | 1033 | | : body.IsKinematic |
| | 99380 | 1034 | | ? body.SampleContinuousCollisionAngularVelocity(Fixed64.One) |
| | 99380 | 1035 | | : body.AngularVelocity; |
| | | 1036 | | |
| | | 1037 | | private static Vector2d ResolveContactNormal(CollisionPair2D pair, |
| | | 1038 | | in ContactGroupKey group, Vector2d normal) => |
| | 19087 | 1039 | | ResolveContactNormal(normal, group.SourceIdA != 0 || group.SourceIdB != 0 |
| | 19087 | 1040 | | ? Vector2d.Zero : pair.ColliderB.Center - pair.ColliderA.Center); |
| | | 1041 | | |
| | | 1042 | | private static Vector2d ResolveContactNormal(Vector2d normal, Vector2d fallbackDirection) |
| | | 1043 | | { |
| | 19087 | 1044 | | Vector2d resolved = normal.MagnitudeSquared > Fixed64.Epsilon |
| | 19087 | 1045 | | ? normal.Normalized |
| | 19087 | 1046 | | : fallbackDirection.MagnitudeSquared > Fixed64.Epsilon |
| | 19087 | 1047 | | ? fallbackDirection.Normalized |
| | 19087 | 1048 | | : Vector2d.Zero; |
| | | 1049 | | |
| | 19087 | 1050 | | if (resolved == Vector2d.Zero) |
| | 6 | 1051 | | return resolved; |
| | | 1052 | | |
| | 19081 | 1053 | | return fallbackDirection.MagnitudeSquared > Fixed64.Epsilon |
| | 19081 | 1054 | | && Vector2d.Dot(resolved, fallbackDirection) < Fixed64.Zero |
| | 19081 | 1055 | | ? -resolved |
| | 19081 | 1056 | | : resolved; |
| | | 1057 | | } |
| | | 1058 | | |
| | | 1059 | | private static void GetExactLevers( |
| | | 1060 | | CollisionPair2D pair, |
| | | 1061 | | SolverContact2D contact, |
| | | 1062 | | Vector2d responsePositionA, |
| | | 1063 | | Vector2d responsePositionB, |
| | | 1064 | | out ExactLever3D exactA, |
| | | 1065 | | out ExactLever3D exactB) |
| | | 1066 | | { |
| | 433 | 1067 | | ManifoldContact2D manifoldContact = |
| | 433 | 1068 | | pair.Manifold.GetGroup(contact.GroupIndex)[contact.PointIndex]; |
| | 433 | 1069 | | ContactAnchor2D zero = |
| | 433 | 1070 | | ContactAnchor2D.FromWorldPoint(Vector2d.Zero); |
| | 433 | 1071 | | exactA = contact.A.Body == null |
| | 433 | 1072 | | ? zero.GetXZLeverFrom(zero) |
| | 433 | 1073 | | : manifoldContact.AnchorA.GetXZLeverFrom( |
| | 433 | 1074 | | new ContactAnchor2D( |
| | 433 | 1075 | | responsePositionA, |
| | 433 | 1076 | | contact.A.Body.Rotation, |
| | 433 | 1077 | | contact.A.Body.LocalCenterOfMassOffset)); |
| | 433 | 1078 | | exactB = contact.B.Body == null |
| | 433 | 1079 | | ? zero.GetXZLeverFrom(zero) |
| | 433 | 1080 | | : manifoldContact.AnchorB.GetXZLeverFrom( |
| | 433 | 1081 | | new ContactAnchor2D( |
| | 433 | 1082 | | responsePositionB, |
| | 433 | 1083 | | contact.B.Body.Rotation, |
| | 433 | 1084 | | contact.B.Body.LocalCenterOfMassOffset)); |
| | 433 | 1085 | | } |
| | | 1086 | | |
| | | 1087 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1088 | | private static bool HasFailedResponse( |
| | | 1089 | | byte failedResponseMask, |
| | | 1090 | | int contactIndex) => |
| | 40401 | 1091 | | (failedResponseMask & (1 << contactIndex)) != 0; |
| | | 1092 | | |
| | | 1093 | | private static void RejectResponse( |
| | | 1094 | | CollisionPair2D pair, |
| | | 1095 | | SolverContact2D contact, |
| | | 1096 | | int contactIndex, |
| | | 1097 | | ref byte failedResponseMask) |
| | | 1098 | | { |
| | 8 | 1099 | | failedResponseMask |= (byte)(1 << contactIndex); |
| | 8 | 1100 | | pair.RemoveWarmStartImpulse(contact.Group, contact.ContactId); |
| | 8 | 1101 | | GravitasLogger.Channel.Error( |
| | 8 | 1102 | | $"2D contact response is outside the representable velocity domain."); |
| | 8 | 1103 | | } |
| | | 1104 | | } |