| | | 1 | | //======================================================================= |
| | | 2 | | // ConvexSweepHitPolicy.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 Gravitas.Colliders; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.Queries; |
| | | 12 | | |
| | | 13 | | /// <summary> |
| | | 14 | | /// Selects the reported sweep normal from available contact evidence and orients it against travel. |
| | | 15 | | /// </summary> |
| | | 16 | | internal static class ConvexSweepHitPolicy |
| | | 17 | | { |
| | | 18 | | internal static Vector3d ResolveHitNormal( |
| | | 19 | | LSCollider targetCollider, |
| | | 20 | | Vector3d point, |
| | | 21 | | Vector3d resultNormal, |
| | | 22 | | Vector3d fallbackNormal, |
| | | 23 | | Vector3d sweepDirection, |
| | | 24 | | Vector3d planarNormal, |
| | | 25 | | bool hasRefinedSurfaceNormal = false, |
| | | 26 | | bool hasMaterializedPoint = true) |
| | | 27 | | { |
| | 3783 | 28 | | Vector3d surfaceNormal = hasRefinedSurfaceNormal |
| | 3783 | 29 | | ? targetCollider.GetNormalAtPoint(point) |
| | 3783 | 30 | | : Vector3d.Zero; |
| | 3783 | 31 | | Vector3d normal = surfaceNormal.MagnitudeSquared > Fixed64.Epsilon |
| | 3783 | 32 | | ? surfaceNormal |
| | 3783 | 33 | | : planarNormal.MagnitudeSquared > Fixed64.Epsilon |
| | 3783 | 34 | | ? planarNormal |
| | 3783 | 35 | | : resultNormal.MagnitudeSquared > Fixed64.Epsilon |
| | 3783 | 36 | | ? resultNormal |
| | 3783 | 37 | | : fallbackNormal.MagnitudeSquared > Fixed64.Epsilon |
| | 3783 | 38 | | ? fallbackNormal |
| | 3783 | 39 | | : hasMaterializedPoint |
| | 3783 | 40 | | ? targetCollider.GetNormalAtPoint(point) |
| | 3783 | 41 | | : Vector3d.Zero; |
| | | 42 | | |
| | 3783 | 43 | | if (normal.MagnitudeSquared <= Fixed64.Epsilon) |
| | | 44 | | { |
| | 7 | 45 | | return sweepDirection.MagnitudeSquared > Fixed64.Epsilon |
| | 7 | 46 | | ? -sweepDirection.Normalized |
| | 7 | 47 | | : Vector3d.Zero; |
| | | 48 | | } |
| | | 49 | | |
| | 3776 | 50 | | normal = normal.Normalized; |
| | 3776 | 51 | | return Vector3d.CompareProjection( |
| | 3776 | 52 | | normal, |
| | 3776 | 53 | | Vector3d.Zero, |
| | 3776 | 54 | | sweepDirection) > 0 |
| | 3776 | 55 | | ? -normal |
| | 3776 | 56 | | : normal; |
| | | 57 | | } |
| | | 58 | | } |