| | | 1 | | //======================================================================= |
| | | 2 | | // ConvexSweepQueryWorker.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; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | using FixedMathSharp.Geometry; |
| | | 12 | | using Gravitas.Colliders; |
| | | 13 | | using Gravitas.CollisionHandling; |
| | | 14 | | using SwiftCollections; |
| | | 15 | | using SwiftCollections.Query; |
| | | 16 | | |
| | | 17 | | namespace Gravitas.Queries; |
| | | 18 | | |
| | | 19 | | |
| | | 20 | | |
| | | 21 | | /// <summary> |
| | | 22 | | /// Performs deterministic translational convex-source sweeps against 3D query |
| | | 23 | | /// targets using support-mapped conservative advancement. |
| | | 24 | | /// </summary> |
| | | 25 | | /// <content>Owns source preparation, conservative advancement and ordered target reduction.</content> |
| | | 26 | | internal sealed partial class ConvexSweepQueryWorker |
| | | 27 | | { |
| | | 28 | | private const int MaxGjkIterations = 32; |
| | | 29 | | private const int MaxConservativeAdvancementIterations = 32; |
| | 1 | 30 | | private static readonly Fixed64 DistanceTolerance = |
| | 1 | 31 | | Fixed64.FromFraction(1, 1_048_576); |
| | 1 | 32 | | internal static readonly Fixed64 ContactTolerance = Fixed64.FromFraction(1, 4096); |
| | 1 | 33 | | private static readonly SweepTriangleCandidateComparer SweepTriangleComparer = new(); |
| | 1 | 34 | | private static readonly FixedPointAnchor ZeroAnchor = |
| | 1 | 35 | | new( |
| | 1 | 36 | | Vector3d.Zero, |
| | 1 | 37 | | FixedQuaternion.Identity, |
| | 1 | 38 | | Vector3d.Zero); |
| | | 39 | | |
| | 37365 | 40 | | private readonly SupportPoint[] _simplex = new SupportPoint[4]; |
| | 37365 | 41 | | private readonly SwiftList<int> _triangleCandidates = new(16); |
| | 37365 | 42 | | private readonly SwiftList<SweepTriangleCandidate> _sweepTriangleCandidates = new(16); |
| | | 43 | | private readonly int _maxConservativeAdvancementIterations; |
| | | 44 | | |
| | | 45 | | private LSCollider? _source; |
| | | 46 | | private ConvexShape _sourceShape; |
| | | 47 | | private bool _hasSource; |
| | | 48 | | private bool _requireClosingInitialContact; |
| | | 49 | | private ContinuousSurfaceTriangleAdmission? _admitTriangle; |
| | | 50 | | private WideDirection3d? _admissionNormal; |
| | | 51 | | private Vector3d _displacement; |
| | | 52 | | private Vector3d _outputDirection; |
| | | 53 | | private Vector3d _sweptSourceBoundsMin; |
| | | 54 | | private Vector3d _sweptSourceBoundsMax; |
| | | 55 | | private FixedPointAnchor _displacementAnchor; |
| | | 56 | | private FixedSegment _chord; |
| | | 57 | | private Fixed64 _length; |
| | | 58 | | |
| | | 59 | | internal int LastMeshTriangleCandidateCount { get; private set; } |
| | | 60 | | |
| | 37365 | 61 | | internal ConvexSweepQueryWorker( |
| | 37365 | 62 | | int maxConservativeAdvancementIterations = MaxConservativeAdvancementIterations) => |
| | 37365 | 63 | | _maxConservativeAdvancementIterations = maxConservativeAdvancementIterations; |
| | | 64 | | |
| | | 65 | | internal void ClearSource() |
| | | 66 | | { |
| | 27961 | 67 | | _source = null; |
| | 27961 | 68 | | _sourceShape = default; |
| | 27961 | 69 | | _hasSource = false; |
| | 27961 | 70 | | _admitTriangle = null; |
| | 27961 | 71 | | } |
| | | 72 | | |
| | | 73 | | public void PrepareConvexMeshSource(LSMeshCollider source, Vector3d displacement) |
| | | 74 | | { |
| | 106 | 75 | | ChronicleGuard.ThrowIfNull(source, nameof(source)); |
| | 106 | 76 | | ThrowIfConcaveSource(source); |
| | 105 | 77 | | Prepare(source, displacement); |
| | 105 | 78 | | } |
| | | 79 | | |
| | | 80 | | public void PrepareCompoundSource(LSCompoundCollider source, Vector3d displacement) |
| | | 81 | | { |
| | 30 | 82 | | ChronicleGuard.ThrowIfNull(source, nameof(source)); |
| | 30 | 83 | | Prepare(source, displacement); |
| | 30 | 84 | | } |
| | | 85 | | |
| | | 86 | | public void PreparePrimitiveSource(LSCollider source, Vector3d displacement) |
| | | 87 | | { |
| | 2360 | 88 | | ChronicleGuard.ThrowIfNull(source, nameof(source)); |
| | 2360 | 89 | | if (!ConvexColliderSupport.IsSupported(source)) |
| | 1 | 90 | | throw new NotSupportedException( |
| | 1 | 91 | | $"Convex swept queries do not support {source.GetType().Name} sources."); |
| | | 92 | | |
| | 2359 | 93 | | Prepare(source, displacement); |
| | 2359 | 94 | | } |
| | | 95 | | |
| | | 96 | | public void PrepareCircleSlabSource(Vector3d center, Fixed64 radius, Fixed64 halfHeight, Vector3d displacement) |
| | | 97 | | { |
| | 32 | 98 | | ChronicleGuard.ThrowIfArgument(radius <= Fixed64.Zero, nameof(radius), "Circle-slab sweep radius must be greater |
| | 31 | 99 | | ChronicleGuard.ThrowIfArgument(halfHeight <= Fixed64.Zero, nameof(halfHeight), "Circle-slab sweep half-height mu |
| | 30 | 100 | | _source = null; |
| | 30 | 101 | | Prepare(ConvexShape.CreateCircleSlab(center, radius, halfHeight), displacement); |
| | 30 | 102 | | } |
| | | 103 | | |
| | | 104 | | internal void PrepareSphereSource( |
| | | 105 | | Vector3d center, |
| | | 106 | | Fixed64 radius, |
| | | 107 | | Vector3d displacement) |
| | | 108 | | { |
| | 22093 | 109 | | ChronicleGuard.ThrowIfArgument( |
| | 22093 | 110 | | radius < Fixed64.Zero, |
| | 22093 | 111 | | nameof(radius), |
| | 22093 | 112 | | "Sphere sweep radius cannot be negative."); |
| | 22093 | 113 | | _source = null; |
| | 22093 | 114 | | Prepare(ConvexShape.CreateSphere(center, radius), displacement); |
| | 22093 | 115 | | } |
| | | 116 | | |
| | | 117 | | public bool TrySweepPreparedSource(LSCollider target, out Physics3DHit hit, |
| | | 118 | | bool requireClosingInitialContact = false, ContinuousSurfaceTriangleAdmission? admitTriangle = null) |
| | | 119 | | { |
| | 3888 | 120 | | _requireClosingInitialContact = requireClosingInitialContact; |
| | 3888 | 121 | | _admitTriangle = admitTriangle; |
| | 3888 | 122 | | LastMeshTriangleCandidateCount = 0; |
| | 3888 | 123 | | hit = default; |
| | 3888 | 124 | | if (!_hasSource |
| | 3888 | 125 | | || _length <= Fixed64.Epsilon |
| | 3888 | 126 | | || !SweepBoundsUtility.OverlapsInclusive(_sweptSourceBoundsMin, _sweptSourceBoundsMax, target.BoundsMin, tar |
| | | 127 | | { |
| | 139 | 128 | | return false; |
| | | 129 | | } |
| | | 130 | | |
| | 3749 | 131 | | if (_source is LSCompoundCollider compound) |
| | 24 | 132 | | return TrySweepCompoundSource(compound, target, out hit); |
| | | 133 | | |
| | 3725 | 134 | | return TrySweepSourceShape( |
| | 3725 | 135 | | _sourceShape, |
| | 3725 | 136 | | target, |
| | 3725 | 137 | | out hit, |
| | 3725 | 138 | | out _); |
| | | 139 | | } |
| | | 140 | | |
| | | 141 | | private void Prepare(LSCollider source, Vector3d displacement) |
| | | 142 | | { |
| | 2494 | 143 | | _source = source; |
| | 2494 | 144 | | Prepare(CreateColliderShape(source, Vector3d.Zero), displacement); |
| | 2494 | 145 | | } |
| | | 146 | | |
| | | 147 | | private void Prepare(ConvexShape sourceShape, Vector3d displacement) |
| | | 148 | | { |
| | 24617 | 149 | | _sourceShape = sourceShape; |
| | 24617 | 150 | | _displacement = displacement; |
| | 24617 | 151 | | _displacementAnchor = new FixedPointAnchor( |
| | 24617 | 152 | | Vector3d.Zero, |
| | 24617 | 153 | | FixedQuaternion.Identity, |
| | 24617 | 154 | | displacement); |
| | 24617 | 155 | | _chord = new FixedSegment(Vector3d.Zero, displacement); |
| | 24617 | 156 | | _hasSource = |
| | 24617 | 157 | | Vector3d.TryGetMagnitude(displacement, out _length) |
| | 24617 | 158 | | && sourceShape.CanTranslateCenter(displacement); |
| | 24617 | 159 | | if (!_hasSource) |
| | | 160 | | { |
| | 5 | 161 | | _outputDirection = Vector3d.Zero; |
| | 5 | 162 | | return; |
| | | 163 | | } |
| | | 164 | | |
| | 24612 | 165 | | _outputDirection = |
| | 24612 | 166 | | _length <= Fixed64.Epsilon |
| | 24612 | 167 | | ? Vector3d.Zero |
| | 24612 | 168 | | : displacement.Normalized; |
| | 24612 | 169 | | sourceShape.GetSourceBounds(out Vector3d sourceMin, out Vector3d sourceMax); |
| | | 170 | | // Bounds are broad-phase clips, not canonical pose coordinates. A |
| | | 171 | | // valid center may have support beyond a scalar face. |
| | 24612 | 172 | | SweepBoundsUtility.CreateSweptBounds( |
| | 24612 | 173 | | sourceMin, |
| | 24612 | 174 | | sourceMax, |
| | 24612 | 175 | | displacement, |
| | 24612 | 176 | | ContactTolerance, |
| | 24612 | 177 | | out _sweptSourceBoundsMin, |
| | 24612 | 178 | | out _sweptSourceBoundsMax); |
| | 24612 | 179 | | } |
| | | 180 | | |
| | | 181 | | private bool TrySweepCompoundSource(LSCompoundCollider source, LSCollider target, out Physics3DHit hit) |
| | | 182 | | { |
| | 24 | 183 | | hit = default; |
| | 24 | 184 | | bool found = false; |
| | 24 | 185 | | Fixed64 closestNumerator = Fixed64.MaxValue; |
| | 24 | 186 | | int closestPartIndex = int.MaxValue; |
| | | 187 | | |
| | 112 | 188 | | for (int i = 0; i < source.PartCount; i++) |
| | | 189 | | { |
| | 32 | 190 | | LSCollider part = source.GetPartCollider(i); |
| | 32 | 191 | | if (!TrySweepSourceShape( |
| | 32 | 192 | | CreateColliderShape(part, Vector3d.Zero), |
| | 32 | 193 | | target, |
| | 32 | 194 | | out Physics3DHit candidate, |
| | 32 | 195 | | out Fixed64 candidateNumerator) |
| | 32 | 196 | | || !ComesBeforeReducerCandidate( |
| | 32 | 197 | | candidateNumerator, |
| | 32 | 198 | | i, |
| | 32 | 199 | | found, |
| | 32 | 200 | | closestNumerator, |
| | 32 | 201 | | closestPartIndex)) |
| | | 202 | | { |
| | | 203 | | continue; |
| | | 204 | | } |
| | | 205 | | |
| | 23 | 206 | | hit = candidate; |
| | 23 | 207 | | closestNumerator = candidateNumerator; |
| | 23 | 208 | | closestPartIndex = i; |
| | 23 | 209 | | found = true; |
| | | 210 | | } |
| | | 211 | | |
| | 24 | 212 | | return found; |
| | | 213 | | } |
| | | 214 | | |
| | | 215 | | private bool TrySweepSourceShape( |
| | | 216 | | ConvexShape sourceShape, |
| | | 217 | | LSCollider target, |
| | | 218 | | out Physics3DHit hit, |
| | | 219 | | out Fixed64 hitNumerator) |
| | | 220 | | { |
| | 4282 | 221 | | hit = default; |
| | 4282 | 222 | | hitNumerator = default; |
| | | 223 | | |
| | 4282 | 224 | | if (!CanSweptSourceShapeReachTarget(sourceShape, target)) |
| | 6 | 225 | | return false; |
| | | 226 | | |
| | 4276 | 227 | | if (target is LSCompoundCollider compound) |
| | | 228 | | { |
| | 520 | 229 | | return TrySweepTargetCompound( |
| | 520 | 230 | | sourceShape, |
| | 520 | 231 | | compound, |
| | 520 | 232 | | out hit, |
| | 520 | 233 | | out hitNumerator); |
| | | 234 | | } |
| | | 235 | | |
| | 3756 | 236 | | if (target is LSMeshCollider mesh && mesh.Mode == MeshColliderMode.Concave) |
| | | 237 | | { |
| | 1132 | 238 | | return TrySweepConcaveMeshTarget( |
| | 1132 | 239 | | sourceShape, |
| | 1132 | 240 | | mesh, |
| | 1132 | 241 | | out hit, |
| | 1132 | 242 | | out hitNumerator); |
| | | 243 | | } |
| | | 244 | | |
| | 2624 | 245 | | return TrySweepConvexTarget( |
| | 2624 | 246 | | sourceShape, |
| | 2624 | 247 | | CreateColliderShape(target, Vector3d.Zero), |
| | 2624 | 248 | | target, |
| | 2624 | 249 | | out hit, |
| | 2624 | 250 | | out hitNumerator); |
| | | 251 | | } |
| | | 252 | | |
| | | 253 | | private bool TrySweepTargetCompound( |
| | | 254 | | ConvexShape sourceShape, |
| | | 255 | | LSCompoundCollider compound, |
| | | 256 | | out Physics3DHit hit, |
| | | 257 | | out Fixed64 hitNumerator) |
| | | 258 | | { |
| | 520 | 259 | | hit = default; |
| | 520 | 260 | | hitNumerator = default; |
| | 520 | 261 | | bool found = false; |
| | 520 | 262 | | Fixed64 closestNumerator = Fixed64.MaxValue; |
| | 520 | 263 | | int closestPartIndex = int.MaxValue; |
| | | 264 | | |
| | 2090 | 265 | | for (int i = 0; i < compound.PartCount; i++) |
| | | 266 | | { |
| | 525 | 267 | | LSCollider part = compound.GetPartCollider(i); |
| | 525 | 268 | | if (!TrySweepSourceShape( |
| | 525 | 269 | | sourceShape, |
| | 525 | 270 | | part, |
| | 525 | 271 | | out Physics3DHit partHit, |
| | 525 | 272 | | out Fixed64 partNumerator) |
| | 525 | 273 | | || !ComesBeforeReducerCandidate( |
| | 525 | 274 | | partNumerator, |
| | 525 | 275 | | i, |
| | 525 | 276 | | found, |
| | 525 | 277 | | closestNumerator, |
| | 525 | 278 | | closestPartIndex)) |
| | | 279 | | { |
| | | 280 | | continue; |
| | | 281 | | } |
| | | 282 | | |
| | 487 | 283 | | hit = new Physics3DHit(compound, partHit.Anchor, partHit.Normal, partHit.Distance, partHit.Direction); |
| | 487 | 284 | | hitNumerator = partNumerator; |
| | 487 | 285 | | closestNumerator = partNumerator; |
| | 487 | 286 | | closestPartIndex = i; |
| | 487 | 287 | | found = true; |
| | | 288 | | } |
| | | 289 | | |
| | 520 | 290 | | return found; |
| | | 291 | | } |
| | | 292 | | |
| | | 293 | | private bool TrySweepConcaveMeshTarget( |
| | | 294 | | ConvexShape sourceShape, |
| | | 295 | | LSMeshCollider mesh, |
| | | 296 | | out Physics3DHit hit, |
| | | 297 | | out Fixed64 hitNumerator) |
| | | 298 | | { |
| | 1132 | 299 | | hit = default; |
| | 1132 | 300 | | hitNumerator = default; |
| | 1132 | 301 | | bool found = false; |
| | 1132 | 302 | | Fixed64 closestNumerator = Fixed64.MaxValue; |
| | 1132 | 303 | | int closestTriangleIndex = int.MaxValue; |
| | | 304 | | |
| | 1132 | 305 | | CreateSweptSourceBoundsInMeshFrame( |
| | 1132 | 306 | | sourceShape, |
| | 1132 | 307 | | mesh, |
| | 1132 | 308 | | out Vector3d min, |
| | 1132 | 309 | | out Vector3d max); |
| | | 310 | | |
| | 1132 | 311 | | mesh.Mesh.GetTrianglesInGeometryBounds( |
| | 1132 | 312 | | new FixedBoundVolume(min, max), |
| | 1132 | 313 | | _triangleCandidates); |
| | 1132 | 314 | | LastMeshTriangleCandidateCount += _triangleCandidates.Count; |
| | 1132 | 315 | | if (TrySweepCertifiedConvexPatch(sourceShape, mesh, out hit, out hitNumerator)) |
| | 173 | 316 | | return true; |
| | 959 | 317 | | BuildOrderedSweepTriangleCandidates(sourceShape, mesh); |
| | | 318 | | |
| | 4884 | 319 | | for (int i = 0; i < _sweepTriangleCandidates.Count; i++) |
| | | 320 | | { |
| | 1485 | 321 | | SweepTriangleCandidate sweepCandidate = _sweepTriangleCandidates[i]; |
| | 1485 | 322 | | if (RemainingSweepTrianglesCannotBeat( |
| | 1485 | 323 | | sweepCandidate.LowerBoundNumerator, |
| | 1485 | 324 | | found, |
| | 1485 | 325 | | closestNumerator)) |
| | | 326 | | { |
| | | 327 | | break; |
| | | 328 | | } |
| | | 329 | | |
| | 1483 | 330 | | int triangleIndex = sweepCandidate.TriangleIndex; |
| | | 331 | | // Zero is the best possible distance; skip triangles that cannot win even then. |
| | 1483 | 332 | | if (!ComesBeforeReducerCandidate(Fixed64.Zero, triangleIndex, |
| | 1483 | 333 | | found, closestNumerator, closestTriangleIndex)) |
| | | 334 | | continue; |
| | 1201 | 335 | | ConvexShape triangle = CreateTriangleShape(mesh, triangleIndex); |
| | 1201 | 336 | | if (!TrySweepConvexTarget( |
| | 1201 | 337 | | sourceShape, |
| | 1201 | 338 | | triangle, |
| | 1201 | 339 | | mesh, |
| | 1201 | 340 | | out Physics3DHit candidate, |
| | 1201 | 341 | | out Fixed64 candidateNumerator) |
| | 1201 | 342 | | || _admitTriangle != null && !_admitTriangle(triangleIndex, ref candidate, _admissionNormal) |
| | 1201 | 343 | | || !ComesBeforeReducerCandidate( |
| | 1201 | 344 | | candidateNumerator, |
| | 1201 | 345 | | triangleIndex, |
| | 1201 | 346 | | found, |
| | 1201 | 347 | | closestNumerator, |
| | 1201 | 348 | | closestTriangleIndex)) |
| | | 349 | | { |
| | | 350 | | continue; |
| | | 351 | | } |
| | | 352 | | |
| | 807 | 353 | | hit = candidate; |
| | 807 | 354 | | hitNumerator = candidateNumerator; |
| | 807 | 355 | | closestNumerator = candidateNumerator; |
| | 807 | 356 | | closestTriangleIndex = triangleIndex; |
| | 807 | 357 | | found = true; |
| | | 358 | | } |
| | | 359 | | |
| | 959 | 360 | | return found; |
| | | 361 | | } |
| | | 362 | | |
| | | 363 | | private void BuildOrderedSweepTriangleCandidates(ConvexShape sourceShape, LSMeshCollider mesh) |
| | | 364 | | { |
| | 959 | 365 | | _sweepTriangleCandidates.FastClear(); |
| | 4892 | 366 | | for (int i = 0; i < _triangleCandidates.Count; i++) |
| | | 367 | | { |
| | 1487 | 368 | | int triangleIndex = _triangleCandidates[i]; |
| | 1487 | 369 | | ConvexShape triangle = CreateTriangleShape(mesh, triangleIndex); |
| | 1487 | 370 | | if (TryComputeSweepLowerBoundNumerator( |
| | 1487 | 371 | | sourceShape, |
| | 1487 | 372 | | triangle, |
| | 1487 | 373 | | out Fixed64 lowerBoundNumerator)) |
| | | 374 | | { |
| | 1486 | 375 | | _sweepTriangleCandidates.Add( |
| | 1486 | 376 | | new SweepTriangleCandidate( |
| | 1486 | 377 | | triangleIndex, |
| | 1486 | 378 | | lowerBoundNumerator)); |
| | | 379 | | } |
| | | 380 | | } |
| | | 381 | | |
| | 959 | 382 | | _sweepTriangleCandidates.SortInPlace(SweepTriangleComparer); |
| | 959 | 383 | | } |
| | | 384 | | |
| | | 385 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 386 | | private bool TryComputeSweepLowerBoundNumerator( |
| | | 387 | | ConvexShape sourceShape, |
| | | 388 | | ConvexShape targetShape, |
| | | 389 | | out Fixed64 lowerBoundNumerator) |
| | | 390 | | { |
| | 1487 | 391 | | sourceShape.GetSourceBounds( |
| | 1487 | 392 | | out Vector3d sourceMin, |
| | 1487 | 393 | | out Vector3d sourceMax); |
| | 1487 | 394 | | targetShape.GetBounds( |
| | 1487 | 395 | | out Vector3d targetMin, |
| | 1487 | 396 | | out Vector3d targetMax); |
| | | 397 | | |
| | 1487 | 398 | | Vector3d padding = Vector3d.One * ContactTolerance; |
| | 1487 | 399 | | sourceMin -= padding; |
| | 1487 | 400 | | sourceMax += padding; |
| | 1487 | 401 | | return TryComputeSweepLowerBoundNumerator( |
| | 1487 | 402 | | sourceMin, |
| | 1487 | 403 | | sourceMax, |
| | 1487 | 404 | | targetMin, |
| | 1487 | 405 | | targetMax, |
| | 1487 | 406 | | _displacement, |
| | 1487 | 407 | | _length, |
| | 1487 | 408 | | out lowerBoundNumerator); |
| | | 409 | | } |
| | | 410 | | |
| | | 411 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 412 | | internal static bool TryComputeSweepLowerBoundNumerator( |
| | | 413 | | Vector3d sourceMin, |
| | | 414 | | Vector3d sourceMax, |
| | | 415 | | Vector3d targetMin, |
| | | 416 | | Vector3d targetMax, |
| | | 417 | | Vector3d displacement, |
| | | 418 | | Fixed64 length, |
| | | 419 | | out Fixed64 lowerBoundNumerator) |
| | | 420 | | { |
| | 1492 | 421 | | lowerBoundNumerator = Fixed64.Zero; |
| | 1492 | 422 | | return IncludeAxisEntryNumerator( |
| | 1492 | 423 | | sourceMin.X, |
| | 1492 | 424 | | sourceMax.X, |
| | 1492 | 425 | | targetMin.X, |
| | 1492 | 426 | | targetMax.X, |
| | 1492 | 427 | | displacement.X, |
| | 1492 | 428 | | length, |
| | 1492 | 429 | | ref lowerBoundNumerator) |
| | 1492 | 430 | | && IncludeAxisEntryNumerator( |
| | 1492 | 431 | | sourceMin.Y, |
| | 1492 | 432 | | sourceMax.Y, |
| | 1492 | 433 | | targetMin.Y, |
| | 1492 | 434 | | targetMax.Y, |
| | 1492 | 435 | | displacement.Y, |
| | 1492 | 436 | | length, |
| | 1492 | 437 | | ref lowerBoundNumerator) |
| | 1492 | 438 | | && IncludeAxisEntryNumerator( |
| | 1492 | 439 | | sourceMin.Z, |
| | 1492 | 440 | | sourceMax.Z, |
| | 1492 | 441 | | targetMin.Z, |
| | 1492 | 442 | | targetMax.Z, |
| | 1492 | 443 | | displacement.Z, |
| | 1492 | 444 | | length, |
| | 1492 | 445 | | ref lowerBoundNumerator); |
| | | 446 | | } |
| | | 447 | | |
| | | 448 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 449 | | private static bool IncludeAxisEntryNumerator( |
| | | 450 | | Fixed64 sourceMin, |
| | | 451 | | Fixed64 sourceMax, |
| | | 452 | | Fixed64 targetMin, |
| | | 453 | | Fixed64 targetMax, |
| | | 454 | | Fixed64 displacement, |
| | | 455 | | Fixed64 length, |
| | | 456 | | ref Fixed64 entryNumerator) |
| | | 457 | | { |
| | 4472 | 458 | | if (sourceMax >= targetMin |
| | 4472 | 459 | | && sourceMin <= targetMax) |
| | | 460 | | { |
| | 4390 | 461 | | return true; |
| | | 462 | | } |
| | | 463 | | |
| | | 464 | | Fixed64 gap; |
| | | 465 | | Fixed64 span; |
| | 82 | 466 | | if (sourceMax < targetMin |
| | 82 | 467 | | && displacement > Fixed64.Zero) |
| | | 468 | | { |
| | | 469 | | // Saturation is safe here: an unrepresentable positive gap is |
| | | 470 | | // necessarily farther than the representable sweep span. |
| | 73 | 471 | | gap = targetMin - sourceMax; |
| | 73 | 472 | | span = displacement; |
| | | 473 | | } |
| | 9 | 474 | | else if (sourceMin > targetMax |
| | 9 | 475 | | && displacement < Fixed64.Zero) |
| | | 476 | | { |
| | 7 | 477 | | gap = sourceMin - targetMax; |
| | | 478 | | // Prepare admits only representable chord magnitudes, so no |
| | | 479 | | // component can be Fixed64.MinValue here. |
| | 7 | 480 | | span = -displacement; |
| | | 481 | | } |
| | | 482 | | else |
| | | 483 | | { |
| | 2 | 484 | | return false; |
| | | 485 | | } |
| | | 486 | | |
| | 80 | 487 | | if (gap > span) |
| | 1 | 488 | | return false; |
| | | 489 | | |
| | | 490 | | // With 0 <= gap <= span, the fused result is bounded by the already |
| | | 491 | | // representable chord length. |
| | 79 | 492 | | _ = Fixed64.TryMultiplyDivide( |
| | 79 | 493 | | length, |
| | 79 | 494 | | gap, |
| | 79 | 495 | | span, |
| | 79 | 496 | | out Fixed64 axisNumerator); |
| | | 497 | | |
| | | 498 | | // Fused conversion keeps u = numerator / chord length exact until the |
| | | 499 | | // final Q32.32 rounding. One raw unit makes that rounded result a |
| | | 500 | | // conservative lower bound without moving an underflowed zero. |
| | 79 | 501 | | axisNumerator = FixedMath.Max( |
| | 79 | 502 | | Fixed64.Zero, |
| | 79 | 503 | | axisNumerator - Fixed64.Epsilon); |
| | | 504 | | |
| | 79 | 505 | | if (axisNumerator > entryNumerator) |
| | 62 | 506 | | entryNumerator = axisNumerator; |
| | 79 | 507 | | return true; |
| | | 508 | | } |
| | | 509 | | |
| | | 510 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 511 | | internal static bool RemainingSweepTrianglesCannotBeat( |
| | | 512 | | Fixed64 candidateLowerBoundNumerator, |
| | | 513 | | bool found, |
| | | 514 | | Fixed64 closestNumerator) |
| | | 515 | | { |
| | 1489 | 516 | | if (!found) |
| | 1181 | 517 | | return false; |
| | | 518 | | |
| | 308 | 519 | | return candidateLowerBoundNumerator |
| | 308 | 520 | | > closestNumerator + Fixed64.Epsilon; |
| | | 521 | | } |
| | | 522 | | |
| | | 523 | | private bool TrySweepConvexTarget( |
| | | 524 | | ConvexShape sourceShape, |
| | | 525 | | ConvexShape targetShape, |
| | | 526 | | LSCollider targetCollider, |
| | | 527 | | out Physics3DHit hit, |
| | | 528 | | out Fixed64 hitNumerator) |
| | | 529 | | { |
| | 4000 | 530 | | hit = default; |
| | 4000 | 531 | | hitNumerator = default; |
| | 4000 | 532 | | _admissionNormal = null; |
| | | 533 | | // u is retained as travelNumerator / _length. Keeping the common |
| | | 534 | | // denominator exact preserves small chord components and physical |
| | | 535 | | // distance resolution without exposing wide arithmetic. |
| | 4000 | 536 | | Fixed64 travelNumerator = Fixed64.Zero; |
| | 4000 | 537 | | Vector3d normal = Vector3d.Zero; |
| | 4000 | 538 | | GjkResult result = default; |
| | | 539 | | |
| | 8914 | 540 | | for (int i = 0; i < _maxConservativeAdvancementIterations; i++) |
| | | 541 | | { |
| | 4451 | 542 | | ConvexShape movedSource = |
| | 4451 | 543 | | sourceShape.WithSourceOffset( |
| | 4451 | 544 | | GetChordOffset(travelNumerator)); |
| | 4451 | 545 | | result = ComputeDistance(movedSource, targetShape); |
| | 4451 | 546 | | if (result.Intersects || result.Distance <= ContactTolerance) |
| | | 547 | | { |
| | 3764 | 548 | | ContactAnchor anchor = ResolveHitAnchor( |
| | 3764 | 549 | | targetShape, |
| | 3764 | 550 | | targetCollider, |
| | 3764 | 551 | | movedSource, |
| | 3764 | 552 | | result, |
| | 3764 | 553 | | out Vector3d point, |
| | 3764 | 554 | | out bool hasMaterializedPoint, |
| | 3764 | 555 | | out bool hasRefinedSurfaceNormal); |
| | 3764 | 556 | | Vector3d hitNormal = ResolveHitNormal( |
| | 3764 | 557 | | targetShape, |
| | 3764 | 558 | | targetCollider, |
| | 3764 | 559 | | point, |
| | 3764 | 560 | | result.Normal, |
| | 3764 | 561 | | normal, |
| | 3764 | 562 | | hasRefinedSurfaceNormal, |
| | 3764 | 563 | | hasMaterializedPoint, |
| | 3764 | 564 | | preserveOrientation: travelNumerator == Fixed64.Zero |
| | 3764 | 565 | | && targetCollider is LSMeshCollider); |
| | | 566 | | |
| | 3764 | 567 | | if (travelNumerator == Fixed64.Zero) |
| | | 568 | | { |
| | 3425 | 569 | | if (targetCollider is LSMeshCollider) |
| | | 570 | | { |
| | 2057 | 571 | | if (!TryResolveInitialMeshContact(sourceShape, targetShape, result, |
| | 2057 | 572 | | ref anchor, ref hitNormal)) |
| | 1 | 573 | | return false; |
| | | 574 | | } |
| | 1368 | 575 | | else if (_requireClosingInitialContact && hasMaterializedPoint) |
| | 5 | 576 | | hitNormal = targetCollider.GetNormalAtPoint(point); |
| | | 577 | | |
| | | 578 | | } |
| | 7187 | 579 | | if (travelNumerator == Fixed64.Zero) _admissionNormal = null; |
| | 3763 | 580 | | RetainCapsuleNormal(movedSource, targetShape, anchor, ref hitNormal); |
| | | 581 | | // Admission precedes leaf/triangle reduction so a separating |
| | | 582 | | // initial feature cannot hide a later closing sibling. |
| | | 583 | | // A declared surface selects closing response from its full |
| | | 584 | | // native region; one triangle's tied exit cannot preempt it. |
| | 3763 | 585 | | if (travelNumerator == Fixed64.Zero && _requireClosingInitialContact && _admitTriangle == null |
| | 3932 | 586 | | && Vector3d.Dot(_displacement, hitNormal) >= -Fixed64.Epsilon) return false; |
| | 3594 | 587 | | hit = new Physics3DHit( |
| | 3594 | 588 | | targetCollider, |
| | 3594 | 589 | | anchor, |
| | 3594 | 590 | | hitNormal, |
| | 3594 | 591 | | travelNumerator, |
| | 3594 | 592 | | _outputDirection); |
| | 3594 | 593 | | hitNumerator = travelNumerator; |
| | 3594 | 594 | | return true; |
| | | 595 | | } |
| | | 596 | | |
| | 687 | 597 | | normal = result.Normal; |
| | | 598 | | // Projecting a representable chord onto a unit normal is bounded |
| | | 599 | | // by the admitted chord length. |
| | 687 | 600 | | _ = _displacementAnchor.TryGetProjectedOffsetFrom( |
| | 687 | 601 | | ZeroAnchor, |
| | 687 | 602 | | -normal, |
| | 687 | 603 | | out Fixed64 closingPerFraction); |
| | 687 | 604 | | if (closingPerFraction <= Fixed64.Epsilon) |
| | | 605 | | { |
| | 138 | 606 | | return TryResolveEndpointBracket( |
| | 138 | 607 | | sourceShape, |
| | 138 | 608 | | targetShape, |
| | 138 | 609 | | targetCollider, |
| | 138 | 610 | | travelNumerator, |
| | 138 | 611 | | out hit, |
| | 138 | 612 | | out hitNumerator); |
| | | 613 | | } |
| | 549 | 614 | | Fixed64 boundedDistance = |
| | 549 | 615 | | FixedMath.Min( |
| | 549 | 616 | | result.Distance, |
| | 549 | 617 | | closingPerFraction); |
| | | 618 | | // Capping the advancement ratio at one preserves the endpoint |
| | | 619 | | // decision while keeping the fused result inside the chord length. |
| | 549 | 620 | | _ = Fixed64.TryMultiplyDivide( |
| | 549 | 621 | | boundedDistance, |
| | 549 | 622 | | _length, |
| | 549 | 623 | | closingPerFraction, |
| | 549 | 624 | | out Fixed64 stepNumerator); |
| | 549 | 625 | | Fixed64 nextTravelNumerator = |
| | 549 | 626 | | travelNumerator + stepNumerator; |
| | 549 | 627 | | if (nextTravelNumerator >= _length) |
| | | 628 | | { |
| | | 629 | | // A capped full-length step also covers every overshoot. Reuse |
| | | 630 | | // endpoint materialization without refining a zero-width bracket. |
| | 92 | 631 | | return TryResolveEndpointBracket(sourceShape, targetShape, targetCollider, |
| | 92 | 632 | | _length, out hit, out hitNumerator, normal); |
| | | 633 | | } |
| | | 634 | | |
| | 457 | 635 | | travelNumerator = nextTravelNumerator; |
| | | 636 | | } |
| | | 637 | | |
| | 6 | 638 | | return TryResolveEndpointBracket( |
| | 6 | 639 | | sourceShape, |
| | 6 | 640 | | targetShape, |
| | 6 | 641 | | targetCollider, |
| | 6 | 642 | | travelNumerator, |
| | 6 | 643 | | out hit, |
| | 6 | 644 | | out hitNumerator); |
| | | 645 | | } |
| | | 646 | | |
| | | 647 | | private bool TryResolveEndpointBracket( |
| | | 648 | | ConvexShape sourceShape, |
| | | 649 | | ConvexShape targetShape, |
| | | 650 | | LSCollider targetCollider, |
| | | 651 | | Fixed64 lowerNumerator, |
| | | 652 | | out Physics3DHit hit, |
| | | 653 | | out Fixed64 hitNumerator, |
| | | 654 | | Vector3d fallbackNormal = default) |
| | | 655 | | { |
| | 236 | 656 | | hit = default; |
| | 236 | 657 | | hitNumerator = default; |
| | 236 | 658 | | Fixed64 upperNumerator = _length; |
| | 236 | 659 | | ConvexShape upperSource = |
| | 236 | 660 | | sourceShape.WithSourceOffset(_displacement); |
| | 236 | 661 | | GjkResult upperResult = |
| | 236 | 662 | | ComputeDistance(upperSource, targetShape); |
| | 236 | 663 | | if (!upperResult.Intersects |
| | 236 | 664 | | && upperResult.Distance > ContactTolerance) |
| | | 665 | | { |
| | 226 | 666 | | return false; |
| | | 667 | | } |
| | | 668 | | |
| | 10 | 669 | | for (int iteration = 0; |
| | 12 | 670 | | iteration < _maxConservativeAdvancementIterations |
| | 12 | 671 | | && upperNumerator - lowerNumerator > DistanceTolerance; |
| | 2 | 672 | | iteration++) |
| | | 673 | | { |
| | 2 | 674 | | Fixed64 middleNumerator = |
| | 2 | 675 | | FixedMath.Midpoint( |
| | 2 | 676 | | lowerNumerator, |
| | 2 | 677 | | upperNumerator); |
| | 2 | 678 | | ConvexShape middleSource = |
| | 2 | 679 | | sourceShape.WithSourceOffset( |
| | 2 | 680 | | GetChordOffset(middleNumerator)); |
| | 2 | 681 | | GjkResult middleResult = |
| | 2 | 682 | | ComputeDistance(middleSource, targetShape); |
| | 2 | 683 | | if (middleResult.Intersects |
| | 2 | 684 | | || middleResult.Distance <= ContactTolerance) |
| | | 685 | | { |
| | 1 | 686 | | upperNumerator = middleNumerator; |
| | 1 | 687 | | upperSource = middleSource; |
| | 1 | 688 | | upperResult = middleResult; |
| | | 689 | | } |
| | | 690 | | else |
| | | 691 | | { |
| | 1 | 692 | | lowerNumerator = middleNumerator; |
| | | 693 | | } |
| | | 694 | | } |
| | | 695 | | |
| | 10 | 696 | | ContactAnchor anchor = ResolveHitAnchor( |
| | 10 | 697 | | targetShape, |
| | 10 | 698 | | targetCollider, |
| | 10 | 699 | | upperSource, |
| | 10 | 700 | | upperResult, |
| | 10 | 701 | | out Vector3d point, |
| | 10 | 702 | | out bool hasMaterializedPoint, |
| | 10 | 703 | | out bool hasRefinedSurfaceNormal); |
| | 10 | 704 | | Vector3d hitNormal = ResolveHitNormal( |
| | 10 | 705 | | targetShape, |
| | 10 | 706 | | targetCollider, |
| | 10 | 707 | | point, |
| | 10 | 708 | | upperResult.Normal, |
| | 10 | 709 | | fallbackNormal, |
| | 10 | 710 | | hasRefinedSurfaceNormal, |
| | 10 | 711 | | hasMaterializedPoint); |
| | 10 | 712 | | RetainCapsuleNormal(upperSource, targetShape, anchor, ref hitNormal); |
| | 10 | 713 | | hit = new Physics3DHit( |
| | 10 | 714 | | targetCollider, |
| | 10 | 715 | | anchor, |
| | 10 | 716 | | hitNormal, |
| | 10 | 717 | | upperNumerator, |
| | 10 | 718 | | _outputDirection); |
| | 10 | 719 | | hitNumerator = upperNumerator; |
| | 10 | 720 | | return true; |
| | | 721 | | } |
| | | 722 | | |
| | | 723 | | private void RetainCapsuleNormal(in ConvexShape source, in ConvexShape target, in ContactAnchor anchor, ref Vector3d |
| | | 724 | | { |
| | 7410 | 725 | | if (_admitTriangle == null || !source.TryGetCapsuleTriangleNormalAxis(target, anchor.GeometryPoint, out WideDire |
| | 136 | 726 | | _admissionNormal = axis; |
| | 136 | 727 | | normal = axis.GetNormal(false); |
| | 136 | 728 | | } |
| | | 729 | | |
| | | 730 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 731 | | private Vector3d GetChordOffset(Fixed64 numerator) => |
| | 4453 | 732 | | _chord.GetPointAtDistance( |
| | 4453 | 733 | | numerator, |
| | 4453 | 734 | | _length); |
| | | 735 | | |
| | | 736 | | private ContactAnchor ResolveHitAnchor( |
| | | 737 | | ConvexShape targetShape, |
| | | 738 | | LSCollider targetCollider, |
| | | 739 | | ConvexShape movedSource, |
| | | 740 | | GjkResult result, |
| | | 741 | | out Vector3d point, |
| | | 742 | | out bool hasMaterializedPoint, |
| | | 743 | | out bool hasRefinedSurfaceNormal) |
| | | 744 | | { |
| | 3774 | 745 | | hasMaterializedPoint = true; |
| | 3774 | 746 | | hasRefinedSurfaceNormal = false; |
| | 3774 | 747 | | if (targetCollider is LSSphereCollider |
| | 3774 | 748 | | && movedSource.TryGetClosestPointOnSurface( |
| | 3774 | 749 | | targetCollider.Center, |
| | 3774 | 750 | | out Vector3d sourcePoint) |
| | 3774 | 751 | | && Vector3d.TrySubtract( |
| | 3774 | 752 | | sourcePoint, |
| | 3774 | 753 | | targetCollider.Center, |
| | 3774 | 754 | | out Vector3d centerToSource) |
| | 3774 | 755 | | && centerToSource.MagnitudeSquared > Fixed64.Epsilon) |
| | | 756 | | { |
| | | 757 | | // A sphere's closest pair is defined by its center and the closest |
| | | 758 | | // source feature. Refining that feature removes arbitrary support |
| | | 759 | | // tie bias without changing the conservative TOI. |
| | 55 | 760 | | FixedPointAnchor sphereAnchor = |
| | 55 | 761 | | ConvexColliderSupport.GetSupportAnchor( |
| | 55 | 762 | | targetCollider, |
| | 55 | 763 | | centerToSource, |
| | 55 | 764 | | Vector3d.Zero); |
| | 55 | 765 | | hasMaterializedPoint = sphereAnchor.TryGetPoint(out point); |
| | 55 | 766 | | hasRefinedSurfaceNormal = hasMaterializedPoint; |
| | 55 | 767 | | return new ContactAnchor(sphereAnchor); |
| | | 768 | | } |
| | | 769 | | |
| | 3719 | 770 | | FixedPointAnchor movedSourceCenter = movedSource.GetCenterAnchor(); |
| | 3719 | 771 | | FixedPointAnchor targetCenter = targetShape.GetCenterAnchor(); |
| | 3719 | 772 | | if (!targetShape.IsConvexPatch && movedSourceCenter.TryGetOffsetFrom( |
| | 3719 | 773 | | targetCenter, |
| | 3719 | 774 | | out Vector3d centerDifference) |
| | 3719 | 775 | | && centerDifference.MagnitudeSquared <= Fixed64.Epsilon) |
| | | 776 | | { |
| | 12 | 777 | | FixedPointAnchor fallbackAnchor = |
| | 12 | 778 | | targetShape.GetFallbackSurfaceAnchor(-_displacement); |
| | 12 | 779 | | hasMaterializedPoint = fallbackAnchor.TryGetPoint(out point); |
| | 12 | 780 | | return new ContactAnchor(fallbackAnchor); |
| | | 781 | | } |
| | | 782 | | |
| | | 783 | | // GJK's target witness identifies the feature that stopped the sweep. |
| | | 784 | | // Center-to-center projection can select an unrelated feature for long, |
| | | 785 | | // offset shapes and therefore produce a non-physical response normal. |
| | 3707 | 786 | | if (!result.PointB.TryGetPoint(out point)) |
| | | 787 | | { |
| | 5 | 788 | | point = default; |
| | 5 | 789 | | hasMaterializedPoint = false; |
| | 5 | 790 | | return new ContactAnchor(result.PointB); |
| | | 791 | | } |
| | | 792 | | |
| | 3702 | 793 | | return new ContactAnchor(result.PointB); |
| | | 794 | | } |
| | | 795 | | |
| | | 796 | | private Vector3d ResolveHitNormal( |
| | | 797 | | ConvexShape targetShape, |
| | | 798 | | LSCollider targetCollider, |
| | | 799 | | Vector3d point, |
| | | 800 | | Vector3d resultNormal, |
| | | 801 | | Vector3d fallbackNormal, |
| | | 802 | | bool hasRefinedSurfaceNormal, |
| | | 803 | | bool hasMaterializedPoint, |
| | | 804 | | bool preserveOrientation = false) |
| | | 805 | | { |
| | 3774 | 806 | | Vector3d planarNormal = Vector3d.Zero; |
| | | 807 | | // Retain the selected triangle plane before the hit policy normalizes |
| | | 808 | | // its already rounded inspection normal. Unknown/initial replacements |
| | | 809 | | // cannot authorize a continuity exclusion. |
| | 3774 | 810 | | if (_admitTriangle != null) |
| | | 811 | | { |
| | | 812 | | // The only production hook targets declared concave meshes; |
| | | 813 | | // its plane does not depend on a scalar world witness. Such a |
| | | 814 | | // witness may lie beyond a scalar face despite bounded geometry. |
| | | 815 | | System.Diagnostics.Debug.Assert(!hasRefinedSurfaceNormal); |
| | 153 | 816 | | targetShape.TryGetPlanarSurfaceNormal(point, out planarNormal); |
| | 153 | 817 | | WideDirection3d plane = targetShape.GetTriangleNormalAxis(); |
| | 153 | 818 | | _admissionNormal = !preserveOrientation && Vector3d.CompareProjection(planarNormal, Vector3d.Zero, _displace |
| | | 819 | | } |
| | 3621 | 820 | | else if (hasMaterializedPoint) |
| | 3616 | 821 | | targetShape.TryGetPlanarSurfaceNormal(point, out planarNormal); |
| | 3774 | 822 | | return ConvexSweepHitPolicy.ResolveHitNormal( |
| | 3774 | 823 | | targetCollider, |
| | 3774 | 824 | | point, |
| | 3774 | 825 | | resultNormal, |
| | 3774 | 826 | | fallbackNormal, |
| | 3774 | 827 | | preserveOrientation ? Vector3d.Zero : _displacement, |
| | 3774 | 828 | | planarNormal, |
| | 3774 | 829 | | hasRefinedSurfaceNormal, |
| | 3774 | 830 | | hasMaterializedPoint); |
| | | 831 | | } |
| | | 832 | | |
| | | 833 | | private static ConvexShape CreateColliderShape(LSCollider collider, Vector3d offset) |
| | | 834 | | { |
| | 5150 | 835 | | return new ConvexShape(collider, offset); |
| | | 836 | | } |
| | | 837 | | |
| | | 838 | | private static ConvexShape CreateTriangleShape(LSMeshCollider mesh, int triangleIndex) |
| | | 839 | | { |
| | 2688 | 840 | | mesh.Mesh.GetGeometryTriangleVertices( |
| | 2688 | 841 | | triangleIndex, |
| | 2688 | 842 | | out Vector3d first, |
| | 2688 | 843 | | out Vector3d second, |
| | 2688 | 844 | | out Vector3d third); |
| | 2688 | 845 | | return new ConvexShape(mesh, triangleIndex, first, second, third); |
| | | 846 | | } |
| | | 847 | | |
| | | 848 | | private static void ThrowIfConcaveSource(LSMeshCollider source) |
| | | 849 | | { |
| | 106 | 850 | | if (source.Mode == MeshColliderMode.Concave) |
| | 1 | 851 | | throw CreateConcaveSourceException(source); |
| | 105 | 852 | | } |
| | | 853 | | |
| | | 854 | | private void CreateSweptSourceBoundsInMeshFrame( |
| | | 855 | | ConvexShape sourceShape, |
| | | 856 | | LSMeshCollider mesh, |
| | | 857 | | out Vector3d min, |
| | | 858 | | out Vector3d max) |
| | | 859 | | { |
| | 1132 | 860 | | if (!mesh.Mesh.GeometryFrame.IsRigid) |
| | | 861 | | { |
| | 33 | 862 | | _ = sourceShape.TryGetBoundsRelativeTo(mesh.Mesh.Origin, FixedQuaternion.Identity, |
| | 33 | 863 | | out Vector3d worldMin, out Vector3d worldMax); |
| | 33 | 864 | | SweepBoundsUtility.CreateSweptBounds(worldMin, worldMax, _displacement, |
| | 33 | 865 | | ContactTolerance, out worldMin, out worldMax); |
| | 33 | 866 | | bool represented = ScaledRigidFrame3d.Rigid(mesh.Mesh.Origin, FixedQuaternion.Identity) |
| | 33 | 867 | | .TryGetBoundsIn(worldMin, worldMax, mesh.Mesh.GeometryFrame, out FixedBoundBox bounds); |
| | | 868 | | System.Diagnostics.Debug.Assert(represented); |
| | 66 | 869 | | min = bounds.Min; max = bounds.Max; |
| | 33 | 870 | | return; |
| | | 871 | | } |
| | | 872 | | // Concave-target dispatch receives only the source's committed shape; |
| | | 873 | | // iterative chord offsets are introduced after broad-phase collection. |
| | 1099 | 874 | | _ = sourceShape.TryGetBoundsRelativeTo( |
| | 1099 | 875 | | mesh.Mesh.Origin, |
| | 1099 | 876 | | mesh.Mesh.Rotation, |
| | 1099 | 877 | | out Vector3d sourceMin, |
| | 1099 | 878 | | out Vector3d sourceMax); |
| | | 879 | | |
| | | 880 | | // Prepare admitted the chord magnitude, so a unit rotation preserves |
| | | 881 | | // representability. |
| | 1099 | 882 | | _ = mesh.Mesh.Rotation.Inverse().TryRotate( |
| | 1099 | 883 | | _displacement, |
| | 1099 | 884 | | out Vector3d localDisplacement); |
| | 1099 | 885 | | SweepBoundsUtility.CreateSweptBounds( |
| | 1099 | 886 | | sourceMin, |
| | 1099 | 887 | | sourceMax, |
| | 1099 | 888 | | localDisplacement, |
| | 1099 | 889 | | ContactTolerance, |
| | 1099 | 890 | | out min, |
| | 1099 | 891 | | out max); |
| | 1099 | 892 | | } |
| | | 893 | | |
| | | 894 | | private bool CanSweptSourceShapeReachTarget(ConvexShape sourceShape, LSCollider target) |
| | | 895 | | { |
| | 4282 | 896 | | if (!sourceShape.CanTranslateCenter(_displacement)) |
| | 1 | 897 | | return false; |
| | | 898 | | |
| | 4281 | 899 | | sourceShape.GetSourceBounds(out Vector3d sourceMin, out Vector3d sourceMax); |
| | 4281 | 900 | | SweepBoundsUtility.CreateSweptBounds( |
| | 4281 | 901 | | sourceMin, |
| | 4281 | 902 | | sourceMax, |
| | 4281 | 903 | | _displacement, |
| | 4281 | 904 | | ContactTolerance, |
| | 4281 | 905 | | out Vector3d min, |
| | 4281 | 906 | | out Vector3d max); |
| | 4281 | 907 | | return SweepBoundsUtility.OverlapsInclusive( |
| | 4281 | 908 | | min, |
| | 4281 | 909 | | max, |
| | 4281 | 910 | | target.BoundsMin, |
| | 4281 | 911 | | target.BoundsMax); |
| | | 912 | | } |
| | | 913 | | |
| | | 914 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 915 | | private static bool ComesBeforeReducerCandidate( |
| | | 916 | | Fixed64 hitNumerator, |
| | | 917 | | int candidateOrdinal, |
| | | 918 | | bool found, |
| | | 919 | | Fixed64 closestNumerator, |
| | | 920 | | int closestOrdinal) |
| | | 921 | | { |
| | 2817 | 922 | | if (!found) |
| | 2487 | 923 | | return true; |
| | | 924 | | |
| | 330 | 925 | | int numeratorCompare = |
| | 330 | 926 | | hitNumerator.CompareTo(closestNumerator); |
| | 330 | 927 | | if (numeratorCompare != 0) |
| | 35 | 928 | | return numeratorCompare < 0; |
| | | 929 | | |
| | 295 | 930 | | return candidateOrdinal < closestOrdinal; |
| | | 931 | | } |
| | | 932 | | |
| | | 933 | | private static ArgumentException CreateConcaveSourceException(LSMeshCollider source) => |
| | 1 | 934 | | new("Concave mesh sources are not supported by swept query APIs. Use an LSCompoundCollider built from authored c |
| | | 935 | | } |