| | | 1 | | //======================================================================= |
| | | 2 | | // QueryDetection2D.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 System; |
| | | 13 | | |
| | | 14 | | namespace Gravitas.Queries; |
| | | 15 | | |
| | | 16 | | /// <summary> |
| | | 17 | | /// Deterministic pure 2D shape checks used by query services. |
| | | 18 | | /// </summary> |
| | | 19 | | internal static partial class QueryDetection2D |
| | | 20 | | { |
| | | 21 | | internal static bool TryOverlapCircle( |
| | | 22 | | Vector2d center, |
| | | 23 | | Fixed64 radius, |
| | | 24 | | LSCollider2D collider, |
| | | 25 | | out Physics2DHit hit) |
| | | 26 | | { |
| | 22510 | 27 | | SwiftThrowHelper.ThrowIfArgument(radius < Fixed64.Zero, nameof(radius), "2D query radius cannot be negative."); |
| | | 28 | | |
| | 22510 | 29 | | if (collider is LSCompoundCollider2D compound) |
| | 4 | 30 | | return TryOverlapCircleCompound(center, radius, compound, out hit); |
| | 22506 | 31 | | if (collider is LSCircleCollider2D circle) |
| | 21743 | 32 | | return TryOverlapCircleCenteredCapsule( |
| | 21743 | 33 | | center, |
| | 21743 | 34 | | radius, |
| | 21743 | 35 | | circle.Center, |
| | 21743 | 36 | | circle.Rotation, |
| | 21743 | 37 | | Fixed64.Zero, |
| | 21743 | 38 | | circle.ScaledRadius, |
| | 21743 | 39 | | circle, |
| | 21743 | 40 | | out hit); |
| | 763 | 41 | | if (collider is LSCapsuleCollider2D capsule) |
| | 373 | 42 | | return TryOverlapCircleCenteredCapsule( |
| | 373 | 43 | | center, |
| | 373 | 44 | | radius, |
| | 373 | 45 | | capsule.Center, |
| | 373 | 46 | | capsule.Rotation, |
| | 373 | 47 | | capsule.AxisLength, |
| | 373 | 48 | | capsule.ScaledRadius, |
| | 373 | 49 | | capsule, |
| | 373 | 50 | | out hit); |
| | 390 | 51 | | if (collider is not IConvexVertexSource2D) |
| | | 52 | | { |
| | 2 | 53 | | hit = default; |
| | 2 | 54 | | return false; |
| | | 55 | | } |
| | | 56 | | |
| | 388 | 57 | | Span<Vector2d> scratch = stackalloc Vector2d[4]; |
| | 388 | 58 | | ReadOnlySpan<Vector2d> vertexOffsets = |
| | 388 | 59 | | GetConvexVertexOffsets(collider, scratch); |
| | 388 | 60 | | if (!FixedConvex2dRelations.TryGetCircleContact( |
| | 388 | 61 | | center, |
| | 388 | 62 | | Fixed64.Zero, |
| | 388 | 63 | | radius, |
| | 388 | 64 | | collider.Center, |
| | 388 | 65 | | collider.ConvexRotation, |
| | 388 | 66 | | vertexOffsets, |
| | 388 | 67 | | out _, |
| | 388 | 68 | | out FixedPointAnchor2d contactAnchor, |
| | 388 | 69 | | out Vector2d circleToColliderNormal, |
| | 388 | 70 | | out Fixed64 depth, |
| | 388 | 71 | | out _)) |
| | | 72 | | { |
| | 212 | 73 | | hit = default; |
| | 212 | 74 | | return false; |
| | | 75 | | } |
| | | 76 | | |
| | 176 | 77 | | bool containsCenter = FixedConvex2dRelations.ContainsPoint( |
| | 176 | 78 | | center, |
| | 176 | 79 | | collider.Center, |
| | 176 | 80 | | collider.ConvexRotation, |
| | 176 | 81 | | vertexOffsets); |
| | 176 | 82 | | Fixed64 distance = Fixed64.Zero; |
| | 176 | 83 | | Vector2d normal = -circleToColliderNormal; |
| | 176 | 84 | | if (!containsCenter) |
| | 168 | 85 | | distance = radius - depth; |
| | | 86 | | |
| | 176 | 87 | | hit = new Physics2DHit( |
| | 176 | 88 | | collider, |
| | 176 | 89 | | new ContactAnchor2D(contactAnchor), |
| | 176 | 90 | | normal, |
| | 176 | 91 | | distance); |
| | 176 | 92 | | return true; |
| | | 93 | | } |
| | | 94 | | |
| | | 95 | | private static bool TryOverlapCircleCenteredCapsule( |
| | | 96 | | Vector2d queryCenter, |
| | | 97 | | Fixed64 queryRadius, |
| | | 98 | | Vector2d targetCenter, |
| | | 99 | | Fixed64 targetRotation, |
| | | 100 | | Fixed64 targetAxisLength, |
| | | 101 | | Fixed64 targetRadius, |
| | | 102 | | LSCollider2D target, |
| | | 103 | | out Physics2DHit hit) |
| | | 104 | | { |
| | 22116 | 105 | | if (!FixedSegment2d.TryGetCenteredCapsulesContact( |
| | 22116 | 106 | | queryCenter, |
| | 22116 | 107 | | Fixed64.Zero, |
| | 22116 | 108 | | Fixed64.Zero, |
| | 22116 | 109 | | queryRadius, |
| | 22116 | 110 | | targetCenter, |
| | 22116 | 111 | | targetRotation, |
| | 22116 | 112 | | targetAxisLength, |
| | 22116 | 113 | | targetRadius, |
| | 22116 | 114 | | ResolveQueryFallbackNormal(queryCenter, targetCenter), |
| | 22116 | 115 | | out FixedContactAnchors2d contact)) |
| | | 116 | | { |
| | 7365 | 117 | | hit = default; |
| | 7365 | 118 | | return false; |
| | | 119 | | } |
| | | 120 | | |
| | 14751 | 121 | | Fixed64 distance = FixedSegment2d.GetDistanceToCenteredCapsule( |
| | 14751 | 122 | | queryCenter, |
| | 14751 | 123 | | targetCenter, |
| | 14751 | 124 | | targetRotation, |
| | 14751 | 125 | | targetAxisLength, |
| | 14751 | 126 | | targetRadius); |
| | | 127 | | |
| | 14751 | 128 | | hit = new Physics2DHit( |
| | 14751 | 129 | | target, |
| | 14751 | 130 | | new ContactAnchor2D(contact.SecondAnchor), |
| | 14751 | 131 | | queryCenter == targetCenter |
| | 14751 | 132 | | ? ResolveQueryFallbackNormal(queryCenter, targetCenter) |
| | 14751 | 133 | | : -contact.Normal, |
| | 14751 | 134 | | distance); |
| | 14751 | 135 | | return true; |
| | | 136 | | } |
| | | 137 | | |
| | | 138 | | internal static bool TryOverlapPolygon( |
| | | 139 | | ReadOnlySpan<Vector2d> vertices, |
| | | 140 | | Vector2d center, |
| | | 141 | | LSCollider2D collider, |
| | | 142 | | out Physics2DHit hit) |
| | | 143 | | { |
| | 7801 | 144 | | return TryOverlapConvexArea(center, vertices, collider, out hit); |
| | | 145 | | } |
| | | 146 | | |
| | | 147 | | internal static void ValidateAabbSize(Vector2d size) |
| | | 148 | | { |
| | 1687 | 149 | | SwiftThrowHelper.ThrowIfArgument( |
| | 1687 | 150 | | size.X <= Fixed64.Zero || size.Y <= Fixed64.Zero, |
| | 1687 | 151 | | nameof(size), |
| | 1687 | 152 | | "2D AABB query size components must be greater than zero."); |
| | 1685 | 153 | | } |
| | | 154 | | |
| | | 155 | | internal static void ValidateConvexQueryPolygon(ReadOnlySpan<Vector2d> vertices) |
| | | 156 | | { |
| | 17 | 157 | | SwiftThrowHelper.ThrowIfArgument(vertices.Length < 3, nameof(vertices), "2D polygon query must contain at least |
| | 17 | 158 | | SwiftThrowHelper.ThrowIfArgument( |
| | 17 | 159 | | !FixedConvex2dRelations.IsStrictlyConvex(vertices), |
| | 17 | 160 | | nameof(vertices), |
| | 17 | 161 | | "2D polygon query vertices must form a strictly convex boundary."); |
| | 17 | 162 | | } |
| | | 163 | | |
| | | 164 | | internal static Vector2d CalculateAverageCenter(ReadOnlySpan<Vector2d> vertices) => |
| | 13 | 165 | | Vector2d.GetAverage(vertices); |
| | | 166 | | |
| | | 167 | | internal static bool TryRaycast(Vector2d start, Vector2d end, LSCollider2D collider, out Physics2DHit hit) |
| | | 168 | | { |
| | 14208 | 169 | | if (!Vector2d.TrySubtract(end, start, out Vector2d segment)) |
| | | 170 | | { |
| | 1 | 171 | | hit = default; |
| | 1 | 172 | | return false; |
| | | 173 | | } |
| | | 174 | | |
| | 14207 | 175 | | if (segment == Vector2d.Zero || !SegmentBoundsOverlap(start, end, collider)) |
| | | 176 | | { |
| | 31 | 177 | | hit = default; |
| | 31 | 178 | | return false; |
| | | 179 | | } |
| | | 180 | | |
| | 14176 | 181 | | if (!Vector2d.TryGetMagnitude(segment, out Fixed64 segmentLength)) |
| | | 182 | | { |
| | 1 | 183 | | hit = default; |
| | 1 | 184 | | return false; |
| | | 185 | | } |
| | | 186 | | |
| | 14175 | 187 | | if (collider is LSCompoundCollider2D compound) |
| | 4 | 188 | | return TryRaycastCompound(start, end, compound, out hit); |
| | | 189 | | |
| | 14171 | 190 | | if (ContainsPointExact(collider, start)) |
| | | 191 | | { |
| | 15 | 192 | | hit = new Physics2DHit( |
| | 15 | 193 | | collider, |
| | 15 | 194 | | start, |
| | 15 | 195 | | ResolveQueryFallbackNormal(start, collider.Center), |
| | 15 | 196 | | Fixed64.Zero); |
| | 15 | 197 | | return true; |
| | | 198 | | } |
| | | 199 | | |
| | 14156 | 200 | | if (collider is LSCircleCollider2D circle) |
| | 13595 | 201 | | return TryRaycastCircle(start, end, segmentLength, circle, out hit); |
| | 561 | 202 | | if (collider is LSCapsuleCollider2D capsule) |
| | 271 | 203 | | return TryRaycastCapsule(start, end, segmentLength, capsule, out hit); |
| | 290 | 204 | | if (collider is not LSAABBoxCollider2D |
| | 290 | 205 | | && collider is not LSPolygonCollider2D) |
| | | 206 | | { |
| | 4 | 207 | | hit = default; |
| | 4 | 208 | | return false; |
| | | 209 | | } |
| | | 210 | | |
| | 286 | 211 | | Vector2d direction = segment.Normalized; |
| | 286 | 212 | | return TryRaycastConvex(start, direction, segmentLength, collider, out hit); |
| | | 213 | | } |
| | | 214 | | |
| | | 215 | | internal static bool TrySweepCircle( |
| | | 216 | | Vector2d start, |
| | | 217 | | Vector2d end, |
| | | 218 | | Fixed64 radius, |
| | | 219 | | LSCollider2D collider, |
| | | 220 | | out Physics2DHit hit) |
| | | 221 | | { |
| | 8274 | 222 | | if (!Vector2d.TrySubtract(end, start, out Vector2d segment)) |
| | | 223 | | { |
| | 1 | 224 | | hit = default; |
| | 1 | 225 | | return false; |
| | | 226 | | } |
| | | 227 | | |
| | 8273 | 228 | | if (segment == Vector2d.Zero || !SweepBoundsOverlap(start, end, radius, collider)) |
| | | 229 | | { |
| | 76 | 230 | | hit = default; |
| | 76 | 231 | | return false; |
| | | 232 | | } |
| | | 233 | | |
| | 8197 | 234 | | if (!Vector2d.TryGetMagnitude(segment, out Fixed64 segmentLength) |
| | 8197 | 235 | | || segmentLength <= Fixed64.Epsilon) |
| | | 236 | | { |
| | 3 | 237 | | hit = default; |
| | 3 | 238 | | return false; |
| | | 239 | | } |
| | | 240 | | |
| | 8194 | 241 | | if (collider is LSCompoundCollider2D compound) |
| | 7 | 242 | | return TrySweepCircleCompound(start, end, radius, compound, out hit); |
| | | 243 | | |
| | 8187 | 244 | | if (TryOverlapCircle(start, radius, collider, out Physics2DHit overlapHit)) |
| | | 245 | | { |
| | 613 | 246 | | hit = new Physics2DHit(collider, start, overlapHit.Normal, Fixed64.Zero); |
| | 613 | 247 | | return true; |
| | | 248 | | } |
| | | 249 | | |
| | 7574 | 250 | | if (collider is LSCircleCollider2D circle) |
| | 7094 | 251 | | return TrySweepCircleCircle(start, end, segmentLength, radius, circle, out hit); |
| | 480 | 252 | | if (collider is LSCapsuleCollider2D capsule) |
| | 268 | 253 | | return TrySweepCircleCapsule(start, end, segmentLength, radius, capsule, out hit); |
| | 212 | 254 | | if (collider is not LSAABBoxCollider2D |
| | 212 | 255 | | && collider is not LSPolygonCollider2D) |
| | | 256 | | { |
| | 1 | 257 | | hit = default; |
| | 1 | 258 | | return false; |
| | | 259 | | } |
| | | 260 | | |
| | 211 | 261 | | Vector2d direction = segment.Normalized; |
| | 211 | 262 | | return TrySweepCircleConvex(start, direction, segmentLength, radius, collider, out hit); |
| | | 263 | | } |
| | | 264 | | |
| | | 265 | | internal static bool TrySweepMoverShape( |
| | | 266 | | LSCollider2D mover, |
| | | 267 | | Vector2d displacement, |
| | | 268 | | LSCollider2D target, |
| | | 269 | | out Physics2DHit hit) |
| | | 270 | | { |
| | 477 | 271 | | SwiftThrowHelper.ThrowIfNull(mover, nameof(mover)); |
| | 477 | 272 | | SwiftThrowHelper.ThrowIfNull(target, nameof(target)); |
| | | 273 | | |
| | 477 | 274 | | if (!Vector2d.TryGetMagnitude(displacement, out Fixed64 displacementLength) |
| | 477 | 275 | | || displacementLength <= Fixed64.Epsilon) |
| | | 276 | | { |
| | 2 | 277 | | hit = default; |
| | 2 | 278 | | return false; |
| | | 279 | | } |
| | | 280 | | |
| | 475 | 281 | | if (mover is LSCompoundCollider2D moverCompound) |
| | 4 | 282 | | return TrySweepMoverCompound(moverCompound, displacement, target, out hit); |
| | | 283 | | |
| | 471 | 284 | | if (target is LSCompoundCollider2D targetCompound) |
| | 3 | 285 | | return TrySweepMoverAgainstCompound(mover, displacement, targetCompound, out hit); |
| | | 286 | | |
| | 468 | 287 | | if (mover is LSCircleCollider2D circle) |
| | | 288 | | { |
| | 182 | 289 | | if (!Vector2d.TryAdd( |
| | 182 | 290 | | circle.Center, |
| | 182 | 291 | | displacement, |
| | 182 | 292 | | out Vector2d end)) |
| | | 293 | | { |
| | 1 | 294 | | hit = default; |
| | 1 | 295 | | return false; |
| | | 296 | | } |
| | | 297 | | |
| | 181 | 298 | | return TrySweepCircle( |
| | 181 | 299 | | circle.Center, |
| | 181 | 300 | | end, |
| | 181 | 301 | | circle.ScaledRadius, |
| | 181 | 302 | | target, |
| | 181 | 303 | | out hit); |
| | | 304 | | } |
| | 286 | 305 | | if (mover is LSCapsuleCollider2D moverCapsule) |
| | 229 | 306 | | return TrySweepCapsuleMover( |
| | 229 | 307 | | moverCapsule, |
| | 229 | 308 | | displacement, |
| | 229 | 309 | | displacementLength, |
| | 229 | 310 | | target, |
| | 229 | 311 | | out hit); |
| | | 312 | | |
| | 57 | 313 | | if (target is LSCircleCollider2D targetCircle) |
| | 40 | 314 | | return TrySweepConvexMoverAgainstCircle(mover, displacement, targetCircle, out hit); |
| | 17 | 315 | | if (target is LSCapsuleCollider2D targetCapsule) |
| | 3 | 316 | | return TrySweepConvexMoverAgainstCapsule( |
| | 3 | 317 | | mover, |
| | 3 | 318 | | displacement, |
| | 3 | 319 | | displacementLength, |
| | 3 | 320 | | targetCapsule, |
| | 3 | 321 | | out hit); |
| | | 322 | | |
| | 14 | 323 | | return TrySweepConvexMoverAgainstConvex( |
| | 14 | 324 | | mover, |
| | 14 | 325 | | displacement, |
| | 14 | 326 | | displacementLength, |
| | 14 | 327 | | target, |
| | 14 | 328 | | out hit); |
| | | 329 | | } |
| | | 330 | | |
| | | 331 | | private static bool TryRaycastCircle( |
| | | 332 | | Vector2d start, |
| | | 333 | | Vector2d end, |
| | | 334 | | Fixed64 segmentLength, |
| | | 335 | | LSCircleCollider2D circle, |
| | | 336 | | out Physics2DHit hit) |
| | | 337 | | { |
| | 13595 | 338 | | var query = new FixedSegment2d(start, end); |
| | 13595 | 339 | | if (!query.TryGetCircleIntersectionDistanceInterval( |
| | 13595 | 340 | | new FixedBoundCircle(circle.Center, circle.ScaledRadius), |
| | 13595 | 341 | | segmentLength, |
| | 13595 | 342 | | out Fixed64 distance, |
| | 13595 | 343 | | out _)) |
| | | 344 | | { |
| | 5 | 345 | | hit = default; |
| | 5 | 346 | | return false; |
| | | 347 | | } |
| | | 348 | | |
| | 13590 | 349 | | Vector2d point = query.GetPointAtDistance(distance, segmentLength); |
| | 13590 | 350 | | Vector2d normal = Vector2d.GetDirection(circle.Center, point); |
| | 13590 | 351 | | hit = new Physics2DHit( |
| | 13590 | 352 | | circle, |
| | 13590 | 353 | | new ContactAnchor2D( |
| | 13590 | 354 | | circle.Center, |
| | 13590 | 355 | | normal * circle.ScaledRadius), |
| | 13590 | 356 | | normal, |
| | 13590 | 357 | | distance); |
| | 13590 | 358 | | return true; |
| | | 359 | | } |
| | | 360 | | |
| | | 361 | | private static bool TrySweepCircleCircle( |
| | | 362 | | Vector2d start, |
| | | 363 | | Vector2d end, |
| | | 364 | | Fixed64 segmentLength, |
| | | 365 | | Fixed64 radius, |
| | | 366 | | LSCircleCollider2D circle, |
| | | 367 | | out Physics2DHit hit) |
| | | 368 | | { |
| | 7094 | 369 | | var query = new FixedSegment2d(start, end); |
| | 7094 | 370 | | if (!query.TryGetCircleIntersectionDistanceInterval( |
| | 7094 | 371 | | new FixedBoundCircle(circle.Center, circle.ScaledRadius), |
| | 7094 | 372 | | radius, |
| | 7094 | 373 | | segmentLength, |
| | 7094 | 374 | | out Fixed64 distance, |
| | 7094 | 375 | | out _, |
| | 7094 | 376 | | out _, |
| | 7094 | 377 | | out _)) |
| | | 378 | | { |
| | 13 | 379 | | hit = default; |
| | 13 | 380 | | return false; |
| | | 381 | | } |
| | | 382 | | |
| | 7081 | 383 | | Vector2d sweptCenter = query.GetPointAtDistance(distance, segmentLength); |
| | 7081 | 384 | | Vector2d normal = Vector2d.GetDirection(circle.Center, sweptCenter); |
| | 7081 | 385 | | hit = new Physics2DHit( |
| | 7081 | 386 | | circle, |
| | 7081 | 387 | | new ContactAnchor2D( |
| | 7081 | 388 | | circle.Center, |
| | 7081 | 389 | | normal * circle.ScaledRadius), |
| | 7081 | 390 | | normal, |
| | 7081 | 391 | | distance); |
| | 7081 | 392 | | return true; |
| | | 393 | | } |
| | | 394 | | |
| | | 395 | | private static bool TryRaycastCapsule( |
| | | 396 | | Vector2d start, |
| | | 397 | | Vector2d end, |
| | | 398 | | Fixed64 segmentLength, |
| | | 399 | | LSCapsuleCollider2D capsule, |
| | | 400 | | out Physics2DHit hit) |
| | | 401 | | { |
| | 271 | 402 | | var query = new FixedSegment2d(start, end); |
| | 271 | 403 | | if (!query.TryGetCapsuleIntersectionDistanceInterval( |
| | 271 | 404 | | capsule.Center, |
| | 271 | 405 | | capsule.Rotation, |
| | 271 | 406 | | capsule.AxisLength, |
| | 271 | 407 | | capsule.ScaledRadius, |
| | 271 | 408 | | Fixed64.Zero, |
| | 271 | 409 | | segmentLength, |
| | 271 | 410 | | out Fixed64 distance, |
| | 271 | 411 | | out _, |
| | 271 | 412 | | out _, |
| | 271 | 413 | | out _)) |
| | | 414 | | { |
| | 3 | 415 | | hit = default; |
| | 3 | 416 | | return false; |
| | | 417 | | } |
| | | 418 | | |
| | 268 | 419 | | Vector2d pointOnRay = query.GetPointAtDistance(distance, segmentLength); |
| | 268 | 420 | | Vector2d normal = capsule.GetNormalFromCenteredAxis(pointOnRay); |
| | 268 | 421 | | hit = new Physics2DHit( |
| | 268 | 422 | | capsule, |
| | 268 | 423 | | ContactAnchor2D.FromWorldPoint(pointOnRay), |
| | 268 | 424 | | normal, |
| | 268 | 425 | | distance); |
| | 268 | 426 | | return true; |
| | | 427 | | } |
| | | 428 | | |
| | | 429 | | private static bool TrySweepCircleCapsule( |
| | | 430 | | Vector2d start, |
| | | 431 | | Vector2d end, |
| | | 432 | | Fixed64 segmentLength, |
| | | 433 | | Fixed64 radius, |
| | | 434 | | LSCapsuleCollider2D capsule, |
| | | 435 | | out Physics2DHit hit) |
| | | 436 | | { |
| | 268 | 437 | | var query = new FixedSegment2d(start, end); |
| | 268 | 438 | | if (!query.TryGetCapsuleIntersectionDistanceInterval( |
| | 268 | 439 | | capsule.Center, |
| | 268 | 440 | | capsule.Rotation, |
| | 268 | 441 | | capsule.AxisLength, |
| | 268 | 442 | | capsule.ScaledRadius, |
| | 268 | 443 | | radius, |
| | 268 | 444 | | segmentLength, |
| | 268 | 445 | | out Fixed64 distance, |
| | 268 | 446 | | out _, |
| | 268 | 447 | | out _, |
| | 268 | 448 | | out _)) |
| | | 449 | | { |
| | 2 | 450 | | hit = default; |
| | 2 | 451 | | return false; |
| | | 452 | | } |
| | | 453 | | |
| | 266 | 454 | | Vector2d sweptCenter = query.GetPointAtDistance(distance, segmentLength); |
| | 266 | 455 | | Vector2d normal = capsule.GetNormalFromCenteredAxis(sweptCenter); |
| | 266 | 456 | | bool hasPoint = TryOffsetPoint( |
| | 266 | 457 | | sweptCenter, |
| | 266 | 458 | | -normal, |
| | 266 | 459 | | radius, |
| | 266 | 460 | | out Vector2d point); |
| | | 461 | | |
| | 266 | 462 | | hit = new Physics2DHit( |
| | 266 | 463 | | capsule, |
| | 266 | 464 | | hasPoint |
| | 266 | 465 | | ? ContactAnchor2D.FromWorldPoint(point) |
| | 266 | 466 | | : new ContactAnchor2D( |
| | 266 | 467 | | sweptCenter, |
| | 266 | 468 | | -normal * radius), |
| | 266 | 469 | | normal, |
| | 266 | 470 | | distance); |
| | 266 | 471 | | return true; |
| | | 472 | | } |
| | | 473 | | |
| | | 474 | | private static bool TryOverlapCircleCompound( |
| | | 475 | | Vector2d center, |
| | | 476 | | Fixed64 radius, |
| | | 477 | | LSCompoundCollider2D compound, |
| | | 478 | | out Physics2DHit hit) |
| | | 479 | | { |
| | 4 | 480 | | bool found = false; |
| | 4 | 481 | | Physics2DHit best = default; |
| | | 482 | | |
| | 24 | 483 | | for (int i = 0; i < compound.PartCount; i++) |
| | | 484 | | { |
| | 8 | 485 | | LSCollider2D part = compound.GetPartCollider(i); |
| | 8 | 486 | | if (!TryOverlapCircle(center, radius, part, out Physics2DHit candidate)) |
| | | 487 | | continue; |
| | | 488 | | |
| | 6 | 489 | | TryKeepEarlierHit(candidate, ref found, ref best); |
| | | 490 | | } |
| | | 491 | | |
| | 4 | 492 | | if (!found) |
| | | 493 | | { |
| | 1 | 494 | | hit = default; |
| | 1 | 495 | | return false; |
| | | 496 | | } |
| | | 497 | | |
| | 3 | 498 | | hit = new Physics2DHit(compound, best.Anchor, best.Normal, best.Distance); |
| | 3 | 499 | | return true; |
| | | 500 | | } |
| | | 501 | | |
| | | 502 | | private static bool TryOverlapConvexArea( |
| | | 503 | | Vector2d center, |
| | | 504 | | ReadOnlySpan<Vector2d> vertices, |
| | | 505 | | LSCollider2D collider, |
| | | 506 | | out Physics2DHit hit) |
| | | 507 | | { |
| | 7805 | 508 | | if (collider is LSCompoundCollider2D compound) |
| | 2 | 509 | | return TryOverlapConvexAreaCompound(center, vertices, compound, out hit); |
| | | 510 | | |
| | 7803 | 511 | | return collider switch |
| | 7803 | 512 | | { |
| | 7803 | 513 | | LSCircleCollider2D circle => |
| | 7506 | 514 | | TryOverlapConvexAreaCircle(center, vertices, circle, out hit), |
| | 7803 | 515 | | LSCapsuleCollider2D capsule => |
| | 136 | 516 | | TryOverlapConvexAreaCapsule(center, vertices, capsule, out hit), |
| | 161 | 517 | | _ => TryOverlapConvexAreaConvex(center, vertices, collider, out hit) |
| | 7803 | 518 | | }; |
| | | 519 | | } |
| | | 520 | | |
| | | 521 | | private static bool TryOverlapConvexAreaCompound( |
| | | 522 | | Vector2d center, |
| | | 523 | | ReadOnlySpan<Vector2d> vertices, |
| | | 524 | | LSCompoundCollider2D compound, |
| | | 525 | | out Physics2DHit hit) |
| | | 526 | | { |
| | 2 | 527 | | bool found = false; |
| | 2 | 528 | | Physics2DHit best = default; |
| | 12 | 529 | | for (int i = 0; i < compound.PartCount; i++) |
| | | 530 | | { |
| | 4 | 531 | | LSCollider2D part = compound.GetPartCollider(i); |
| | 4 | 532 | | if (!TryOverlapConvexArea(center, vertices, part, out Physics2DHit candidate)) |
| | | 533 | | continue; |
| | | 534 | | |
| | 2 | 535 | | TryKeepEarlierHit(candidate, ref found, ref best); |
| | | 536 | | } |
| | | 537 | | |
| | 2 | 538 | | if (!found) |
| | | 539 | | { |
| | 1 | 540 | | hit = default; |
| | 1 | 541 | | return false; |
| | | 542 | | } |
| | | 543 | | |
| | 1 | 544 | | hit = new Physics2DHit(compound, best.Anchor, best.Normal, best.Distance); |
| | 1 | 545 | | return true; |
| | | 546 | | } |
| | | 547 | | |
| | | 548 | | private static bool TryOverlapConvexAreaCircle( |
| | | 549 | | Vector2d center, |
| | | 550 | | ReadOnlySpan<Vector2d> vertices, |
| | | 551 | | LSCircleCollider2D circle, |
| | | 552 | | out Physics2DHit hit) |
| | | 553 | | { |
| | 7506 | 554 | | if (!FixedConvex2dRelations.TryGetCircleContact( |
| | 7506 | 555 | | circle.Center, |
| | 7506 | 556 | | circle.Rotation, |
| | 7506 | 557 | | circle.ScaledRadius, |
| | 7506 | 558 | | Vector2d.Zero, |
| | 7506 | 559 | | Fixed64.Zero, |
| | 7506 | 560 | | vertices, |
| | 7506 | 561 | | out _, |
| | 7506 | 562 | | out _, |
| | 7506 | 563 | | out _, |
| | 7506 | 564 | | out _, |
| | 7506 | 565 | | out _)) |
| | | 566 | | { |
| | 8 | 567 | | hit = default; |
| | 8 | 568 | | return false; |
| | | 569 | | } |
| | | 570 | | |
| | 7498 | 571 | | return TryBuildCenteredCapsuleAreaHit( |
| | 7498 | 572 | | center, |
| | 7498 | 573 | | circle.Center, |
| | 7498 | 574 | | circle.Rotation, |
| | 7498 | 575 | | Fixed64.Zero, |
| | 7498 | 576 | | circle.ScaledRadius, |
| | 7498 | 577 | | circle, |
| | 7498 | 578 | | out hit); |
| | | 579 | | } |
| | | 580 | | |
| | | 581 | | private static bool TryOverlapConvexAreaCapsule( |
| | | 582 | | Vector2d center, |
| | | 583 | | ReadOnlySpan<Vector2d> vertices, |
| | | 584 | | LSCapsuleCollider2D capsule, |
| | | 585 | | out Physics2DHit hit) |
| | | 586 | | { |
| | 136 | 587 | | if (!FixedSegment2d.TryGetCenteredCapsuleConvexMinimumTranslation( |
| | 136 | 588 | | capsule.Center, |
| | 136 | 589 | | capsule.Rotation, |
| | 136 | 590 | | capsule.AxisLength, |
| | 136 | 591 | | capsule.ScaledRadius, |
| | 136 | 592 | | Vector2d.Zero, |
| | 136 | 593 | | vertices, |
| | 136 | 594 | | out _, |
| | 136 | 595 | | out _)) |
| | | 596 | | { |
| | 3 | 597 | | hit = default; |
| | 3 | 598 | | return false; |
| | | 599 | | } |
| | | 600 | | |
| | 133 | 601 | | return TryBuildCenteredCapsuleAreaHit( |
| | 133 | 602 | | center, |
| | 133 | 603 | | capsule.Center, |
| | 133 | 604 | | capsule.Rotation, |
| | 133 | 605 | | capsule.AxisLength, |
| | 133 | 606 | | capsule.ScaledRadius, |
| | 133 | 607 | | capsule, |
| | 133 | 608 | | out hit); |
| | | 609 | | } |
| | | 610 | | |
| | | 611 | | private static bool TryOverlapConvexAreaConvex( |
| | | 612 | | Vector2d center, |
| | | 613 | | ReadOnlySpan<Vector2d> vertices, |
| | | 614 | | LSCollider2D collider, |
| | | 615 | | out Physics2DHit hit) |
| | | 616 | | { |
| | 161 | 617 | | Span<Vector2d> targetScratch = stackalloc Vector2d[4]; |
| | 161 | 618 | | ReadOnlySpan<Vector2d> targetOffsets = |
| | 161 | 619 | | GetConvexVertexOffsets(collider, targetScratch); |
| | 161 | 620 | | Span<FixedPointAnchor2d> queryContacts = |
| | 161 | 621 | | stackalloc FixedPointAnchor2d[2]; |
| | 161 | 622 | | Span<FixedPointAnchor2d> targetContacts = |
| | 161 | 623 | | stackalloc FixedPointAnchor2d[2]; |
| | 161 | 624 | | if (!FixedConvex2dRelations.TryGetConvexContacts( |
| | 161 | 625 | | Vector2d.Zero, |
| | 161 | 626 | | Fixed64.Zero, |
| | 161 | 627 | | vertices, |
| | 161 | 628 | | collider.Center, |
| | 161 | 629 | | collider.ConvexRotation, |
| | 161 | 630 | | targetOffsets, |
| | 161 | 631 | | queryContacts, |
| | 161 | 632 | | targetContacts, |
| | 161 | 633 | | out _, |
| | 161 | 634 | | out _, |
| | 161 | 635 | | out _, |
| | 161 | 636 | | out _)) |
| | | 637 | | { |
| | 9 | 638 | | hit = default; |
| | 9 | 639 | | return false; |
| | | 640 | | } |
| | | 641 | | |
| | 152 | 642 | | return TryBuildConvexAreaHit( |
| | 152 | 643 | | center, |
| | 152 | 644 | | collider, |
| | 152 | 645 | | targetOffsets, |
| | 152 | 646 | | out hit); |
| | | 647 | | } |
| | | 648 | | |
| | | 649 | | private static bool TryBuildCenteredCapsuleAreaHit( |
| | | 650 | | Vector2d queryCenter, |
| | | 651 | | Vector2d targetCenter, |
| | | 652 | | Fixed64 targetRotation, |
| | | 653 | | Fixed64 targetAxisLength, |
| | | 654 | | Fixed64 targetRadius, |
| | | 655 | | LSCollider2D target, |
| | | 656 | | out Physics2DHit hit) |
| | | 657 | | { |
| | 7631 | 658 | | bool containsCenter = FixedSegment2d.ContainsPointInCenteredCapsule( |
| | 7631 | 659 | | queryCenter, |
| | 7631 | 660 | | targetCenter, |
| | 7631 | 661 | | targetRotation, |
| | 7631 | 662 | | targetAxisLength, |
| | 7631 | 663 | | targetRadius, |
| | 7631 | 664 | | Fixed64.Zero); |
| | 7631 | 665 | | if (containsCenter) |
| | | 666 | | { |
| | 843 | 667 | | hit = new Physics2DHit( |
| | 843 | 668 | | target, |
| | 843 | 669 | | ContactAnchor2D.FromWorldPoint(queryCenter), |
| | 843 | 670 | | ResolveQueryFallbackNormal(queryCenter, targetCenter), |
| | 843 | 671 | | Fixed64.Zero); |
| | 843 | 672 | | return true; |
| | | 673 | | } |
| | | 674 | | |
| | 6788 | 675 | | Vector2d normal = FixedSegment2d.GetDirectionFromCenteredAxis( |
| | 6788 | 676 | | queryCenter, |
| | 6788 | 677 | | targetCenter, |
| | 6788 | 678 | | targetRotation, |
| | 6788 | 679 | | targetAxisLength); |
| | | 680 | | |
| | 6788 | 681 | | hit = new Physics2DHit( |
| | 6788 | 682 | | target, |
| | 6788 | 683 | | new ContactAnchor2D( |
| | 6788 | 684 | | GetCenteredCapsuleSurfaceAnchor( |
| | 6788 | 685 | | queryCenter, |
| | 6788 | 686 | | targetCenter, |
| | 6788 | 687 | | targetRotation, |
| | 6788 | 688 | | targetAxisLength, |
| | 6788 | 689 | | targetRadius, |
| | 6788 | 690 | | normal)), |
| | 6788 | 691 | | normal, |
| | 6788 | 692 | | FixedSegment2d.GetDistanceToCenteredCapsule( |
| | 6788 | 693 | | queryCenter, |
| | 6788 | 694 | | targetCenter, |
| | 6788 | 695 | | targetRotation, |
| | 6788 | 696 | | targetAxisLength, |
| | 6788 | 697 | | targetRadius)); |
| | 6788 | 698 | | return true; |
| | | 699 | | } |
| | | 700 | | |
| | | 701 | | private static FixedPointAnchor2d GetCenteredCapsuleSurfaceAnchor( |
| | | 702 | | Vector2d point, |
| | | 703 | | Vector2d center, |
| | | 704 | | Fixed64 rotation, |
| | | 705 | | Fixed64 axisLength, |
| | | 706 | | Fixed64 radius, |
| | | 707 | | Vector2d worldNormal) |
| | | 708 | | { |
| | | 709 | | // A unit rotation preserves the admitted normalized direction. |
| | 6788 | 710 | | _ = Vector2d.TryRotate( |
| | 6788 | 711 | | worldNormal, |
| | 6788 | 712 | | -rotation, |
| | 6788 | 713 | | out Vector2d localNormal); |
| | 6788 | 714 | | return FixedSegment2d.GetSurfaceAnchorOnCenteredCapsule( |
| | 6788 | 715 | | point, |
| | 6788 | 716 | | center, |
| | 6788 | 717 | | rotation, |
| | 6788 | 718 | | Vector2d.Forward, |
| | 6788 | 719 | | axisLength, |
| | 6788 | 720 | | radius, |
| | 6788 | 721 | | localNormal.Normalized); |
| | | 722 | | } |
| | | 723 | | |
| | | 724 | | private static bool TryBuildConvexAreaHit( |
| | | 725 | | Vector2d queryCenter, |
| | | 726 | | LSCollider2D target, |
| | | 727 | | ReadOnlySpan<Vector2d> targetVertexOffsets, |
| | | 728 | | out Physics2DHit hit) |
| | | 729 | | { |
| | 152 | 730 | | if (FixedConvex2dRelations.ContainsPoint( |
| | 152 | 731 | | queryCenter, |
| | 152 | 732 | | target.Center, |
| | 152 | 733 | | target.ConvexRotation, |
| | 152 | 734 | | targetVertexOffsets)) |
| | | 735 | | { |
| | 16 | 736 | | hit = new Physics2DHit( |
| | 16 | 737 | | target, |
| | 16 | 738 | | ContactAnchor2D.FromWorldPoint(queryCenter), |
| | 16 | 739 | | ResolveQueryFallbackNormal(queryCenter, target.Center), |
| | 16 | 740 | | Fixed64.Zero); |
| | 16 | 741 | | return true; |
| | | 742 | | } |
| | | 743 | | |
| | 136 | 744 | | FixedPointAnchor2d targetAnchor = |
| | 136 | 745 | | FixedConvex2dRelations.GetClosestPointAnchor( |
| | 136 | 746 | | queryCenter, |
| | 136 | 747 | | target.Center, |
| | 136 | 748 | | target.ConvexRotation, |
| | 136 | 749 | | targetVertexOffsets); |
| | | 750 | | |
| | 136 | 751 | | var anchor = new ContactAnchor2D(targetAnchor); |
| | 136 | 752 | | if (!anchor.TryGetOffsetFrom(queryCenter, out Vector2d centerToTarget) |
| | 136 | 753 | | || !Vector2d.TryGetMagnitude(centerToTarget, out Fixed64 distance)) |
| | | 754 | | { |
| | 2 | 755 | | hit = default; |
| | 2 | 756 | | return false; |
| | | 757 | | } |
| | | 758 | | |
| | 134 | 759 | | Vector2d normal = -centerToTarget / distance; |
| | 134 | 760 | | hit = new Physics2DHit(target, anchor, normal, distance); |
| | 134 | 761 | | return true; |
| | | 762 | | } |
| | | 763 | | |
| | | 764 | | private static bool TryRaycastConvex( |
| | | 765 | | Vector2d start, |
| | | 766 | | Vector2d direction, |
| | | 767 | | Fixed64 segmentLength, |
| | | 768 | | LSCollider2D collider, |
| | | 769 | | out Physics2DHit hit) |
| | | 770 | | { |
| | 286 | 771 | | Span<Vector2d> scratch = stackalloc Vector2d[4]; |
| | 286 | 772 | | ReadOnlySpan<Vector2d> vertexOffsets = |
| | 286 | 773 | | GetConvexVertexOffsets(collider, scratch); |
| | 286 | 774 | | if (!FixedConvex2dRelations.TryGetSegmentFirstIntersectionDistance( |
| | 286 | 775 | | start, |
| | 286 | 776 | | direction, |
| | 286 | 777 | | segmentLength, |
| | 286 | 778 | | collider.Center, |
| | 286 | 779 | | collider.ConvexRotation, |
| | 286 | 780 | | vertexOffsets, |
| | 286 | 781 | | out Fixed64 distance, |
| | 286 | 782 | | out Vector2d normal, |
| | 286 | 783 | | out FixedPointAnchor2d contactAnchor)) |
| | | 784 | | { |
| | 1 | 785 | | hit = default; |
| | 1 | 786 | | return false; |
| | | 787 | | } |
| | | 788 | | |
| | 285 | 789 | | hit = new Physics2DHit( |
| | 285 | 790 | | collider, |
| | 285 | 791 | | new ContactAnchor2D(contactAnchor), |
| | 285 | 792 | | normal, |
| | 285 | 793 | | distance); |
| | 285 | 794 | | return true; |
| | | 795 | | } |
| | | 796 | | |
| | | 797 | | private static bool TrySweepCircleConvex( |
| | | 798 | | Vector2d start, |
| | | 799 | | Vector2d direction, |
| | | 800 | | Fixed64 segmentLength, |
| | | 801 | | Fixed64 radius, |
| | | 802 | | LSCollider2D collider, |
| | | 803 | | out Physics2DHit hit) |
| | | 804 | | { |
| | 211 | 805 | | Span<Vector2d> scratch = stackalloc Vector2d[4]; |
| | 211 | 806 | | ReadOnlySpan<Vector2d> vertexOffsets = |
| | 211 | 807 | | GetConvexVertexOffsets(collider, scratch); |
| | 211 | 808 | | if (!FixedConvex2dRelations.TryGetSweptCircleFirstDistance( |
| | 211 | 809 | | start, |
| | 211 | 810 | | radius, |
| | 211 | 811 | | direction, |
| | 211 | 812 | | segmentLength, |
| | 211 | 813 | | collider.Center, |
| | 211 | 814 | | collider.ConvexRotation, |
| | 211 | 815 | | vertexOffsets, |
| | 211 | 816 | | out Fixed64 distance, |
| | 211 | 817 | | out Vector2d normal, |
| | 211 | 818 | | out FixedPointAnchor2d contactAnchor)) |
| | | 819 | | { |
| | 2 | 820 | | hit = default; |
| | 2 | 821 | | return false; |
| | | 822 | | } |
| | | 823 | | |
| | 209 | 824 | | hit = new Physics2DHit( |
| | 209 | 825 | | collider, |
| | 209 | 826 | | new ContactAnchor2D(contactAnchor), |
| | 209 | 827 | | normal, |
| | 209 | 828 | | distance); |
| | 209 | 829 | | return true; |
| | | 830 | | } |
| | | 831 | | |
| | | 832 | | private static bool TrySweepConvexMoverAgainstCircle( |
| | | 833 | | LSCollider2D mover, |
| | | 834 | | Vector2d displacement, |
| | | 835 | | LSCircleCollider2D target, |
| | | 836 | | out Physics2DHit hit) |
| | | 837 | | { |
| | 40 | 838 | | if (CollisionDetection2D.TryCollide(mover, target, out Contact2D overlap)) |
| | | 839 | | { |
| | 2 | 840 | | Vector2d normal = -overlap.Normal; |
| | 2 | 841 | | hit = new Physics2DHit( |
| | 2 | 842 | | target, |
| | 2 | 843 | | overlap.AnchorB, |
| | 2 | 844 | | normal, |
| | 2 | 845 | | Fixed64.Zero); |
| | 2 | 846 | | return true; |
| | | 847 | | } |
| | | 848 | | |
| | 38 | 849 | | if (!TrySweepCircle( |
| | 38 | 850 | | target.Center, |
| | 38 | 851 | | target.Center - displacement, |
| | 38 | 852 | | target.ScaledRadius, |
| | 38 | 853 | | mover, |
| | 38 | 854 | | out Physics2DHit reverseHit)) |
| | | 855 | | { |
| | 35 | 856 | | hit = default; |
| | 35 | 857 | | return false; |
| | | 858 | | } |
| | | 859 | | |
| | 3 | 860 | | Vector2d hitNormal = -reverseHit.Normal; |
| | 3 | 861 | | hit = new Physics2DHit( |
| | 3 | 862 | | target, |
| | 3 | 863 | | new ContactAnchor2D( |
| | 3 | 864 | | target.Center, |
| | 3 | 865 | | hitNormal * target.ScaledRadius), |
| | 3 | 866 | | hitNormal, |
| | 3 | 867 | | reverseHit.Distance); |
| | 3 | 868 | | return true; |
| | | 869 | | } |
| | | 870 | | |
| | | 871 | | } |