| | | 1 | | //======================================================================= |
| | | 2 | | // FiniteSlabProjectionSweep.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 | | |
| | | 14 | | namespace Gravitas.Queries; |
| | | 15 | | |
| | | 16 | | /// <summary> |
| | | 17 | | /// Sweeps an X/Z circle against the projection of curved 3D targets clipped to |
| | | 18 | | /// a finite Y slab. |
| | | 19 | | /// </summary> |
| | | 20 | | /// <content>Owns conservative advancement and projected support simplex reduction for curved targets.</content> |
| | | 21 | | internal static partial class FiniteSlabProjectionSweep |
| | | 22 | | { |
| | | 23 | | private const int MaxGjkIterations = 32; |
| | | 24 | | private const int MaxConservativeAdvancementIterations = 32; |
| | 1 | 25 | | private static readonly Fixed64 DistanceTolerance = Fixed64.FromFraction(1, 1_048_576); |
| | 1 | 26 | | private static readonly Fixed64 SweepContactTolerance = DistanceTolerance; |
| | | 27 | | |
| | | 28 | | public static bool TrySweepCircleAgainstCapsule( |
| | | 29 | | Vector2d start, |
| | | 30 | | Vector2d end, |
| | | 31 | | Vector2d direction, |
| | | 32 | | Fixed64 length, |
| | | 33 | | Fixed64 radius, |
| | | 34 | | Fixed64 slabMinY, |
| | | 35 | | Fixed64 slabMaxY, |
| | | 36 | | LSCapsuleCollider capsule, |
| | | 37 | | out Fixed64 distance, |
| | | 38 | | int maxConservativeAdvancementIterations = MaxConservativeAdvancementIterations) |
| | | 39 | | { |
| | 205 | 40 | | var target = ProjectionTarget.CreateCapsule(capsule, slabMinY, slabMaxY); |
| | 205 | 41 | | return TrySweepCircle( |
| | 205 | 42 | | start, |
| | 205 | 43 | | end, |
| | 205 | 44 | | direction, |
| | 205 | 45 | | length, |
| | 205 | 46 | | radius, |
| | 205 | 47 | | target, |
| | 205 | 48 | | out distance, |
| | 205 | 49 | | maxConservativeAdvancementIterations); |
| | | 50 | | } |
| | | 51 | | |
| | | 52 | | public static bool TrySweepCircleAgainstCylinder( |
| | | 53 | | Vector2d start, |
| | | 54 | | Vector2d end, |
| | | 55 | | Vector2d direction, |
| | | 56 | | Fixed64 length, |
| | | 57 | | Fixed64 radius, |
| | | 58 | | Fixed64 slabMinY, |
| | | 59 | | Fixed64 slabMaxY, |
| | | 60 | | LSCylinderCollider cylinder, |
| | | 61 | | out Fixed64 distance, |
| | | 62 | | int maxConservativeAdvancementIterations = MaxConservativeAdvancementIterations) |
| | | 63 | | { |
| | 210 | 64 | | var target = ProjectionTarget.CreateCylinder(cylinder, slabMinY, slabMaxY); |
| | 210 | 65 | | return TrySweepCircle( |
| | 210 | 66 | | start, |
| | 210 | 67 | | end, |
| | 210 | 68 | | direction, |
| | 210 | 69 | | length, |
| | 210 | 70 | | radius, |
| | 210 | 71 | | target, |
| | 210 | 72 | | out distance, |
| | 210 | 73 | | maxConservativeAdvancementIterations); |
| | | 74 | | } |
| | | 75 | | |
| | | 76 | | public static bool TrySweepCircleAgainstCone( |
| | | 77 | | Vector2d start, |
| | | 78 | | Vector2d end, |
| | | 79 | | Vector2d direction, |
| | | 80 | | Fixed64 length, |
| | | 81 | | Fixed64 radius, |
| | | 82 | | Fixed64 slabMinY, |
| | | 83 | | Fixed64 slabMaxY, |
| | | 84 | | LSConeCollider cone, |
| | | 85 | | out Fixed64 distance, |
| | | 86 | | int maxConservativeAdvancementIterations = MaxConservativeAdvancementIterations) |
| | | 87 | | { |
| | 187 | 88 | | var target = ProjectionTarget.CreateCone(cone, slabMinY, slabMaxY); |
| | 187 | 89 | | return TrySweepCircle( |
| | 187 | 90 | | start, |
| | 187 | 91 | | end, |
| | 187 | 92 | | direction, |
| | 187 | 93 | | length, |
| | 187 | 94 | | radius, |
| | 187 | 95 | | target, |
| | 187 | 96 | | out distance, |
| | 187 | 97 | | maxConservativeAdvancementIterations); |
| | | 98 | | } |
| | | 99 | | |
| | | 100 | | private static bool TrySweepCircle( |
| | | 101 | | Vector2d start, |
| | | 102 | | Vector2d end, |
| | | 103 | | Vector2d direction, |
| | | 104 | | Fixed64 length, |
| | | 105 | | Fixed64 radius, |
| | | 106 | | ProjectionTarget target, |
| | | 107 | | out Fixed64 distance, |
| | | 108 | | int maxConservativeAdvancementIterations) |
| | | 109 | | { |
| | 602 | 110 | | distance = Fixed64.Zero; |
| | 602 | 111 | | if (!Vector2d.TryGetMagnitude(direction, out Fixed64 directionMagnitude) |
| | 602 | 112 | | || (directionMagnitude != Fixed64.Zero |
| | 602 | 113 | | && FixedMath.Abs(directionMagnitude - Fixed64.One) > Fixed64.Epsilon) |
| | 602 | 114 | | || maxConservativeAdvancementIterations <= 0 |
| | 602 | 115 | | || !target.TrySupport(Vector2d.Right, out Vector2d rightSupport, out bool translated)) |
| | 18 | 116 | | return false; |
| | | 117 | | |
| | 584 | 118 | | if (target.TryGetPlanarCircle(rightSupport, translated, out Vector2d targetCenter, out Fixed64 targetRadius)) |
| | | 119 | | { |
| | 257 | 120 | | return new FixedSegment2d(start, end) |
| | 257 | 121 | | .TryGetCircleIntersectionDistanceInterval( |
| | 257 | 122 | | new FixedBoundCircle(targetCenter, targetRadius), |
| | 257 | 123 | | radius, |
| | 257 | 124 | | length, |
| | 257 | 125 | | out distance, |
| | 257 | 126 | | out _, |
| | 257 | 127 | | out _, |
| | 257 | 128 | | out _); |
| | | 129 | | } |
| | | 130 | | |
| | 327 | 131 | | Fixed64 travelDistance = Fixed64.Zero; |
| | 327 | 132 | | Vector2d searchDirection = Vector2d.Zero; |
| | 327 | 133 | | var segment = new FixedSegment2d(start, end); |
| | 327 | 134 | | Vector2d displacement = end - start; |
| | 1088 | 135 | | for (int i = 0; i < maxConservativeAdvancementIterations; i++) |
| | | 136 | | { |
| | 543 | 137 | | Vector2d point = segment.GetPointAtDistance(travelDistance, length); |
| | 543 | 138 | | PlanarGjkResult result = ComputeDistance(point, radius, target, searchDirection); |
| | | 139 | | |
| | 543 | 140 | | if (result.Distance <= SweepContactTolerance) |
| | | 141 | | { |
| | 313 | 142 | | distance = travelDistance; |
| | 313 | 143 | | return true; |
| | | 144 | | } |
| | | 145 | | |
| | 230 | 146 | | Vector2d normal = result.Normal; |
| | | 147 | | // Reuse the resolved separating direction at the next nearby pose. |
| | 230 | 148 | | searchDirection = -normal; |
| | | 149 | | // Keep the authored chord over its distance denominator. A rounded |
| | | 150 | | // unit ray can enter a shape that the original segment only grazes. |
| | 230 | 151 | | Signed320 closing = WideArithmetic.SubtractSigned320(default, |
| | 230 | 152 | | WideArithmetic.AddSigned320( |
| | 230 | 153 | | WideArithmetic.MultiplySigned192(Signed192.Raw(displacement.X), Signed192.Raw(normal.X)), |
| | 230 | 154 | | WideArithmetic.MultiplySigned192(Signed192.Raw(displacement.Y), Signed192.Raw(normal.Y)))); |
| | 230 | 155 | | if (WideArithmetic.SubtractSigned320(closing, |
| | 230 | 156 | | WideArithmetic.MultiplySigned192(Signed192.Raw(Fixed64.Epsilon), Signed192.Raw(length))).Sign <= 0) |
| | 8 | 157 | | return false; |
| | | 158 | | |
| | 222 | 159 | | Fixed64 remainingDistance = length - travelDistance; |
| | 222 | 160 | | bool reachedEndpoint = !Fixed64.TryGetSignedRawRatio( |
| | 222 | 161 | | Signed832.ExtendValue(Signed576.ExtendValue(WideArithmetic.MultiplySigned192( |
| | 222 | 162 | | Signed192.Raw(result.Distance), Signed192.Raw(length)))), |
| | 222 | 163 | | Signed832.ExtendValue(Signed576.ExtendValue(closing)), |
| | 222 | 164 | | 32, |
| | 222 | 165 | | out Fixed64 stepDistance) |
| | 222 | 166 | | || stepDistance > remainingDistance; |
| | 222 | 167 | | if (reachedEndpoint) |
| | | 168 | | { |
| | 5 | 169 | | PlanarGjkResult endpoint = ComputeDistance(end, radius, target, searchDirection); |
| | | 170 | | |
| | 5 | 171 | | if (endpoint.Distance <= SweepContactTolerance) |
| | | 172 | | { |
| | 1 | 173 | | distance = length; |
| | 1 | 174 | | return true; |
| | | 175 | | } |
| | | 176 | | |
| | 4 | 177 | | return false; |
| | | 178 | | } |
| | | 179 | | |
| | 217 | 180 | | travelDistance += stepDistance; |
| | | 181 | | } |
| | | 182 | | |
| | 1 | 183 | | return false; |
| | | 184 | | } |
| | | 185 | | |
| | | 186 | | private static PlanarGjkResult ComputeDistance( |
| | | 187 | | Vector2d point, |
| | | 188 | | Fixed64 expansionRadius, |
| | | 189 | | ProjectionTarget target, |
| | | 190 | | Vector2d initialDirection) |
| | | 191 | | { |
| | 548 | 192 | | Span<PlanarSupportPoint> simplex = stackalloc PlanarSupportPoint[3]; |
| | 548 | 193 | | int workingShift = target.SelectWorkingShift(point, expansionRadius); |
| | 548 | 194 | | Fixed64 workingScale = GjkSimplexScale.GetCoordinateScale(workingShift); |
| | 548 | 195 | | Fixed64 workingDistanceTolerance = DistanceTolerance * workingScale; |
| | 548 | 196 | | int simplexCount = 0; |
| | 548 | 197 | | Vector2d direction = initialDirection == Vector2d.Zero |
| | 548 | 198 | | ? GjkSimplexScale.CreateWorkingDifference(target.Center, point, workingShift) |
| | 548 | 199 | | : initialDirection; |
| | 548 | 200 | | if (direction == Vector2d.Zero) |
| | 9 | 201 | | direction = Vector2d.Right; |
| | | 202 | | |
| | 548 | 203 | | bool hasPreviousDistance = false; |
| | 548 | 204 | | Fixed64 previousDistance = Fixed64.Zero; |
| | 548 | 205 | | ClosestPlanarSimplexResult closest = default; |
| | 548 | 206 | | bool distanceIsRepresentable = false; |
| | 548 | 207 | | Fixed64 workingDistance = Fixed64.MaxValue; |
| | | 208 | | |
| | 3640 | 209 | | for (int i = 0; i < MaxGjkIterations; i++) |
| | | 210 | | { |
| | 1820 | 211 | | PlanarSupportPoint support = CreateSupportPoint( |
| | 1820 | 212 | | point, expansionRadius, target, direction, workingShift); |
| | | 213 | | |
| | 1820 | 214 | | if (ContainsSupportPoint(simplex, simplexCount, support.Point)) |
| | | 215 | | break; |
| | | 216 | | |
| | 1653 | 217 | | simplex[simplexCount++] = support; |
| | 1653 | 218 | | closest = SolveClosestSimplex(simplex, ref simplexCount); |
| | 1653 | 219 | | distanceIsRepresentable = Vector2d.TryGetMagnitude(closest.Point, out workingDistance); |
| | 1653 | 220 | | if (closest.Intersects |
| | 1653 | 221 | | || (distanceIsRepresentable && workingDistance <= workingDistanceTolerance)) |
| | | 222 | | { |
| | 314 | 223 | | return PlanarGjkResult.Intersection; |
| | | 224 | | } |
| | | 225 | | |
| | 1339 | 226 | | if (hasPreviousDistance |
| | 1339 | 227 | | && distanceIsRepresentable |
| | 1339 | 228 | | && previousDistance - workingDistance <= Fixed64.Epsilon) |
| | | 229 | | { |
| | | 230 | | break; |
| | | 231 | | } |
| | | 232 | | |
| | 1272 | 233 | | hasPreviousDistance = distanceIsRepresentable; |
| | 1272 | 234 | | previousDistance = workingDistance; |
| | 1272 | 235 | | direction = closest.SearchDirection; |
| | | 236 | | } |
| | | 237 | | |
| | 234 | 238 | | Fixed64 distance = GjkSimplexScale.RestoreDistance(workingDistance, workingShift); |
| | 234 | 239 | | Vector2d normal = -WideNormalization.GetNormalized(closest.SearchDirection); |
| | 234 | 240 | | return new PlanarGjkResult(distance, normal); |
| | | 241 | | } |
| | | 242 | | |
| | | 243 | | private static PlanarSupportPoint CreateSupportPoint( |
| | | 244 | | Vector2d point, |
| | | 245 | | Fixed64 expansionRadius, |
| | | 246 | | ProjectionTarget target, |
| | | 247 | | Vector2d direction, |
| | | 248 | | int workingShift) |
| | | 249 | | { |
| | 1820 | 250 | | Vector2d supportDirection = WideNormalization.GetNormalized(direction); |
| | 1820 | 251 | | Vector2d targetDirection = -supportDirection; |
| | | 252 | | // The initial right support established a nonempty slab intersection; |
| | | 253 | | // admission and the chosen chart bound every later directional support. |
| | 1820 | 254 | | _ = target.TrySupport(targetDirection, out Vector2d targetSupport, out bool translated); |
| | | 255 | | |
| | 1820 | 256 | | Vector2d expansion = targetDirection * expansionRadius; |
| | 1820 | 257 | | return new PlanarSupportPoint( |
| | 1820 | 258 | | translated |
| | 1820 | 259 | | ? GjkSimplexScale.CreateWorkingDifference(point, target.Center, targetSupport, expansion, workingShift) |
| | 1820 | 260 | | : GjkSimplexScale.CreateWorkingDifference(point, targetSupport, expansion, workingShift)); |
| | | 261 | | } |
| | | 262 | | |
| | | 263 | | private static ClosestPlanarSimplexResult SolveClosestSimplex( |
| | | 264 | | Span<PlanarSupportPoint> simplex, |
| | | 265 | | ref int count) |
| | | 266 | | { |
| | 1653 | 267 | | if (count == 1) |
| | 548 | 268 | | return ClosestPlanarSimplexResult.FromPoint(simplex[0].Point); |
| | | 269 | | |
| | 1105 | 270 | | if (count == 2) |
| | 327 | 271 | | return ReduceSegment(simplex, ref count); |
| | | 272 | | |
| | 778 | 273 | | return ReduceTriangle(simplex, ref count); |
| | | 274 | | } |
| | | 275 | | |
| | | 276 | | private static ClosestPlanarSimplexResult ReduceSegment( |
| | | 277 | | Span<PlanarSupportPoint> simplex, |
| | | 278 | | ref int count) |
| | | 279 | | { |
| | 1513 | 280 | | Vector2d a = simplex[0].Point; |
| | 1513 | 281 | | Vector2d b = simplex[1].Point; |
| | 1513 | 282 | | Fixed64 t = Vector2d.GetClosestPointOnLineSegmentParameter(Vector2d.Zero, a, b); |
| | | 283 | | |
| | 1513 | 284 | | if (t == Fixed64.Zero) |
| | | 285 | | { |
| | 201 | 286 | | count = 1; |
| | 201 | 287 | | return ClosestPlanarSimplexResult.FromPoint(a); |
| | | 288 | | } |
| | | 289 | | |
| | 1312 | 290 | | if (t == Fixed64.One) |
| | | 291 | | { |
| | 319 | 292 | | simplex[0] = simplex[1]; |
| | 319 | 293 | | count = 1; |
| | 319 | 294 | | return ClosestPlanarSimplexResult.FromPoint(b); |
| | | 295 | | } |
| | | 296 | | |
| | 993 | 297 | | count = 2; |
| | 993 | 298 | | return ClosestPlanarSimplexResult.FromSegment(new Vector2d( |
| | 993 | 299 | | FixedMath.Lerp(a.X, b.X, t), FixedMath.Lerp(a.Y, b.Y, t)), a, b); |
| | | 300 | | } |
| | | 301 | | |
| | | 302 | | private static ClosestPlanarSimplexResult ReduceTriangle( |
| | | 303 | | Span<PlanarSupportPoint> simplex, |
| | | 304 | | ref int count) |
| | | 305 | | { |
| | 778 | 306 | | Vector2d a = simplex[0].Point; |
| | 778 | 307 | | Vector2d b = simplex[1].Point; |
| | 778 | 308 | | Vector2d c = simplex[2].Point; |
| | 778 | 309 | | int winding = Vector2d.OrientationSign(a, b, c); |
| | 778 | 310 | | int ab = Vector2d.OrientationSign(a, b, Vector2d.Zero); |
| | 778 | 311 | | int bc = Vector2d.OrientationSign(b, c, Vector2d.Zero); |
| | 778 | 312 | | int ca = Vector2d.OrientationSign(c, a, Vector2d.Zero); |
| | 778 | 313 | | if (winding != 0 && ab != -winding && bc != -winding && ca != -winding) |
| | | 314 | | { |
| | 5 | 315 | | count = 3; |
| | 5 | 316 | | return ClosestPlanarSimplexResult.Intersection; |
| | | 317 | | } |
| | | 318 | | |
| | 773 | 319 | | bool hasBest = false; |
| | 773 | 320 | | int bestFirst = 0; |
| | 773 | 321 | | int bestSecond = 1; |
| | 773 | 322 | | ClosestPlanarSimplexResult best = default; |
| | | 323 | | // Only origin-facing edges can own a closest feature of a proper triangle. |
| | | 324 | | // Collinear simplexes retain every edge because they have no interior side. |
| | 773 | 325 | | if (winding == 0 || ab != winding) |
| | 13 | 326 | | EvaluateTriangleEdge(simplex, 0, 1, ref best, ref hasBest, ref bestFirst, ref bestSecond); |
| | 773 | 327 | | if (winding == 0 || bc != winding) |
| | 575 | 328 | | EvaluateTriangleEdge(simplex, 1, 2, ref best, ref hasBest, ref bestFirst, ref bestSecond); |
| | 773 | 329 | | if (winding == 0 || ca != winding) |
| | 598 | 330 | | EvaluateTriangleEdge(simplex, 2, 0, ref best, ref hasBest, ref bestFirst, ref bestSecond); |
| | | 331 | | |
| | 773 | 332 | | PlanarSupportPoint first = simplex[bestFirst]; |
| | 773 | 333 | | PlanarSupportPoint second = simplex[bestSecond]; |
| | 773 | 334 | | simplex[0] = first; |
| | 773 | 335 | | simplex[1] = second; |
| | 773 | 336 | | count = 2; |
| | 773 | 337 | | return best; |
| | | 338 | | } |
| | | 339 | | |
| | | 340 | | private static void EvaluateTriangleEdge( |
| | | 341 | | Span<PlanarSupportPoint> simplex, |
| | | 342 | | int first, |
| | | 343 | | int second, |
| | | 344 | | ref ClosestPlanarSimplexResult best, |
| | | 345 | | ref bool hasBest, |
| | | 346 | | ref int bestFirst, |
| | | 347 | | ref int bestSecond) |
| | | 348 | | { |
| | 1186 | 349 | | Span<PlanarSupportPoint> edge = stackalloc PlanarSupportPoint[2]; |
| | 1186 | 350 | | edge[0] = simplex[first]; |
| | 1186 | 351 | | edge[1] = simplex[second]; |
| | 1186 | 352 | | int edgeCount = 2; |
| | 1186 | 353 | | ClosestPlanarSimplexResult candidate = ReduceSegment(edge, ref edgeCount); |
| | 1186 | 354 | | if (hasBest && Vector2d.CompareMagnitudeSquared(candidate.Point, best.Point) >= 0) |
| | 205 | 355 | | return; |
| | | 356 | | |
| | 981 | 357 | | best = candidate; |
| | 981 | 358 | | hasBest = true; |
| | 981 | 359 | | bestFirst = first; |
| | 981 | 360 | | bestSecond = second; |
| | 981 | 361 | | } |
| | | 362 | | |
| | | 363 | | private static bool ContainsSupportPoint( |
| | | 364 | | Span<PlanarSupportPoint> simplex, |
| | | 365 | | int count, |
| | | 366 | | Vector2d point) |
| | | 367 | | { |
| | 7414 | 368 | | for (int i = 0; i < count; i++) |
| | | 369 | | { |
| | 2054 | 370 | | if (Vector2d.TryGetDistance(simplex[i].Point, point, out Fixed64 distance) |
| | 2054 | 371 | | && distance <= Fixed64.Epsilon) |
| | 167 | 372 | | return true; |
| | | 373 | | } |
| | | 374 | | |
| | 1653 | 375 | | return false; |
| | | 376 | | } |
| | | 377 | | |
| | | 378 | | private readonly struct ProjectionTarget |
| | | 379 | | { |
| | | 380 | | private readonly LSCapsuleCollider? _capsule; |
| | | 381 | | private readonly LSCylinderCollider? _cylinder; |
| | | 382 | | private readonly LSConeCollider? _cone; |
| | | 383 | | private readonly FixedRange _slabY; |
| | | 384 | | private readonly Vector2d _center; |
| | | 385 | | private readonly Vector3d _axis; |
| | | 386 | | private readonly bool _useTranslatedSupport; |
| | | 387 | | private readonly Fixed64 _supportExtent; |
| | | 388 | | |
| | | 389 | | private ProjectionTarget( |
| | | 390 | | LSCapsuleCollider? capsule, |
| | | 391 | | LSCylinderCollider? cylinder, |
| | | 392 | | LSConeCollider? cone, |
| | | 393 | | Fixed64 slabMinY, |
| | | 394 | | Fixed64 slabMaxY, |
| | | 395 | | Vector2d center, |
| | | 396 | | Vector3d axis) |
| | | 397 | | { |
| | 602 | 398 | | _capsule = capsule; |
| | 602 | 399 | | _cylinder = cylinder; |
| | 602 | 400 | | _cone = cone; |
| | 602 | 401 | | _slabY = new FixedRange(slabMinY, slabMaxY); |
| | 602 | 402 | | _center = center; |
| | 602 | 403 | | _axis = axis; |
| | 602 | 404 | | Fixed64 length = capsule?.AxisLength ?? cylinder?.Height ?? cone!.Height; |
| | 602 | 405 | | Fixed64 radius = capsule?.ScaledRadius ?? cylinder?.ScaledRadius ?? cone!.ScaledRadius; |
| | 602 | 406 | | Fixed64 halfLength = Fixed64.FromRaw((length.m_rawValue >> 1) + (length.m_rawValue & 1)); |
| | 602 | 407 | | Fixed64 axialComponent = FixedMath.Max(axis.X.Abs(), axis.Z.Abs()); |
| | | 408 | | // Support uses a rounded axis, so exact rigid bounds alone cannot select the frame. |
| | | 409 | | // The admitted local dimensions bound this envelope well below the scalar limit. |
| | 602 | 410 | | _ = Fixed64.TryMultiplyAdd(halfLength, axialComponent, radius, out Fixed64 extent); |
| | 602 | 411 | | _supportExtent = extent + Fixed64.MinIncrement; |
| | 602 | 412 | | Vector2d planarExtent = new(_supportExtent, _supportExtent); |
| | 602 | 413 | | _useTranslatedSupport = !Vector2d.TryAdd(center, planarExtent, out _) |
| | 602 | 414 | | || !Vector2d.TrySubtract(center, planarExtent, out _); |
| | 602 | 415 | | } |
| | | 416 | | |
| | 739 | 417 | | public Vector2d Center => _center; |
| | | 418 | | |
| | 381 | 419 | | public Vector2d BoundsMin => new(TargetBoundsMin.X, TargetBoundsMin.Z); |
| | | 420 | | |
| | 381 | 421 | | public Vector2d BoundsMax => new(TargetBoundsMax.X, TargetBoundsMax.Z); |
| | | 422 | | |
| | 762 | 423 | | private Vector3d TargetBoundsMin => _capsule?.BoundsMin ?? _cylinder?.BoundsMin ?? _cone!.BoundsMin; |
| | | 424 | | |
| | 762 | 425 | | private Vector3d TargetBoundsMax => _capsule?.BoundsMax ?? _cylinder?.BoundsMax ?? _cone!.BoundsMax; |
| | | 426 | | |
| | | 427 | | public static ProjectionTarget CreateCapsule(LSCapsuleCollider capsule, Fixed64 slabMinY, Fixed64 slabMaxY) => |
| | 205 | 428 | | new( |
| | 205 | 429 | | capsule, |
| | 205 | 430 | | null, |
| | 205 | 431 | | null, |
| | 205 | 432 | | slabMinY, |
| | 205 | 433 | | slabMaxY, |
| | 205 | 434 | | new Vector2d(capsule.Center.X, capsule.Center.Z), |
| | 205 | 435 | | GetRigidUpAxis(capsule.Rotation)); |
| | | 436 | | |
| | | 437 | | public static ProjectionTarget CreateCylinder(LSCylinderCollider cylinder, Fixed64 slabMinY, Fixed64 slabMaxY) = |
| | 210 | 438 | | new( |
| | 210 | 439 | | null, |
| | 210 | 440 | | cylinder, |
| | 210 | 441 | | null, |
| | 210 | 442 | | slabMinY, |
| | 210 | 443 | | slabMaxY, |
| | 210 | 444 | | new Vector2d(cylinder.Center.X, cylinder.Center.Z), |
| | 210 | 445 | | GetRigidUpAxis(cylinder.Rotation)); |
| | | 446 | | |
| | | 447 | | public static ProjectionTarget CreateCone(LSConeCollider cone, Fixed64 slabMinY, Fixed64 slabMaxY) => |
| | 187 | 448 | | new( |
| | 187 | 449 | | null, |
| | 187 | 450 | | null, |
| | 187 | 451 | | cone, |
| | 187 | 452 | | slabMinY, |
| | 187 | 453 | | slabMaxY, |
| | 187 | 454 | | new Vector2d(cone.Center.X, cone.Center.Z), |
| | 187 | 455 | | GetRigidUpAxis(cone.Rotation)); |
| | | 456 | | |
| | | 457 | | public int SelectWorkingShift(Vector2d point, Fixed64 radius) |
| | | 458 | | { |
| | 548 | 459 | | if (!_useTranslatedSupport) |
| | 381 | 460 | | return GjkSimplexScale.SelectThreeTermShift(point, BoundsMin, BoundsMax, radius); |
| | 167 | 461 | | Vector2d extent = new(_supportExtent, _supportExtent); |
| | 167 | 462 | | return GjkSimplexScale.SelectFourTermShift(point, _center, -extent, extent, radius); |
| | | 463 | | } |
| | | 464 | | |
| | | 465 | | public bool TrySupport(Vector2d unitDirection, out Vector2d support, out bool translated) |
| | | 466 | | { |
| | | 467 | | // Target support and circle expansion consume the same normalized direction. |
| | 2419 | 468 | | Vector3d center = _capsule?.Center ?? _cylinder?.Center ?? _cone!.Center; |
| | | 469 | | // Select the frame once; retrying every unavailable support repeats the expensive solve. |
| | 2419 | 470 | | translated = _useTranslatedSupport; |
| | 2419 | 471 | | if (translated) |
| | 572 | 472 | | center = new Vector3d(Fixed64.Zero, center.Y, Fixed64.Zero); |
| | | 473 | | |
| | 2419 | 474 | | if (_capsule != null) |
| | | 475 | | { |
| | 1061 | 476 | | return FixedSlabProjection.TryGetCapsuleSupport( |
| | 1061 | 477 | | center, |
| | 1061 | 478 | | _axis, |
| | 1061 | 479 | | _capsule.AxisLength, |
| | 1061 | 480 | | _capsule.ScaledRadius, |
| | 1061 | 481 | | _slabY, |
| | 1061 | 482 | | unitDirection, |
| | 1061 | 483 | | out support); |
| | | 484 | | } |
| | | 485 | | |
| | 1358 | 486 | | if (_cylinder != null) |
| | | 487 | | { |
| | 785 | 488 | | return FixedSlabProjection.TryGetCylinderSupport( |
| | 785 | 489 | | center, |
| | 785 | 490 | | _axis, |
| | 785 | 491 | | _cylinder.Height, |
| | 785 | 492 | | _cylinder.ScaledRadius, |
| | 785 | 493 | | _slabY, |
| | 785 | 494 | | unitDirection, |
| | 785 | 495 | | out support); |
| | | 496 | | } |
| | | 497 | | |
| | 573 | 498 | | return FixedSlabProjection.TryGetConeSupport( |
| | 573 | 499 | | center, |
| | 573 | 500 | | _axis, |
| | 573 | 501 | | _cone!.Height, |
| | 573 | 502 | | _cone.ScaledRadius, |
| | 573 | 503 | | _slabY, |
| | 573 | 504 | | unitDirection, |
| | 573 | 505 | | out support); |
| | | 506 | | } |
| | | 507 | | |
| | | 508 | | public bool TryGetPlanarCircle( |
| | | 509 | | Vector2d rightSupport, |
| | | 510 | | bool translated, |
| | | 511 | | out Vector2d center, |
| | | 512 | | out Fixed64 radius) |
| | | 513 | | { |
| | 584 | 514 | | if ((_axis.X != Fixed64.Zero) | (_axis.Z != Fixed64.Zero)) |
| | | 515 | | { |
| | 327 | 516 | | center = default; |
| | 327 | 517 | | radius = default; |
| | 327 | 518 | | return false; |
| | | 519 | | } |
| | | 520 | | |
| | 257 | 521 | | center = _center; |
| | 257 | 522 | | radius = translated ? rightSupport.X : rightSupport.X - _center.X; |
| | 257 | 523 | | return true; |
| | | 524 | | } |
| | | 525 | | |
| | | 526 | | } |
| | | 527 | | |
| | | 528 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 529 | | private static Vector3d GetRigidUpAxis(FixedQuaternion rotation) => |
| | 602 | 530 | | (rotation * Vector3d.Up).Normalized; |
| | | 531 | | |
| | | 532 | | } |