| | | 1 | | //======================================================================= |
| | | 2 | | // SolidBody.ContinuousCollision.Hits.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2026-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using FixedMathSharp; |
| | | 9 | | using FixedMathSharp.Geometry; |
| | | 10 | | using Gravitas.Colliders; |
| | | 11 | | using Gravitas.CollisionHandling; |
| | | 12 | | using Gravitas.Queries; |
| | | 13 | | using Gravitas.Support; |
| | | 14 | | using SwiftCollections; |
| | | 15 | | |
| | | 16 | | namespace Gravitas; |
| | | 17 | | |
| | | 18 | | /// <content>Selects spatial sweep hits and refines contact geometry for stationary, moving, and mixed targets.</content |
| | | 19 | | public partial class SolidBody |
| | | 20 | | { |
| | | 21 | | private ContinuousSurfaceTriangleAdmission? _admitContinuousSurfaceTriangle; |
| | | 22 | | private Vector3d _continuousSurfaceStart, _continuousSurfaceEnd, _continuousSurfaceBodyStart; |
| | | 23 | | |
| | | 24 | | private bool TryGetFirstContinuousCollisionHit( |
| | | 25 | | Vector3d startPosition, |
| | | 26 | | Vector3d proposedPosition, |
| | | 27 | | Fixed64 proxyRadius, |
| | | 28 | | Fixed64 elapsedFrameFraction, |
| | | 29 | | out Vector3d normal, |
| | | 30 | | out Fixed64 distance, |
| | | 31 | | out ContinuousCollisionTargetKind targetKind, |
| | | 32 | | out LSCollider? target3D, |
| | | 33 | | out LSCollider2D? target2D) |
| | | 34 | | { |
| | 1421 | 35 | | Vector3d originalPosition = Position3d; |
| | 1421 | 36 | | bool originalPositionMutated = _positionMutated; |
| | | 37 | | try |
| | | 38 | | { |
| | 1421 | 39 | | Position3d = startPosition; |
| | 1421 | 40 | | Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false); |
| | | 41 | | |
| | 1421 | 42 | | int hitCount = QueryStaticContinuousCollisionHits( |
| | 1421 | 43 | | startPosition, |
| | 1421 | 44 | | proposedPosition, |
| | 1421 | 45 | | proxyRadius, |
| | 1421 | 46 | | out bool staticHitsAreShapeExact); |
| | 1421 | 47 | | int mixedHitCount = Context.Settings.RuntimeMode.RunsMixedContacts() |
| | 1421 | 48 | | ? Context.QueryMixed.SweepSphereAgainstStatic2DAll( |
| | 1421 | 49 | | startPosition, |
| | 1421 | 50 | | proposedPosition, |
| | 1421 | 51 | | proxyRadius, |
| | 1421 | 52 | | PhysicsLayerMask.All, |
| | 1421 | 53 | | _continuousMixedCollisionHits, |
| | 1421 | 54 | | Collider, |
| | 1421 | 55 | | includeTriggers: false, |
| | 1421 | 56 | | cacheTargetPartitions: true, |
| | 1421 | 57 | | requireClosingContact: true) |
| | 1421 | 58 | | : 0; |
| | | 59 | | |
| | 1421 | 60 | | bool found3D = TryGetFirstValidContinuousCollisionHit( |
| | 1421 | 61 | | startPosition, |
| | 1421 | 62 | | proposedPosition, |
| | 1421 | 63 | | hitCount, |
| | 1421 | 64 | | staticHitsAreShapeExact, |
| | 1421 | 65 | | out Physics3DHit hit3D); |
| | 1421 | 66 | | ContinuousCollisionTargetKind hit3DKind = found3D |
| | 1421 | 67 | | ? ContinuousCollisionTargetKind.Static3D |
| | 1421 | 68 | | : ContinuousCollisionTargetKind.None; |
| | 1421 | 69 | | bool foundDynamic3D = TryGetFirstDynamicContinuousCollisionHit( |
| | 1421 | 70 | | startPosition, |
| | 1421 | 71 | | proposedPosition, |
| | 1421 | 72 | | proxyRadius, |
| | 1421 | 73 | | elapsedFrameFraction, |
| | 1421 | 74 | | out Physics3DHit dynamicHit3D, |
| | 1421 | 75 | | out Fixed64 dynamicClosingSpeed3D); |
| | 1421 | 76 | | if (ContinuousCollisionCandidateOrdering.ShouldReplaceHit(dynamicHit3D, dynamicClosingSpeed3D, foundDynamic3 |
| | | 77 | | { |
| | 49 | 78 | | hit3D = dynamicHit3D; |
| | 49 | 79 | | found3D = true; |
| | 49 | 80 | | hit3DKind = ContinuousCollisionTargetKind.Dynamic3D; |
| | | 81 | | } |
| | | 82 | | |
| | 1421 | 83 | | bool foundMixed = TryGetFirstValidMixedContinuousCollisionHit(startPosition, proposedPosition, mixedHitCount |
| | 1421 | 84 | | ContinuousCollisionTargetKind hitMixedKind = foundMixed |
| | 1421 | 85 | | ? ContinuousCollisionTargetKind.Static2D |
| | 1421 | 86 | | : ContinuousCollisionTargetKind.None; |
| | 1421 | 87 | | Fixed64 hitMixedDistance = foundMixed |
| | 1421 | 88 | | ? hitMixed.Distance |
| | 1421 | 89 | | : Fixed64.Zero; |
| | 1421 | 90 | | bool foundDynamicMixed = TryGetFirstDynamicMixedContinuousCollisionHit( |
| | 1421 | 91 | | startPosition, |
| | 1421 | 92 | | proposedPosition, |
| | 1421 | 93 | | proxyRadius, |
| | 1421 | 94 | | elapsedFrameFraction, |
| | 1421 | 95 | | out DynamicMixedIntervalHit dynamicMixed); |
| | 1421 | 96 | | if (DynamicMixedIntervalHit.ShouldReplaceStatic( |
| | 1421 | 97 | | dynamicMixed, |
| | 1421 | 98 | | foundDynamicMixed, |
| | 1421 | 99 | | hitMixed, |
| | 1421 | 100 | | foundMixed)) |
| | | 101 | | { |
| | 52 | 102 | | hitMixed = dynamicMixed.ExactHit; |
| | 52 | 103 | | hitMixedDistance = dynamicMixed.SafeDistance; |
| | 52 | 104 | | foundMixed = true; |
| | 52 | 105 | | hitMixedKind = dynamicMixed.Status |
| | 52 | 106 | | == IntervalSearchStatus.ExactHit |
| | 52 | 107 | | ? ContinuousCollisionTargetKind.Dynamic2D |
| | 52 | 108 | | : ContinuousCollisionTargetKind.UnresolvedMixed; |
| | | 109 | | } |
| | | 110 | | |
| | 1421 | 111 | | if (DynamicMixedIntervalHit.ShouldSelect3D( |
| | 1421 | 112 | | found3D, |
| | 1421 | 113 | | hit3D.Distance, |
| | 1421 | 114 | | foundMixed, |
| | 1421 | 115 | | hitMixedDistance)) |
| | | 116 | | { |
| | 434 | 117 | | normal = hit3D.Normal; |
| | 434 | 118 | | distance = hit3D.Distance; |
| | 434 | 119 | | targetKind = hit3DKind; |
| | 434 | 120 | | target3D = hit3D.Collider; |
| | 434 | 121 | | target2D = null; |
| | 434 | 122 | | return true; |
| | | 123 | | } |
| | | 124 | | |
| | 987 | 125 | | if (foundMixed) |
| | | 126 | | { |
| | 213 | 127 | | normal = hitMixedKind == ContinuousCollisionTargetKind.UnresolvedMixed |
| | 213 | 128 | | ? Vector3d.Zero |
| | 213 | 129 | | : hitMixed.NormalFor3DSource; |
| | 213 | 130 | | distance = hitMixedDistance; |
| | 213 | 131 | | targetKind = hitMixedKind; |
| | 213 | 132 | | target3D = null; |
| | 213 | 133 | | target2D = hitMixedKind == ContinuousCollisionTargetKind.UnresolvedMixed |
| | 213 | 134 | | ? null |
| | 213 | 135 | | : hitMixed.Collider2D; |
| | 213 | 136 | | return true; |
| | | 137 | | } |
| | 774 | 138 | | } |
| | | 139 | | finally |
| | | 140 | | { |
| | 1421 | 141 | | Position3d = originalPosition; |
| | 1421 | 142 | | Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false); |
| | 1421 | 143 | | _positionMutated = originalPositionMutated; |
| | 1421 | 144 | | } |
| | | 145 | | |
| | 774 | 146 | | normal = Vector3d.Zero; |
| | 774 | 147 | | distance = Fixed64.Zero; |
| | 774 | 148 | | targetKind = ContinuousCollisionTargetKind.None; |
| | 774 | 149 | | target3D = null; |
| | 774 | 150 | | target2D = null; |
| | 774 | 151 | | return false; |
| | 647 | 152 | | } |
| | | 153 | | |
| | | 154 | | private bool TryGetFirstValidContinuousCollisionHit( |
| | | 155 | | Vector3d startPosition, |
| | | 156 | | Vector3d proposedPosition, |
| | | 157 | | int hitCount, |
| | | 158 | | bool hitsAreShapeExact, |
| | | 159 | | out Physics3DHit hit) |
| | | 160 | | { |
| | 7718 | 161 | | Vector3d displacement = proposedPosition - startPosition; |
| | 7718 | 162 | | Vector3d direction = displacement.Normalized; |
| | 7718 | 163 | | bool found = false; |
| | 7718 | 164 | | Physics3DHit best = default; |
| | 29238 | 165 | | for (int i = 0; i < hitCount; i++) |
| | | 166 | | { |
| | 6901 | 167 | | Physics3DHit candidate = _continuousCollisionHits[i]; |
| | 6901 | 168 | | if (!IsValidContinuousCollisionTarget(candidate.Collider!)) |
| | | 169 | | continue; |
| | | 170 | | |
| | | 171 | | Physics3DHit refined; |
| | | 172 | | // Other convex sources already apply native admission in their |
| | | 173 | | // exact candidate query; sphere candidates use the proxy collector. |
| | 6896 | 174 | | bool surfaceHit = Collider is LSSphereCollider |
| | 6896 | 175 | | && candidate.Collider is LSMeshCollider { StaticSurface: not null }; |
| | 6896 | 176 | | if (surfaceHit) |
| | | 177 | | { |
| | 339 | 178 | | _continuousSurfaceBodyStart = startPosition; |
| | 339 | 179 | | _continuousSurfaceStart = Collider.Center; |
| | 339 | 180 | | _continuousSurfaceEnd = Collider.Center + displacement; |
| | 339 | 181 | | var admission = _admitContinuousSurfaceTriangle ??= AdmitContinuousSurfaceTriangle; |
| | 339 | 182 | | _shapeExactContinuousSweepWorker.Prepare(_continuousSurfaceStart, _continuousSurfaceEnd, Collider.Scaled |
| | 339 | 183 | | if (!_shapeExactContinuousSweepWorker.TrySweepMeshForContinuousCollision((LSMeshCollider)candidate.Colli |
| | 339 | 184 | | admission, out refined)) continue; |
| | 160 | 185 | | refined = ApplyShapeExactContinuousContactSlop(refined); |
| | | 186 | | } |
| | 6557 | 187 | | else if (hitsAreShapeExact) |
| | 136 | 188 | | refined = ApplyShapeExactContinuousContactSlop(candidate); |
| | 6421 | 189 | | else if (TryRefineShapeExactContinuousCollisionHit(candidate, displacement, direction, out Physics3DHit exac |
| | 1 | 190 | | refined = exactHit; |
| | 6420 | 191 | | else if (exactSupported) |
| | | 192 | | continue; |
| | | 193 | | else |
| | 6419 | 194 | | refined = candidate; |
| | | 195 | | |
| | 6716 | 196 | | Vector3d closingNormal = hitsAreShapeExact ? candidate.Normal |
| | 6716 | 197 | | : refined.Normal; |
| | | 198 | | |
| | 6716 | 199 | | if (!IsClosingContinuousCollisionHit(displacement, closingNormal)) |
| | | 200 | | continue; |
| | | 201 | | |
| | 6629 | 202 | | if (found && !ContinuousCollisionHitComesBefore(refined, best)) |
| | | 203 | | continue; |
| | | 204 | | |
| | 6592 | 205 | | best = refined; |
| | 6592 | 206 | | found = true; |
| | | 207 | | } |
| | | 208 | | |
| | 7718 | 209 | | hit = best; |
| | 7718 | 210 | | return found; |
| | | 211 | | } |
| | | 212 | | |
| | | 213 | | private bool AdmitContinuousSurfaceTriangle(int triangle, ref Physics3DHit hit, WideDirection3d? exactNormal) |
| | | 214 | | { |
| | 515 | 215 | | var target = (LSMeshCollider)hit.Collider!; |
| | 515 | 216 | | Vector3d displacement = _continuousSurfaceEnd - _continuousSurfaceStart; |
| | 515 | 217 | | Vector3d center = new FixedSegment(_continuousSurfaceStart, _continuousSurfaceEnd) |
| | 515 | 218 | | .GetPointAtDistance(hit.Distance, displacement.Magnitude); |
| | 515 | 219 | | Vector3d original = Position3d; |
| | 515 | 220 | | bool positionMutated = _positionMutated; |
| | | 221 | | try |
| | | 222 | | { |
| | 515 | 223 | | Position3d = _continuousSurfaceBodyStart + (center - _continuousSurfaceStart); |
| | 515 | 224 | | Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false); |
| | | 225 | | // Filled joined contact owners remove authored diagonals before a |
| | | 226 | | // triangle's conservative sweep witness can select response. |
| | 515 | 227 | | if (TryBuildContinuousCollisionManifold(target)) |
| | | 228 | | { |
| | 474 | 229 | | OrderRotationalContinuousCollisionPair(target, out _, out _, out bool sourceIsA); |
| | 1710 | 230 | | foreach (ManifoldContact row in _rotationalContinuousCollisionManifold) |
| | | 231 | | { |
| | 497 | 232 | | Vector3d normal = ResolveSourceOutwardContactNormal(target, row); |
| | 497 | 233 | | if (!IsClosingContinuousCollisionHit(displacement, normal)) continue; |
| | 232 | 234 | | hit = new(target, sourceIsA ? row.AnchorB : row.AnchorA, normal, hit.Distance, hit.Direction); |
| | 232 | 235 | | return true; |
| | | 236 | | } |
| | 242 | 237 | | return false; |
| | | 238 | | } |
| | 41 | 239 | | return IsClosingContinuousCollisionHit(displacement, hit.Normal) |
| | 41 | 240 | | && target.StaticSurface!.AdmitContinuousWitness(Collider, null, center, target, triangle, hit.Anchor, ex |
| | | 241 | | } |
| | | 242 | | finally |
| | | 243 | | { |
| | 1030 | 244 | | Position3d = original; Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false); |
| | 515 | 245 | | _positionMutated = positionMutated; |
| | 515 | 246 | | } |
| | 515 | 247 | | } |
| | | 248 | | private int QueryStaticContinuousCollisionHits( |
| | | 249 | | Vector3d startPosition, |
| | | 250 | | Vector3d proposedPosition, |
| | | 251 | | Fixed64 proxyRadius, |
| | | 252 | | out bool hitsAreShapeExact) |
| | | 253 | | { |
| | 7718 | 254 | | Vector3d displacement = proposedPosition - startPosition; |
| | 7718 | 255 | | if (Collider is not LSSphereCollider && IsExactConvexSourceSupported(Collider)) |
| | | 256 | | { |
| | 575 | 257 | | hitsAreShapeExact = true; |
| | 575 | 258 | | _continuousSurfaceBodyStart = startPosition; |
| | 575 | 259 | | _continuousSurfaceStart = Collider.Center; |
| | 575 | 260 | | _continuousSurfaceEnd = Collider.Center + displacement; |
| | 575 | 261 | | return Context.Query3D.SweepExactSourceAgainstStaticAll( |
| | 575 | 262 | | Collider, |
| | 575 | 263 | | displacement, |
| | 575 | 264 | | PhysicsLayerMask.All, |
| | 575 | 265 | | _continuousCollisionHits, |
| | 575 | 266 | | Collider, |
| | 575 | 267 | | includeTriggers: false, |
| | 575 | 268 | | surfaceAdmission: _admitContinuousSurfaceTriangle ??= AdmitContinuousSurfaceTriangle); |
| | | 269 | | } |
| | | 270 | | |
| | 7143 | 271 | | hitsAreShapeExact = false; |
| | 7143 | 272 | | return Context.Query3D.SweepSphereAgainstStaticAll( |
| | 7143 | 273 | | startPosition, |
| | 7143 | 274 | | proposedPosition, |
| | 7143 | 275 | | proxyRadius, |
| | 7143 | 276 | | PhysicsLayerMask.All, |
| | 7143 | 277 | | _continuousCollisionHits, |
| | 7143 | 278 | | Collider, |
| | 7143 | 279 | | includeTriggers: false); |
| | | 280 | | } |
| | | 281 | | |
| | | 282 | | private bool TryGetFirstValidMixedContinuousCollisionHit( |
| | | 283 | | Vector3d startPosition, |
| | | 284 | | Vector3d proposedPosition, |
| | | 285 | | int hitCount, |
| | | 286 | | out PhysicsMixedHit hit) |
| | | 287 | | { |
| | 7718 | 288 | | Vector3d displacement = proposedPosition - startPosition; |
| | 15454 | 289 | | for (int i = 0; i < hitCount; i++) |
| | | 290 | | { |
| | 173 | 291 | | PhysicsMixedHit candidate = _continuousMixedCollisionHits[i]; |
| | 173 | 292 | | if (!IsValidMixedContinuousCollisionHit(candidate.Collider2D!)) |
| | | 293 | | continue; |
| | 170 | 294 | | bool exactSphere = candidate.Collider2D!.StaticSurface != null && GravitasQueryMixedService.TryResolveExactS |
| | 170 | 295 | | candidate.Collider2D!, displacement, out _, out _, out _); |
| | | 296 | | |
| | 170 | 297 | | if (UsesMixedStaticManifold(candidate.Collider2D!)) |
| | | 298 | | { |
| | 166 | 299 | | Vector3d position = new FixedSegment(startPosition, proposedPosition) |
| | 166 | 300 | | .GetPointAtDistance(candidate.Distance, displacement.Magnitude); |
| | 166 | 301 | | if (TryBuildStaticMixedContinuousCollisionManifold(candidate.Collider2D!, position, Rotation)) |
| | | 302 | | { |
| | 160 | 303 | | ContactAnchor center = ContactAnchor.FromWorldPoint(Vector3d.Zero); |
| | 160 | 304 | | if (!MixedContact.TrySelectClosingContact(_rotationalMixedEarliestManifold, |
| | 160 | 305 | | center, displacement, Vector3d.Zero, center, Vector2d.Zero, Fixed64.Zero, |
| | 160 | 306 | | out MixedContact closing)) |
| | | 307 | | continue; |
| | 158 | 308 | | candidate = new PhysicsMixedHit(Collider, candidate.Collider2D, closing.Anchor3D, |
| | 158 | 309 | | closing.Anchor2D, closing.Normal3DTo2D, candidate.ReducerKind, |
| | 158 | 310 | | candidate.Distance, candidate.Direction3D); |
| | | 311 | | } |
| | 6 | 312 | | else if (exactSphere && candidate.Collider2D!.StaticSurface is { } surface) |
| | | 313 | | { |
| | 5 | 314 | | Vector3d original = Position3d; |
| | | 315 | | try |
| | | 316 | | { |
| | 5 | 317 | | SetMixedContinuousCollisionSamplePose(position, Rotation); |
| | 5 | 318 | | if (!surface.AdmitMixedContinuousWitness(Collider, Collider.Center, candidate.Collider2D, |
| | 9 | 319 | | candidate.Anchor2D)) continue; |
| | 1 | 320 | | } |
| | 10 | 321 | | finally { SetMixedContinuousCollisionSamplePose(original, Rotation); } |
| | | 322 | | } |
| | | 323 | | } |
| | 164 | 324 | | if (!IsClosingContinuousCollisionHit(displacement, candidate.NormalFor3DSource)) |
| | | 325 | | continue; |
| | | 326 | | |
| | 164 | 327 | | hit = candidate; |
| | 164 | 328 | | return true; |
| | | 329 | | } |
| | | 330 | | |
| | 7554 | 331 | | hit = default; |
| | 7554 | 332 | | return false; |
| | | 333 | | } |
| | | 334 | | |
| | | 335 | | private bool UsesMixedStaticManifold(LSCollider2D target) |
| | 331 | 336 | | => target.StaticSurface != null || target is LSCompoundCollider2D || Collider is LSCompoundCollider or LSMeshCol |
| | | 337 | | |
| | | 338 | | private bool TryBuildStaticMixedContinuousCollisionManifold(LSCollider2D target, |
| | | 339 | | Vector3d position, FixedQuaternion rotation) |
| | | 340 | | { |
| | 166 | 341 | | Vector3d originalPosition = Position3d; |
| | 166 | 342 | | FixedQuaternion originalRotation = Rotation; |
| | | 343 | | try |
| | | 344 | | { |
| | 166 | 345 | | SetMixedContinuousCollisionSamplePose(position, rotation); |
| | | 346 | | // Translational moving-target searches overwrite Known only; the |
| | | 347 | | // mutually exclusive rotational path owns this same accepted buffer. |
| | 166 | 348 | | return CollisionDetectionMixed.TryBuildDiscreteManifold(Collider, target, _rotationalMixedEarliestManifold); |
| | | 349 | | } |
| | | 350 | | finally |
| | | 351 | | { |
| | 166 | 352 | | SetMixedContinuousCollisionSamplePose(originalPosition, originalRotation); |
| | 166 | 353 | | } |
| | 166 | 354 | | } |
| | | 355 | | |
| | | 356 | | private bool TryApplyStaticMixedContinuousCollisionResponse(LSCollider2D target, |
| | | 357 | | Vector3d position, Fixed64 elapsedTime, Fixed64 remainingTime) |
| | | 358 | | { |
| | 161 | 359 | | if (!UsesMixedStaticManifold(target) |
| | 161 | 360 | | || !_rotationalMixedEarliestManifold.HasContact) |
| | 4 | 361 | | return false; |
| | 157 | 362 | | return TryApplyMixedRotationalContinuousCollisionResponse(target, |
| | 157 | 363 | | MixedContact.FromManifoldContact(_rotationalMixedEarliestManifold.PrimaryContact), |
| | 157 | 364 | | Fixed64.Zero, position, Vector3d.Zero, Rotation, Rotation, elapsedTime, remainingTime, |
| | 157 | 365 | | IsKinematic, _rotationalMixedEarliestManifold); |
| | | 366 | | } |
| | | 367 | | |
| | | 368 | | private bool TryRefineShapeExactContinuousCollisionHit( |
| | | 369 | | Physics3DHit candidate, |
| | | 370 | | Vector3d displacement, |
| | | 371 | | Vector3d direction, |
| | | 372 | | out Physics3DHit refined, |
| | | 373 | | out bool exactSupported) |
| | | 374 | | { |
| | 6421 | 375 | | refined = default; |
| | 6421 | 376 | | exactSupported = false; |
| | | 377 | | |
| | 6421 | 378 | | if (Collider is LSSphereCollider) |
| | 6417 | 379 | | return false; |
| | | 380 | | |
| | 4 | 381 | | if (candidate.Collider is not LSSphereCollider targetSphere) |
| | 2 | 382 | | return false; |
| | | 383 | | |
| | 2 | 384 | | return TryRefineContinuousCollisionAgainstTargetSphere(targetSphere, displacement, direction, out refined, out e |
| | | 385 | | } |
| | | 386 | | |
| | | 387 | | private bool TryRefineContinuousCollisionAgainstTargetSphere( |
| | | 388 | | LSSphereCollider targetSphere, |
| | | 389 | | Vector3d displacement, |
| | | 390 | | Vector3d direction, |
| | | 391 | | out Physics3DHit refined, |
| | | 392 | | out bool exactSupported) |
| | | 393 | | { |
| | 36 | 394 | | exactSupported = true; |
| | 36 | 395 | | refined = default; |
| | | 396 | | |
| | 36 | 397 | | Vector3d reverseStart = targetSphere.Center; |
| | 36 | 398 | | Vector3d reverseEnd = targetSphere.Center - displacement; |
| | 36 | 399 | | _shapeExactContinuousSweepWorker.Prepare(reverseStart, reverseEnd, targetSphere.ScaledRadius); |
| | 36 | 400 | | if (!_shapeExactContinuousSweepWorker.TrySweep(Collider, out Vector3d reverseCenterAtImpact, out Fixed64 distanc |
| | 32 | 401 | | return false; |
| | | 402 | | |
| | 4 | 403 | | _ = ContinuousCollisionContactPolicy.TryResolveSweptSphereContact( |
| | 4 | 404 | | Collider, |
| | 4 | 405 | | reverseCenterAtImpact, |
| | 4 | 406 | | -direction, |
| | 4 | 407 | | out _, |
| | 4 | 408 | | out Vector3d sourceSurfaceNormal); |
| | | 409 | | |
| | 4 | 410 | | Vector3d normal = -sourceSurfaceNormal; |
| | 4 | 411 | | refined = new Physics3DHit( |
| | 4 | 412 | | targetSphere, |
| | 4 | 413 | | new ContactAnchor( |
| | 4 | 414 | | targetSphere.Center, |
| | 4 | 415 | | normal * targetSphere.ScaledRadius), |
| | 4 | 416 | | normal, |
| | 4 | 417 | | distance, |
| | 4 | 418 | | direction); |
| | 4 | 419 | | return true; |
| | | 420 | | } |
| | | 421 | | |
| | | 422 | | private static Physics3DHit ApplyShapeExactContinuousContactSlop(Physics3DHit hit) |
| | | 423 | | { |
| | 299 | 424 | | Fixed64 distance = hit.Distance > ShapeExactContinuousContactSlop |
| | 299 | 425 | | ? hit.Distance - ShapeExactContinuousContactSlop |
| | 299 | 426 | | : Fixed64.Zero; |
| | 299 | 427 | | return new Physics3DHit(hit.Collider, hit.Anchor, hit.Normal, distance, hit.Direction); |
| | | 428 | | } |
| | | 429 | | |
| | | 430 | | private void PrepareExactConvexSourceSweep(Vector3d displacement) |
| | | 431 | | { |
| | 7 | 432 | | switch (Collider) |
| | | 433 | | { |
| | | 434 | | case LSMeshCollider mesh: |
| | 1 | 435 | | _shapeExactContinuousConvexSweepWorker.PrepareConvexMeshSource(mesh, displacement); |
| | 1 | 436 | | return; |
| | | 437 | | case LSCompoundCollider compound: |
| | 1 | 438 | | _shapeExactContinuousConvexSweepWorker.PrepareCompoundSource(compound, displacement); |
| | 1 | 439 | | return; |
| | | 440 | | default: |
| | 5 | 441 | | _shapeExactContinuousConvexSweepWorker.PreparePrimitiveSource(Collider, displacement); |
| | 5 | 442 | | return; |
| | | 443 | | } |
| | | 444 | | } |
| | | 445 | | |
| | | 446 | | private static bool IsExactConvexSourceSupported(LSCollider collider) |
| | | 447 | | { |
| | 615 | 448 | | return collider switch |
| | 615 | 449 | | { |
| | 270 | 450 | | LSCapsuleCollider => true, |
| | 284 | 451 | | LSCuboidCollider => true, |
| | 9 | 452 | | LSCylinderCollider => true, |
| | 2 | 453 | | LSConeCollider => true, |
| | 14 | 454 | | LSMeshCollider { Mode: MeshColliderMode.Convex } => true, |
| | 10 | 455 | | LSCompoundCollider => true, |
| | 26 | 456 | | _ => false |
| | 615 | 457 | | }; |
| | | 458 | | } |
| | | 459 | | |
| | | 460 | | private static bool ContinuousCollisionHitComesBefore(Physics3DHit left, Physics3DHit right) |
| | | 461 | | { |
| | 37 | 462 | | int distanceCompare = left.Distance.CompareTo(right.Distance); |
| | 37 | 463 | | if (distanceCompare != 0) |
| | 32 | 464 | | return distanceCompare < 0; |
| | | 465 | | |
| | 5 | 466 | | return left.Collider!.Id < right.Collider!.Id; |
| | | 467 | | } |
| | | 468 | | |
| | | 469 | | private bool TryGetFirstDynamicContinuousCollisionHit( |
| | | 470 | | Vector3d startPosition, |
| | | 471 | | Vector3d proposedPosition, |
| | | 472 | | Fixed64 proxyRadius, |
| | | 473 | | Fixed64 elapsedFrameFraction, |
| | | 474 | | out Physics3DHit hit, |
| | | 475 | | out Fixed64 closingSpeed) |
| | | 476 | | { |
| | 1421 | 477 | | hit = default; |
| | 1421 | 478 | | closingSpeed = Fixed64.Zero; |
| | | 479 | | |
| | 1421 | 480 | | Vector3d sourceDisplacement = proposedPosition - startPosition; |
| | 1421 | 481 | | Fixed64 sourceLength = sourceDisplacement.Magnitude; |
| | 1421 | 482 | | bool found = false; |
| | 1421 | 483 | | Physics3DHit best = default; |
| | 1421 | 484 | | Fixed64 bestClosingSpeed = Fixed64.Zero; |
| | 1421 | 485 | | long token = Context.LateSimulateToken; |
| | 1421 | 486 | | SwiftList<int> candidateIds = Context.Physics.QueryContinuousCollisionCandidates( |
| | 1421 | 487 | | DynamicCcdCandidateIndex.CreateSweptSphereBounds(startPosition, sourceDisplacement, proxyRadius)); |
| | 6116 | 488 | | for (int candidateIndex = 0; candidateIndex < candidateIds.Count; candidateIndex++) |
| | | 489 | | { |
| | 1637 | 490 | | int dynamicId = candidateIds[candidateIndex]; |
| | 1637 | 491 | | SolidBody target = Context.Physics.GetContinuousCollisionCandidate(dynamicId); |
| | 1637 | 492 | | if (!IsEligibleDynamicContinuousCollisionTarget(target)) |
| | | 493 | | { |
| | | 494 | | continue; |
| | | 495 | | } |
| | | 496 | | |
| | 104 | 497 | | target.EnsureContinuousCollisionFramePrepared(token); |
| | 104 | 498 | | if (!TryGetDynamicRelativeContinuousCollisionHit( |
| | 104 | 499 | | target, |
| | 104 | 500 | | startPosition, |
| | 104 | 501 | | sourceDisplacement, |
| | 104 | 502 | | proxyRadius, |
| | 104 | 503 | | sourceLength, |
| | 104 | 504 | | elapsedFrameFraction, |
| | 104 | 505 | | out Physics3DHit candidate, |
| | 104 | 506 | | out Fixed64 candidateClosingSpeed)) |
| | | 507 | | continue; |
| | | 508 | | |
| | 60 | 509 | | if (!ContinuousCollisionCandidateOrdering.ShouldReplaceHit(candidate, candidateClosingSpeed, true, found, be |
| | | 510 | | continue; |
| | | 511 | | |
| | 52 | 512 | | best = candidate; |
| | 52 | 513 | | bestClosingSpeed = candidateClosingSpeed; |
| | 52 | 514 | | found = true; |
| | | 515 | | } |
| | | 516 | | |
| | 1421 | 517 | | hit = best; |
| | 1421 | 518 | | closingSpeed = bestClosingSpeed; |
| | 1421 | 519 | | return found; |
| | | 520 | | } |
| | | 521 | | |
| | | 522 | | internal bool TryGetDynamicRelativeContinuousCollisionHit( |
| | | 523 | | SolidBody target, |
| | | 524 | | Vector3d sourceStart, |
| | | 525 | | Vector3d sourceDisplacement, |
| | | 526 | | Fixed64 sourceRadius, |
| | | 527 | | Fixed64 sourceLength, |
| | | 528 | | Fixed64 queryStartFraction, |
| | | 529 | | out Physics3DHit hit, |
| | | 530 | | out Fixed64 closingSpeed) |
| | | 531 | | { |
| | 121 | 532 | | hit = default; |
| | 121 | 533 | | closingSpeed = Fixed64.Zero; |
| | 121 | 534 | | Vector3d sourceEnd = sourceStart + sourceDisplacement; |
| | 121 | 535 | | Vector3d sourceDirection = sourceDisplacement.Normalized; |
| | 121 | 536 | | Fixed64 targetRadius = target.ResolveContinuousCollisionProxyRadius(); |
| | 121 | 537 | | bool exactSupported = Collider is LSSphereCollider |
| | 121 | 538 | | || target.Collider is LSSphereCollider |
| | 121 | 539 | | || IsExactConvexSourceSupported(Collider); |
| | 121 | 540 | | bool found = false; |
| | 121 | 541 | | Physics3DHit best = default; |
| | 121 | 542 | | Fixed64 bestClosingSpeed = Fixed64.Zero; |
| | | 543 | | |
| | 121 | 544 | | Vector3d originalSourcePosition = Position3d; |
| | 121 | 545 | | FixedQuaternion originalSourceRotation = Rotation; |
| | 121 | 546 | | bool originalSourcePositionMutated = _positionMutated; |
| | 121 | 547 | | bool originalSourceRotationMutated = _rotationMutated; |
| | 121 | 548 | | Vector3d originalTargetPosition = target.Position3d; |
| | 121 | 549 | | FixedQuaternion originalTargetRotation = target.Rotation; |
| | 121 | 550 | | bool originalTargetPositionMutated = target._positionMutated; |
| | 121 | 551 | | bool originalTargetRotationMutated = target._rotationMutated; |
| | | 552 | | |
| | | 553 | | try |
| | | 554 | | { |
| | 121 | 555 | | target.GetContinuousCollisionTrajectorySegmentRange( |
| | 121 | 556 | | queryStartFraction, |
| | 121 | 557 | | out int segmentStartIndex, |
| | 121 | 558 | | out int segmentEndExclusive); |
| | 121 | 559 | | for (int segmentIndex = segmentStartIndex; |
| | 175 | 560 | | segmentIndex < segmentEndExclusive; |
| | 54 | 561 | | segmentIndex++) |
| | | 562 | | { |
| | 128 | 563 | | ContinuousCollisionMotionSegment3D segment = |
| | 128 | 564 | | target.GetContinuousCollisionTrajectorySegment(segmentIndex); |
| | 128 | 565 | | ContinuousCollisionMath.ClipTranslationalTrajectoryInterval( |
| | 128 | 566 | | queryStartFraction, |
| | 128 | 567 | | segment.StartFraction, |
| | 128 | 568 | | segment.EndFraction, |
| | 128 | 569 | | out Fixed64 overlapStart, |
| | 128 | 570 | | out Fixed64 overlapEnd, |
| | 128 | 571 | | out Fixed64 sourceStartTime, |
| | 128 | 572 | | out Fixed64 sourceEndTime); |
| | | 573 | | |
| | 128 | 574 | | Vector3d sourceSegmentStart = Vector3d.Lerp( |
| | 128 | 575 | | sourceStart, |
| | 128 | 576 | | sourceEnd, |
| | 128 | 577 | | sourceStartTime); |
| | 128 | 578 | | Vector3d sourceSegmentEnd = Vector3d.Lerp( |
| | 128 | 579 | | sourceStart, |
| | 128 | 580 | | sourceEnd, |
| | 128 | 581 | | sourceEndTime); |
| | 128 | 582 | | Vector3d sourceSegmentDisplacement = |
| | 128 | 583 | | ContinuousCollisionSweepRange.ValidateEndpoint( |
| | 128 | 584 | | sourceSegmentStart, |
| | 128 | 585 | | sourceSegmentEnd, |
| | 128 | 586 | | out Fixed64 sourceSegmentLength); |
| | 128 | 587 | | Vector3d targetStart = segment.SamplePosition(overlapStart); |
| | 128 | 588 | | Vector3d targetEnd = segment.SamplePosition(overlapEnd); |
| | 128 | 589 | | Vector3d targetDisplacement = |
| | 128 | 590 | | ContinuousCollisionSweepRange.ValidateEndpoint( |
| | 128 | 591 | | targetStart, |
| | 128 | 592 | | targetEnd, |
| | 128 | 593 | | out _); |
| | 128 | 594 | | LSSphereCollider? radialSourceSphere = Collider as LSSphereCollider; |
| | 128 | 595 | | LSSphereCollider? radialTargetSphere = target.Collider as LSSphereCollider; |
| | 128 | 596 | | bool radialPair = radialSourceSphere is not null |
| | 128 | 597 | | && radialTargetSphere is not null; |
| | 128 | 598 | | Fixed64 radialEntryDistance = default; |
| | 128 | 599 | | Vector3d relativeDisplacement = default; |
| | 128 | 600 | | Fixed64 relativeLength = default; |
| | 128 | 601 | | Vector3d normal = default; |
| | 128 | 602 | | if (!radialPair |
| | 128 | 603 | | && !ContinuousCollisionMath.TryGetRelativeSphereOverlapDistanceInterval( |
| | 128 | 604 | | sourceSegmentStart, |
| | 128 | 605 | | sourceSegmentDisplacement, |
| | 128 | 606 | | sourceRadius, |
| | 128 | 607 | | targetStart, |
| | 128 | 608 | | targetDisplacement, |
| | 128 | 609 | | targetRadius, |
| | 128 | 610 | | out radialEntryDistance, |
| | 128 | 611 | | out _, |
| | 128 | 612 | | out relativeDisplacement, |
| | 128 | 613 | | out relativeLength, |
| | 128 | 614 | | out normal, |
| | 128 | 615 | | out _)) |
| | | 616 | | { |
| | | 617 | | continue; |
| | | 618 | | } |
| | | 619 | | |
| | 128 | 620 | | Position3d = sourceSegmentStart; |
| | 128 | 621 | | target.Position3d = targetStart; |
| | 128 | 622 | | target.Rotation = target.SampleContinuousCollisionRotation( |
| | 128 | 623 | | overlapStart); |
| | 128 | 624 | | Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false); |
| | 128 | 625 | | target.Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false); |
| | | 626 | | |
| | 128 | 627 | | Physics3DHit relativeHit = default; |
| | 128 | 628 | | Vector3d closingNormal = normal; |
| | 128 | 629 | | bool foundExact = false; |
| | 128 | 630 | | if (radialPair) |
| | | 631 | | { |
| | 79 | 632 | | foundExact = ContinuousCollisionMath |
| | 79 | 633 | | .TryGetRelativeSphereOverlapDistanceInterval( |
| | 79 | 634 | | radialSourceSphere!.Center, |
| | 79 | 635 | | sourceSegmentDisplacement, |
| | 79 | 636 | | radialSourceSphere.ScaledRadius, |
| | 79 | 637 | | radialTargetSphere!.Center, |
| | 79 | 638 | | targetDisplacement, |
| | 79 | 639 | | radialTargetSphere.ScaledRadius, |
| | 79 | 640 | | out radialEntryDistance, |
| | 79 | 641 | | out _, |
| | 79 | 642 | | out relativeDisplacement, |
| | 79 | 643 | | out relativeLength, |
| | 79 | 644 | | out normal, |
| | 79 | 645 | | out _); |
| | | 646 | | } |
| | | 647 | | |
| | 128 | 648 | | Vector3d relativeDirection = relativeDisplacement.Normalized; |
| | 128 | 649 | | if (radialPair && foundExact) |
| | | 650 | | { |
| | 64 | 651 | | relativeHit = new Physics3DHit( |
| | 64 | 652 | | radialTargetSphere!, |
| | 64 | 653 | | new ContactAnchor( |
| | 64 | 654 | | radialTargetSphere!.Center, |
| | 64 | 655 | | normal * radialTargetSphere.ScaledRadius), |
| | 64 | 656 | | normal, |
| | 64 | 657 | | radialEntryDistance, |
| | 64 | 658 | | relativeDirection); |
| | 64 | 659 | | closingNormal = relativeHit.Normal; |
| | | 660 | | } |
| | 64 | 661 | | else if (!radialPair && Collider is LSSphereCollider sourceSphere) |
| | | 662 | | { |
| | 6 | 663 | | foundExact = TrySweepRelativeSourceSphere( |
| | 6 | 664 | | sourceSphere, |
| | 6 | 665 | | target.Collider, |
| | 6 | 666 | | relativeDisplacement, |
| | 6 | 667 | | relativeDirection, |
| | 6 | 668 | | out relativeHit); |
| | 6 | 669 | | if (foundExact) |
| | 5 | 670 | | closingNormal = relativeHit.Normal; |
| | | 671 | | } |
| | 58 | 672 | | else if (!radialPair && target.Collider is LSSphereCollider targetSphere) |
| | | 673 | | { |
| | 34 | 674 | | foundExact = TryRefineContinuousCollisionAgainstTargetSphere( |
| | 34 | 675 | | targetSphere, |
| | 34 | 676 | | relativeDisplacement, |
| | 34 | 677 | | relativeDirection, |
| | 34 | 678 | | out relativeHit, |
| | 34 | 679 | | out _); |
| | 34 | 680 | | if (foundExact) |
| | 3 | 681 | | closingNormal = relativeHit.Normal; |
| | | 682 | | } |
| | 24 | 683 | | else if (IsExactConvexSourceSupported(Collider)) |
| | | 684 | | { |
| | 7 | 685 | | PrepareExactConvexSourceSweep(relativeDisplacement); |
| | 7 | 686 | | foundExact = _shapeExactContinuousConvexSweepWorker |
| | 7 | 687 | | .TrySweepPreparedSource( |
| | 7 | 688 | | target.Collider, |
| | 7 | 689 | | out Physics3DHit shapeExactConvexHit, |
| | 7 | 690 | | requireClosingInitialContact: true); |
| | 7 | 691 | | if (foundExact) |
| | | 692 | | { |
| | 3 | 693 | | relativeHit = ApplyShapeExactContinuousContactSlop( |
| | 3 | 694 | | shapeExactConvexHit); |
| | 3 | 695 | | closingNormal = shapeExactConvexHit.Normal; |
| | | 696 | | } |
| | | 697 | | } |
| | | 698 | | |
| | 128 | 699 | | if (exactSupported && !foundExact) |
| | | 700 | | continue; |
| | | 701 | | |
| | 77 | 702 | | Fixed64 contactDistance = radialEntryDistance; |
| | 77 | 703 | | if (foundExact) |
| | | 704 | | { |
| | 75 | 705 | | contactDistance = relativeHit.Distance; |
| | 75 | 706 | | normal = relativeHit.Normal; |
| | | 707 | | } |
| | | 708 | | |
| | 77 | 709 | | Fixed64 successorStart = segmentIndex + 1 |
| | 77 | 710 | | < target.ContinuousCollisionTrajectoryCount |
| | 77 | 711 | | ? target.GetContinuousCollisionTrajectorySegment(segmentIndex + 1) |
| | 77 | 712 | | .StartFraction |
| | 77 | 713 | | : Fixed64.Zero; |
| | 77 | 714 | | if (ContinuousCollisionMath.IsSupersededTranslationalBoundaryHit( |
| | 77 | 715 | | contactDistance >= relativeLength, |
| | 77 | 716 | | overlapEnd, |
| | 77 | 717 | | segmentIndex, |
| | 77 | 718 | | target.ContinuousCollisionTrajectoryCount, |
| | 77 | 719 | | successorStart)) |
| | | 720 | | { |
| | | 721 | | continue; |
| | | 722 | | } |
| | | 723 | | |
| | 76 | 724 | | Fixed64 localClosingSpeed = |
| | 76 | 725 | | -Vector3d.Dot(relativeDisplacement, closingNormal); |
| | 76 | 726 | | if (!ContinuousCollisionMath.TryNormalizeTranslationalClosingSpeed( |
| | 76 | 727 | | localClosingSpeed, |
| | 76 | 728 | | sourceEndTime - sourceStartTime, |
| | 76 | 729 | | out Fixed64 candidateClosingSpeed)) |
| | | 730 | | continue; |
| | | 731 | | |
| | 74 | 732 | | Vector3d sourceCenter = new FixedSegment( |
| | 74 | 733 | | sourceSegmentStart, |
| | 74 | 734 | | sourceSegmentEnd).GetPointAtDistance( |
| | 74 | 735 | | contactDistance, |
| | 74 | 736 | | relativeLength); |
| | 74 | 737 | | Vector3d targetCenter = new FixedSegment( |
| | 74 | 738 | | targetStart, |
| | 74 | 739 | | targetEnd).GetPointAtDistance( |
| | 74 | 740 | | contactDistance, |
| | 74 | 741 | | relativeLength); |
| | | 742 | | ContactAnchor anchor; |
| | 74 | 743 | | if (foundExact |
| | 74 | 744 | | && relativeHit.Anchor.TryGetOffsetFrom( |
| | 74 | 745 | | target.Collider.Center, |
| | 74 | 746 | | out Vector3d targetOffset)) |
| | | 747 | | { |
| | 72 | 748 | | anchor = new ContactAnchor(targetCenter, targetOffset); |
| | | 749 | | } |
| | | 750 | | else |
| | | 751 | | { |
| | 2 | 752 | | anchor = ContactAnchor.FromWorldPoint( |
| | 2 | 753 | | ContinuousCollisionMath.ResolveContactPointOnTarget( |
| | 2 | 754 | | sourceCenter, |
| | 2 | 755 | | targetCenter, |
| | 2 | 756 | | normal, |
| | 2 | 757 | | targetRadius)); |
| | | 758 | | } |
| | 74 | 759 | | if (!Fixed64.TryMultiplyDivide( |
| | 74 | 760 | | sourceSegmentLength, |
| | 74 | 761 | | contactDistance, |
| | 74 | 762 | | relativeLength, |
| | 74 | 763 | | out Fixed64 localSourceDistance) |
| | 74 | 764 | | || !Fixed64.TryMultiplyAdd( |
| | 74 | 765 | | sourceLength, |
| | 74 | 766 | | sourceStartTime, |
| | 74 | 767 | | localSourceDistance, |
| | 74 | 768 | | out Fixed64 sourceDistance)) |
| | | 769 | | { |
| | | 770 | | continue; |
| | | 771 | | } |
| | 74 | 772 | | var candidate = new Physics3DHit( |
| | 74 | 773 | | target.Collider, |
| | 74 | 774 | | anchor, |
| | 74 | 775 | | normal, |
| | 74 | 776 | | sourceDistance, |
| | 74 | 777 | | sourceDirection); |
| | 74 | 778 | | best = candidate; |
| | 74 | 779 | | bestClosingSpeed = candidateClosingSpeed; |
| | 74 | 780 | | found = true; |
| | 74 | 781 | | break; |
| | | 782 | | } |
| | 47 | 783 | | } |
| | | 784 | | finally |
| | | 785 | | { |
| | 121 | 786 | | Position3d = originalSourcePosition; |
| | 121 | 787 | | Rotation = originalSourceRotation; |
| | 121 | 788 | | target.Position3d = originalTargetPosition; |
| | 121 | 789 | | target.Rotation = originalTargetRotation; |
| | 121 | 790 | | target.Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false); |
| | 121 | 791 | | Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false); |
| | 121 | 792 | | target._positionMutated = originalTargetPositionMutated; |
| | 121 | 793 | | target._rotationMutated = originalTargetRotationMutated; |
| | 121 | 794 | | _positionMutated = originalSourcePositionMutated; |
| | 121 | 795 | | _rotationMutated = originalSourceRotationMutated; |
| | 121 | 796 | | } |
| | | 797 | | |
| | 121 | 798 | | hit = best; |
| | 121 | 799 | | closingSpeed = bestClosingSpeed; |
| | 121 | 800 | | return found; |
| | | 801 | | } |
| | | 802 | | |
| | | 803 | | private bool TrySweepRelativeSourceSphere( |
| | | 804 | | LSSphereCollider sourceSphere, |
| | | 805 | | LSCollider target, |
| | | 806 | | Vector3d relativeDisplacement, |
| | | 807 | | Vector3d relativeDirection, |
| | | 808 | | out Physics3DHit hit) |
| | | 809 | | { |
| | 6 | 810 | | hit = default; |
| | 6 | 811 | | _shapeExactContinuousSweepWorker.Prepare( |
| | 6 | 812 | | sourceSphere.Center, |
| | 6 | 813 | | sourceSphere.Center + relativeDisplacement, |
| | 6 | 814 | | sourceSphere.ScaledRadius); |
| | 6 | 815 | | if (!_shapeExactContinuousSweepWorker.TrySweep(target, out Vector3d sphereCenterAtImpact, out Fixed64 distance)) |
| | 1 | 816 | | return false; |
| | | 817 | | |
| | 5 | 818 | | _ = ContinuousCollisionContactPolicy.TryResolveSweptSphereContact( |
| | 5 | 819 | | target, |
| | 5 | 820 | | sphereCenterAtImpact, |
| | 5 | 821 | | relativeDirection, |
| | 5 | 822 | | out ContactAnchor anchor, |
| | 5 | 823 | | out Vector3d normal); |
| | | 824 | | |
| | 5 | 825 | | hit = new Physics3DHit(target, anchor, normal, distance, relativeDirection); |
| | 5 | 826 | | return true; |
| | | 827 | | } |
| | | 828 | | |
| | | 829 | | private bool TryGetFirstDynamicMixedContinuousCollisionHit( |
| | | 830 | | Vector3d startPosition, |
| | | 831 | | Vector3d proposedPosition, |
| | | 832 | | Fixed64 proxyRadius, |
| | | 833 | | Fixed64 elapsedFrameFraction, |
| | | 834 | | out DynamicMixedIntervalHit hit) |
| | | 835 | | { |
| | 1421 | 836 | | hit = default; |
| | | 837 | | |
| | 1421 | 838 | | if (!Context.Settings.RuntimeMode.RunsMixedContacts()) |
| | 969 | 839 | | return false; |
| | | 840 | | |
| | 452 | 841 | | Vector3d sourceDisplacement = proposedPosition - startPosition; |
| | 452 | 842 | | Fixed64 sourceLength = sourceDisplacement.Magnitude; |
| | 452 | 843 | | Vector3d sourceDirection = sourceDisplacement.Normalized; |
| | 452 | 844 | | bool found = false; |
| | 452 | 845 | | DynamicMixedIntervalHit best = default; |
| | 452 | 846 | | long token = Context.LateSimulateToken; |
| | 452 | 847 | | SwiftList<int> candidateIds = Context.Physics2D.QueryMixedContinuousCollisionCandidates( |
| | 452 | 848 | | DynamicCcdCandidateIndex.CreateSweptSphereBounds(startPosition, sourceDisplacement, proxyRadius)); |
| | 1152 | 849 | | for (int candidateIndex = 0; candidateIndex < candidateIds.Count; candidateIndex++) |
| | | 850 | | { |
| | 124 | 851 | | int dynamicId = candidateIds[candidateIndex]; |
| | 124 | 852 | | SolidBody2D target = Context.Physics2D.GetContinuousCollisionCandidate(dynamicId); |
| | 124 | 853 | | if (!IsEligibleDynamicMixed2DTarget(target)) |
| | | 854 | | { |
| | | 855 | | continue; |
| | | 856 | | } |
| | | 857 | | |
| | 55 | 858 | | target.EnsureContinuousCollisionFramePrepared(token); |
| | 55 | 859 | | Fixed64 targetRadius = |
| | 55 | 860 | | target.ResolveMixedContinuousCollisionProxyRadius(); |
| | 55 | 861 | | IntervalSearchStatus status = |
| | 55 | 862 | | TryGetDynamicMixed2DContinuousCollisionHit( |
| | 55 | 863 | | target, |
| | 55 | 864 | | startPosition, |
| | 55 | 865 | | sourceDisplacement, |
| | 55 | 866 | | proxyRadius, |
| | 55 | 867 | | targetRadius, |
| | 55 | 868 | | sourceLength, |
| | 55 | 869 | | elapsedFrameFraction, |
| | 55 | 870 | | out DynamicMixedIntervalHit candidate); |
| | 55 | 871 | | if (status |
| | 55 | 872 | | == IntervalSearchStatus.CertifiedNoHit) |
| | | 873 | | { |
| | | 874 | | continue; |
| | | 875 | | } |
| | | 876 | | |
| | 53 | 877 | | best = DynamicMixedIntervalHit.Select( |
| | 53 | 878 | | candidate, |
| | 53 | 879 | | best, |
| | 53 | 880 | | ref found); |
| | | 881 | | } |
| | | 882 | | |
| | 452 | 883 | | hit = best; |
| | 452 | 884 | | return found; |
| | | 885 | | } |
| | | 886 | | |
| | | 887 | | private IntervalSearchStatus |
| | | 888 | | TryGetDynamicMixed2DContinuousCollisionHit( |
| | | 889 | | SolidBody2D target, |
| | | 890 | | Vector3d sourceStart, |
| | | 891 | | Vector3d sourceDisplacement, |
| | | 892 | | Fixed64 sourceRadius, |
| | | 893 | | Fixed64 targetRadius, |
| | | 894 | | Fixed64 sourceLength, |
| | | 895 | | Fixed64 queryStartFraction, |
| | | 896 | | out DynamicMixedIntervalHit hit) |
| | | 897 | | { |
| | 79 | 898 | | hit = default; |
| | 79 | 899 | | Vector3d sourceEnd = sourceStart + sourceDisplacement; |
| | 79 | 900 | | Vector3d sourceDirection = sourceDisplacement.Normalized; |
| | 79 | 901 | | target.GetContinuousCollisionTrajectorySegmentRange( |
| | 79 | 902 | | queryStartFraction, |
| | 79 | 903 | | out int segmentStartIndex, |
| | 79 | 904 | | out int segmentEndExclusive); |
| | 79 | 905 | | for (int segmentIndex = segmentStartIndex; |
| | 90 | 906 | | segmentIndex < segmentEndExclusive; |
| | 11 | 907 | | segmentIndex++) |
| | | 908 | | { |
| | 82 | 909 | | ContinuousCollisionMotionSegment2D segment = |
| | 82 | 910 | | target.GetContinuousCollisionTrajectorySegment(segmentIndex); |
| | 82 | 911 | | ContinuousCollisionMath.ClipTranslationalTrajectoryInterval( |
| | 82 | 912 | | queryStartFraction, |
| | 82 | 913 | | segment.StartFraction, |
| | 82 | 914 | | segment.EndFraction, |
| | 82 | 915 | | out Fixed64 overlapStart, |
| | 82 | 916 | | out Fixed64 overlapEnd, |
| | 82 | 917 | | out Fixed64 sourceStartTime, |
| | 82 | 918 | | out Fixed64 sourceEndTime); |
| | | 919 | | |
| | 82 | 920 | | Vector3d sourceSegmentStart = Vector3d.Lerp( |
| | 82 | 921 | | sourceStart, |
| | 82 | 922 | | sourceEnd, |
| | 82 | 923 | | sourceStartTime); |
| | 82 | 924 | | Vector3d sourceSegmentEnd = Vector3d.Lerp( |
| | 82 | 925 | | sourceStart, |
| | 82 | 926 | | sourceEnd, |
| | 82 | 927 | | sourceEndTime); |
| | 82 | 928 | | Vector3d sourceSegmentDisplacement = |
| | 82 | 929 | | ContinuousCollisionSweepRange.ValidateEndpoint( |
| | 82 | 930 | | sourceSegmentStart, |
| | 82 | 931 | | sourceSegmentEnd, |
| | 82 | 932 | | out Fixed64 sourceSegmentLength); |
| | 82 | 933 | | Vector2d targetStart2D = segment.SamplePosition(overlapStart); |
| | 82 | 934 | | Vector2d targetEnd2D = segment.SamplePosition(overlapEnd); |
| | 82 | 935 | | Vector2d targetDisplacement2D = |
| | 82 | 936 | | ContinuousCollisionSweepRange.ValidateEndpoint( |
| | 82 | 937 | | targetStart2D, |
| | 82 | 938 | | targetEnd2D, |
| | 82 | 939 | | out Fixed64 targetSegmentLength); |
| | 82 | 940 | | Vector3d targetStart = new( |
| | 82 | 941 | | targetStart2D.X, |
| | 82 | 942 | | target.Collider.MixedSlabCenterY, |
| | 82 | 943 | | targetStart2D.Y); |
| | 82 | 944 | | Vector3d targetEnd = new( |
| | 82 | 945 | | targetEnd2D.X, |
| | 82 | 946 | | target.Collider.MixedSlabCenterY, |
| | 82 | 947 | | targetEnd2D.Y); |
| | 82 | 948 | | Vector3d targetDisplacement = new( |
| | 82 | 949 | | targetDisplacement2D.X, |
| | 82 | 950 | | Fixed64.Zero, |
| | 82 | 951 | | targetDisplacement2D.Y); |
| | 82 | 952 | | if (!ContinuousCollisionMath.TryGetRelativeSphereOverlapDistanceInterval( |
| | 82 | 953 | | sourceSegmentStart, |
| | 82 | 954 | | sourceSegmentDisplacement, |
| | 82 | 955 | | sourceRadius, |
| | 82 | 956 | | targetStart, |
| | 82 | 957 | | targetDisplacement, |
| | 82 | 958 | | targetRadius, |
| | 82 | 959 | | out Fixed64 entryDistance, |
| | 82 | 960 | | out Fixed64 exitDistance, |
| | 82 | 961 | | out Vector3d relativeDisplacement, |
| | 82 | 962 | | out Fixed64 relativeLength, |
| | 82 | 963 | | out _, |
| | 82 | 964 | | out _)) |
| | | 965 | | { |
| | | 966 | | continue; |
| | | 967 | | } |
| | | 968 | | |
| | 79 | 969 | | Fixed64 successorStart = segmentIndex + 1 |
| | 79 | 970 | | < target.ContinuousCollisionTrajectoryCount |
| | 79 | 971 | | ? target.GetContinuousCollisionTrajectorySegment(segmentIndex + 1) |
| | 79 | 972 | | .StartFraction |
| | 79 | 973 | | : Fixed64.Zero; |
| | 79 | 974 | | if (ContinuousCollisionMath.IsSupersededTranslationalBoundaryHit( |
| | 79 | 975 | | entryDistance >= relativeLength, |
| | 79 | 976 | | overlapEnd, |
| | 79 | 977 | | segmentIndex, |
| | 79 | 978 | | target.ContinuousCollisionTrajectoryCount, |
| | 79 | 979 | | successorStart)) |
| | | 980 | | { |
| | | 981 | | continue; |
| | | 982 | | } |
| | | 983 | | |
| | 79 | 984 | | FixedQuaternion sourceStartRotation = |
| | 79 | 985 | | SampleContinuousCollisionRotation(overlapStart); |
| | 79 | 986 | | FixedQuaternion sourceEndRotation = |
| | 79 | 987 | | SampleContinuousCollisionRotation(overlapEnd); |
| | 79 | 988 | | Fixed64 angularDistance = |
| | 79 | 989 | | ResolveKinematicAngularDistanceRadians( |
| | 79 | 990 | | sourceStartRotation, |
| | 79 | 991 | | sourceEndRotation); |
| | 79 | 992 | | Fixed64 intervalElapsedTime = |
| | 79 | 993 | | overlapStart * Context.DeltaTime; |
| | 79 | 994 | | Fixed64 intervalDuration = |
| | 79 | 995 | | (overlapEnd - overlapStart) * Context.DeltaTime; |
| | 79 | 996 | | bool foundExactContact = |
| | 79 | 997 | | TryGetExactSphereCircleTranslationalContact( |
| | 79 | 998 | | target, |
| | 79 | 999 | | segment, |
| | 79 | 1000 | | new FixedSegment2d(targetStart2D, targetEnd2D), |
| | 79 | 1001 | | targetDisplacement2D, |
| | 79 | 1002 | | targetSegmentLength, |
| | 79 | 1003 | | new FixedSegment(sourceSegmentStart, sourceSegmentEnd), |
| | 79 | 1004 | | sourceSegmentDisplacement, |
| | 79 | 1005 | | sourceSegmentLength, |
| | 79 | 1006 | | sourceStartRotation, |
| | 79 | 1007 | | angularDistance, |
| | 79 | 1008 | | out bool requiresRotationalFallback, |
| | 79 | 1009 | | out Fixed64 exactRelativeDistance, |
| | 79 | 1010 | | out Fixed64 sourceContactDistance, |
| | 79 | 1011 | | out Fixed64 exactLocalClosingSpeed, |
| | 79 | 1012 | | out MixedContact exactContact); |
| | 79 | 1013 | | if (foundExactContact) |
| | | 1014 | | { |
| | 28 | 1015 | | if (ContinuousCollisionMath.IsSupersededTranslationalBoundaryHit( |
| | 28 | 1016 | | exactRelativeDistance >= relativeLength, |
| | 28 | 1017 | | overlapEnd, |
| | 28 | 1018 | | segmentIndex, |
| | 28 | 1019 | | target.ContinuousCollisionTrajectoryCount, |
| | 28 | 1020 | | successorStart)) |
| | | 1021 | | { |
| | | 1022 | | continue; |
| | | 1023 | | } |
| | | 1024 | | |
| | 26 | 1025 | | if (!ContinuousCollisionMath.TryNormalizeTranslationalClosingSpeed( |
| | 26 | 1026 | | exactLocalClosingSpeed, |
| | 26 | 1027 | | sourceEndTime - sourceStartTime, |
| | 26 | 1028 | | out Fixed64 exactClosingSpeed)) |
| | | 1029 | | { |
| | | 1030 | | continue; |
| | | 1031 | | } |
| | | 1032 | | |
| | 26 | 1033 | | if (!Fixed64.TryMultiplyAdd( |
| | 26 | 1034 | | sourceLength, |
| | 26 | 1035 | | sourceStartTime, |
| | 26 | 1036 | | sourceContactDistance, |
| | 26 | 1037 | | out Fixed64 exactSourceDistance)) |
| | | 1038 | | { |
| | | 1039 | | continue; |
| | | 1040 | | } |
| | | 1041 | | |
| | 26 | 1042 | | var analyticHit = new PhysicsMixedHit( |
| | 26 | 1043 | | null, |
| | 26 | 1044 | | target.Collider, |
| | 26 | 1045 | | exactContact.Anchor3D, |
| | 26 | 1046 | | exactContact.Anchor2D, |
| | 26 | 1047 | | exactContact.Normal3DTo2D, |
| | 26 | 1048 | | PhysicsQueryReducerKind.Exact, |
| | 26 | 1049 | | exactSourceDistance, |
| | 26 | 1050 | | sourceDirection); |
| | 26 | 1051 | | hit = new DynamicMixedIntervalHit( |
| | 26 | 1052 | | IntervalSearchStatus.ExactHit, |
| | 26 | 1053 | | analyticHit, |
| | 26 | 1054 | | analyticHit.Distance, |
| | 26 | 1055 | | exactClosingSpeed, |
| | 26 | 1056 | | target.Collider.Id); |
| | 26 | 1057 | | return IntervalSearchStatus.ExactHit; |
| | | 1058 | | } |
| | 51 | 1059 | | if (!requiresRotationalFallback) |
| | | 1060 | | continue; |
| | | 1061 | | |
| | 47 | 1062 | | Fixed64 entryTime = entryDistance / relativeLength; |
| | 47 | 1063 | | Fixed64 exitTime = exitDistance / relativeLength; |
| | | 1064 | | |
| | 47 | 1065 | | bool searchFound = |
| | 47 | 1066 | | TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget( |
| | 47 | 1067 | | target.Collider, |
| | 47 | 1068 | | sourceSegmentStart, |
| | 47 | 1069 | | sourceSegmentDisplacement, |
| | 47 | 1070 | | sourceStartRotation, |
| | 47 | 1071 | | sourceEndRotation, |
| | 47 | 1072 | | angularDistance, |
| | 47 | 1073 | | sourceRadius, |
| | 47 | 1074 | | intervalElapsedTime, |
| | 47 | 1075 | | intervalDuration, |
| | 47 | 1076 | | IsKinematic, |
| | 47 | 1077 | | entryTime, |
| | 47 | 1078 | | exitTime, |
| | 47 | 1079 | | needsResponseContact: true, |
| | 47 | 1080 | | out Fixed64 safeTime, |
| | 47 | 1081 | | out MixedContact contact, |
| | 47 | 1082 | | out bool hasContact, |
| | 47 | 1083 | | out Fixed64 contactTime); |
| | 47 | 1084 | | if (!searchFound) |
| | | 1085 | | { |
| | | 1086 | | continue; |
| | | 1087 | | } |
| | | 1088 | | |
| | 45 | 1089 | | bool closingResolved = Vector3d.TryDot( |
| | 45 | 1090 | | relativeDisplacement, |
| | 45 | 1091 | | contact.Normal3DTo2D, |
| | 45 | 1092 | | out Fixed64 contactClosingSpeed); |
| | 45 | 1093 | | bool hasOnlyTranslationalMotion = |
| | 45 | 1094 | | angularDistance <= Fixed64.Epsilon |
| | 45 | 1095 | | & segment.AngularDistance <= Fixed64.Epsilon; |
| | 45 | 1096 | | bool useContactClosingSpeed = hasContact & hasOnlyTranslationalMotion; |
| | 45 | 1097 | | if (useContactClosingSpeed |
| | 45 | 1098 | | & (!closingResolved |
| | 45 | 1099 | | | contactClosingSpeed <= Fixed64.Epsilon)) |
| | | 1100 | | { |
| | | 1101 | | continue; |
| | | 1102 | | } |
| | 45 | 1103 | | Fixed64 candidateClosingSpeed = |
| | 45 | 1104 | | (useContactClosingSpeed |
| | 45 | 1105 | | ? contactClosingSpeed |
| | 45 | 1106 | | : relativeLength) |
| | 45 | 1107 | | / (sourceEndTime - sourceStartTime); |
| | | 1108 | | |
| | 45 | 1109 | | Fixed64 safeSourceTime = FixedMath.Lerp( |
| | 45 | 1110 | | sourceStartTime, |
| | 45 | 1111 | | sourceEndTime, |
| | 45 | 1112 | | safeTime); |
| | 45 | 1113 | | bool exactHit = hasContact & contactTime == safeTime; |
| | 45 | 1114 | | PhysicsMixedHit exact = exactHit |
| | 45 | 1115 | | ? new PhysicsMixedHit( |
| | 45 | 1116 | | null, |
| | 45 | 1117 | | target.Collider, |
| | 45 | 1118 | | contact.Anchor3D, |
| | 45 | 1119 | | contact.Anchor2D, |
| | 45 | 1120 | | contact.Normal3DTo2D, |
| | 45 | 1121 | | PhysicsQueryReducerKind.ConservativeFallback, |
| | 45 | 1122 | | sourceLength * safeSourceTime, |
| | 45 | 1123 | | sourceDirection) |
| | 45 | 1124 | | : default; |
| | 45 | 1125 | | IntervalSearchStatus status = exactHit |
| | 45 | 1126 | | ? IntervalSearchStatus.ExactHit |
| | 45 | 1127 | | : IntervalSearchStatus.Unresolved; |
| | 45 | 1128 | | hit = new DynamicMixedIntervalHit( |
| | 45 | 1129 | | status, |
| | 45 | 1130 | | exact, |
| | 45 | 1131 | | sourceLength * safeSourceTime, |
| | 45 | 1132 | | candidateClosingSpeed, |
| | 45 | 1133 | | target.Collider.Id); |
| | 45 | 1134 | | return status; |
| | | 1135 | | } |
| | | 1136 | | |
| | 8 | 1137 | | return IntervalSearchStatus.CertifiedNoHit; |
| | | 1138 | | } |
| | | 1139 | | |
| | | 1140 | | } |