| | | 1 | | //======================================================================= |
| | | 2 | | // GridNavigationPortal.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System.Runtime.CompilerServices; |
| | | 9 | | using System.Runtime.InteropServices; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | |
| | | 12 | | namespace GridForge.Grids.Topology; |
| | | 13 | | |
| | | 14 | | /// <summary> |
| | | 15 | | /// Stores one exact, agent-independent navigation crossing compiled from two cell prisms. |
| | | 16 | | /// </summary> |
| | | 17 | | /// <remarks> |
| | | 18 | | /// The value retains no live grid state. Profile resolution uses only the stored face geometry, |
| | | 19 | | /// direction, and conservative fixed-point capacities. |
| | | 20 | | /// </remarks> |
| | | 21 | | [StructLayout(LayoutKind.Sequential)] |
| | | 22 | | public readonly struct GridNavigationPortal |
| | | 23 | | { |
| | | 24 | | /// <summary>The stable retained value size used by exact byte-accounted storage.</summary> |
| | | 25 | | public const int SizeInBytes = 104; |
| | | 26 | | |
| | | 27 | | /// <summary>The orientation of the shared positive-area face.</summary> |
| | | 28 | | public VoxelContactFaceKind FaceKind { get; } |
| | | 29 | | |
| | | 30 | | /// <summary>The exact target-center displacement from the source center.</summary> |
| | | 31 | | public Vector3d SourceToTarget { get; } |
| | | 32 | | |
| | | 33 | | /// <summary>The canonical foot crossing on the shared face.</summary> |
| | | 34 | | public Vector3d CanonicalFacePoint { get; } |
| | | 35 | | |
| | | 36 | | /// <summary>The greatest conservatively representable horizontal body radius at the crossing.</summary> |
| | | 37 | | public Fixed64 MaximumHorizontalRadius { get; } |
| | | 38 | | |
| | | 39 | | /// <summary>The greatest body height supported by both directed sides of the crossing.</summary> |
| | | 40 | | public Fixed64 MaximumBodyHeight { get; } |
| | | 41 | | |
| | | 42 | | /// <summary>The first exact XZ endpoint certifying a compiled vertical face.</summary> |
| | | 43 | | public Vector2d VerticalFaceSegmentStart { get; } |
| | | 44 | | |
| | | 45 | | /// <summary>The second exact XZ endpoint certifying a compiled vertical face.</summary> |
| | | 46 | | public Vector2d VerticalFaceSegmentEnd { get; } |
| | | 47 | | |
| | | 48 | | /// <summary>Whether this value contains a compiled positive-area navigation face.</summary> |
| | | 49 | | public bool IsValid => |
| | 15139 | 50 | | ((FaceKind == VoxelContactFaceKind.Vertical |
| | 15139 | 51 | | && VerticalFaceSegmentStart != VerticalFaceSegmentEnd) |
| | 15139 | 52 | | || FaceKind == VoxelContactFaceKind.Horizontal) |
| | 15139 | 53 | | && MaximumHorizontalRadius >= Fixed64.Zero |
| | 15139 | 54 | | && MaximumBodyHeight > Fixed64.Zero; |
| | | 55 | | |
| | | 56 | | internal GridNavigationPortal( |
| | | 57 | | VoxelContactFaceKind faceKind, |
| | | 58 | | Vector3d sourceToTarget, |
| | | 59 | | Vector3d canonicalFacePoint, |
| | | 60 | | Fixed64 maximumHorizontalRadius, |
| | | 61 | | Fixed64 maximumBodyHeight, |
| | | 62 | | Vector2d verticalFaceSegmentStart, |
| | | 63 | | Vector2d verticalFaceSegmentEnd) |
| | | 64 | | { |
| | 2443 | 65 | | FaceKind = faceKind; |
| | 2443 | 66 | | SourceToTarget = sourceToTarget; |
| | 2443 | 67 | | CanonicalFacePoint = canonicalFacePoint; |
| | 2443 | 68 | | MaximumHorizontalRadius = maximumHorizontalRadius; |
| | 2443 | 69 | | MaximumBodyHeight = maximumBodyHeight; |
| | 2443 | 70 | | VerticalFaceSegmentStart = verticalFaceSegmentStart; |
| | 2443 | 71 | | VerticalFaceSegmentEnd = verticalFaceSegmentEnd; |
| | 2443 | 72 | | } |
| | | 73 | | |
| | | 74 | | /// <summary> |
| | | 75 | | /// Attempts to rigidly translate the canonical face point without fixed-point saturation. |
| | | 76 | | /// </summary> |
| | | 77 | | /// <param name="offset">The exact translation applied to the canonical face point.</param> |
| | | 78 | | /// <param name="translated">The translated portal, or <see langword="default"/> on failure.</param> |
| | | 79 | | /// <returns> |
| | | 80 | | /// <see langword="true"/> when this portal is valid and the translated face point is representable; |
| | | 81 | | /// otherwise, <see langword="false"/>. |
| | | 82 | | /// </returns> |
| | | 83 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 84 | | public bool TryTranslate(Vector3d offset, out GridNavigationPortal translated) |
| | | 85 | | { |
| | 13 | 86 | | translated = default; |
| | 13 | 87 | | if (!IsValid |
| | 13 | 88 | | || !Vector3d.TryAdd(CanonicalFacePoint, offset, out Vector3d translatedFacePoint)) |
| | | 89 | | { |
| | 3 | 90 | | return false; |
| | | 91 | | } |
| | | 92 | | |
| | 10 | 93 | | Vector2d translatedSegmentStart = default; |
| | 10 | 94 | | Vector2d translatedSegmentEnd = default; |
| | 10 | 95 | | if (FaceKind == VoxelContactFaceKind.Vertical) |
| | | 96 | | { |
| | 9 | 97 | | var horizontalOffset = new Vector2d(offset.X, offset.Z); |
| | 9 | 98 | | if (!Vector2d.TryAdd( |
| | 9 | 99 | | VerticalFaceSegmentStart, |
| | 9 | 100 | | horizontalOffset, |
| | 9 | 101 | | out translatedSegmentStart) |
| | 9 | 102 | | || !Vector2d.TryAdd( |
| | 9 | 103 | | VerticalFaceSegmentEnd, |
| | 9 | 104 | | horizontalOffset, |
| | 9 | 105 | | out translatedSegmentEnd)) |
| | | 106 | | { |
| | 2 | 107 | | return false; |
| | | 108 | | } |
| | | 109 | | } |
| | | 110 | | |
| | 8 | 111 | | translated = new GridNavigationPortal( |
| | 8 | 112 | | FaceKind, |
| | 8 | 113 | | SourceToTarget, |
| | 8 | 114 | | translatedFacePoint, |
| | 8 | 115 | | MaximumHorizontalRadius, |
| | 8 | 116 | | MaximumBodyHeight, |
| | 8 | 117 | | translatedSegmentStart, |
| | 8 | 118 | | translatedSegmentEnd); |
| | 8 | 119 | | return true; |
| | | 120 | | } |
| | | 121 | | |
| | | 122 | | /// <summary> |
| | | 123 | | /// Attempts to fit a fixed-point body profile and resolve its directed source and target foot anchors. |
| | | 124 | | /// </summary> |
| | | 125 | | /// <param name="horizontalRadius">The required nonnegative horizontal body radius.</param> |
| | | 126 | | /// <param name="bodyHeight">The required positive body height.</param> |
| | | 127 | | /// <param name="sourceFootAnchor">The last canonical foot anchor on the source side.</param> |
| | | 128 | | /// <param name="targetFootAnchor">The first canonical foot anchor on the target side.</param> |
| | | 129 | | /// <returns><see langword="true"/> when the profile fits and both anchors are representable.</returns> |
| | | 130 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 131 | | public bool TryResolveProfile( |
| | | 132 | | Fixed64 horizontalRadius, |
| | | 133 | | Fixed64 bodyHeight, |
| | | 134 | | out Vector3d sourceFootAnchor, |
| | | 135 | | out Vector3d targetFootAnchor) |
| | | 136 | | { |
| | 2351 | 137 | | SwiftThrowHelper.ThrowIfArgument( |
| | 2351 | 138 | | horizontalRadius < Fixed64.Zero, |
| | 2351 | 139 | | nameof(horizontalRadius), |
| | 2351 | 140 | | "Horizontal radius must be nonnegative."); |
| | 2350 | 141 | | SwiftThrowHelper.ThrowIfArgument( |
| | 2350 | 142 | | bodyHeight <= Fixed64.Zero, |
| | 2350 | 143 | | nameof(bodyHeight), |
| | 2350 | 144 | | "Body height must be positive."); |
| | | 145 | | |
| | 2349 | 146 | | sourceFootAnchor = default; |
| | 2349 | 147 | | targetFootAnchor = default; |
| | 2349 | 148 | | if (!IsValid |
| | 2349 | 149 | | || horizontalRadius > MaximumHorizontalRadius |
| | 2349 | 150 | | || bodyHeight > MaximumBodyHeight) |
| | | 151 | | { |
| | 10 | 152 | | return false; |
| | | 153 | | } |
| | | 154 | | |
| | 2339 | 155 | | if (FaceKind == VoxelContactFaceKind.Vertical) |
| | | 156 | | { |
| | 2322 | 157 | | sourceFootAnchor = CanonicalFacePoint; |
| | 2322 | 158 | | targetFootAnchor = CanonicalFacePoint; |
| | 2322 | 159 | | return true; |
| | | 160 | | } |
| | | 161 | | |
| | 17 | 162 | | if (!Fixed64.TrySubtract(CanonicalFacePoint.Y, bodyHeight, out Fixed64 lowerFootY)) |
| | 1 | 163 | | return false; |
| | | 164 | | |
| | 16 | 165 | | Vector3d lowerFoot = new Vector3d(CanonicalFacePoint.X, lowerFootY, CanonicalFacePoint.Z); |
| | 16 | 166 | | if (SourceToTarget.Y > Fixed64.Zero) |
| | | 167 | | { |
| | 12 | 168 | | sourceFootAnchor = lowerFoot; |
| | 12 | 169 | | targetFootAnchor = CanonicalFacePoint; |
| | 12 | 170 | | return true; |
| | | 171 | | } |
| | | 172 | | |
| | 4 | 173 | | if (SourceToTarget.Y < Fixed64.Zero) |
| | | 174 | | { |
| | 3 | 175 | | sourceFootAnchor = CanonicalFacePoint; |
| | 3 | 176 | | targetFootAnchor = lowerFoot; |
| | 3 | 177 | | return true; |
| | | 178 | | } |
| | | 179 | | |
| | 1 | 180 | | sourceFootAnchor = default; |
| | 1 | 181 | | targetFootAnchor = default; |
| | 1 | 182 | | return false; |
| | | 183 | | } |
| | | 184 | | } |