| | | 1 | | //======================================================================= |
| | | 2 | | // TransitState.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 FixedMathSharp; |
| | | 9 | | |
| | | 10 | | namespace Trailblazer.Navigation.Motor; |
| | | 11 | | |
| | | 12 | | /// <summary> |
| | | 13 | | /// Represents the traversal state for a object while using a <see cref="NavMotor"/> |
| | | 14 | | /// and provides synchronization with <see cref="TrekCondition"/>. |
| | | 15 | | /// </summary> |
| | | 16 | | public class TransitState |
| | | 17 | | { |
| | | 18 | | #region State Properties |
| | | 19 | | |
| | | 20 | | /// <summary> |
| | | 21 | | /// The current traversal medium. |
| | | 22 | | /// </summary> |
| | | 23 | | public TraversalMedium Medium { get; private set; } |
| | | 24 | | |
| | | 25 | | /// <summary> |
| | | 26 | | /// The height of the surface the entity is interacting with. |
| | | 27 | | /// </summary> |
| | | 28 | | public Fixed64 SurfaceLevel { get; private set; } |
| | | 29 | | |
| | | 30 | | /// <summary> |
| | | 31 | | /// The ceiling height above the entity. |
| | | 32 | | /// </summary> |
| | | 33 | | public Fixed64 CeilingLevel { get; private set; } = Fixed64.MaxValue; |
| | | 34 | | |
| | | 35 | | /// <summary> |
| | | 36 | | /// The ground state, if applicable. |
| | | 37 | | /// </summary> |
| | | 38 | | public GroundCondition? GroundState { get; private set; } |
| | | 39 | | |
| | | 40 | | /// <summary> |
| | | 41 | | /// The normal of the ground surface. |
| | | 42 | | /// </summary> |
| | | 43 | | public Vector3d SurfaceNormal { get; private set; } |
| | | 44 | | |
| | | 45 | | /// <summary> |
| | | 46 | | /// Gets the angle of the slope at the current position, in fixed-point degrees. |
| | | 47 | | /// </summary> |
| | | 48 | | public Fixed64 SlopeAngle { get; private set; } |
| | | 49 | | |
| | | 50 | | /// <summary> |
| | | 51 | | /// The previous traversal state (for comparison and transition detection). |
| | | 52 | | /// </summary> |
| | | 53 | | public TrekCondition? PreviousState { get; private set; } |
| | | 54 | | |
| | | 55 | | /// <summary> |
| | | 56 | | /// The previous traversal medium, treating a missing previous sample as <see cref="TraversalMedium.Unknown"/>. |
| | | 57 | | /// </summary> |
| | 6591 | 58 | | public TraversalMedium PreviousMedium => PreviousState?.Medium ?? TraversalMedium.Unknown; |
| | | 59 | | |
| | | 60 | | #endregion |
| | | 61 | | |
| | | 62 | | #region Constructors |
| | | 63 | | |
| | | 64 | | /// <summary> |
| | | 65 | | /// Initializes a new instance of the TransitState class using the specified trek condition. |
| | | 66 | | /// </summary> |
| | | 67 | | /// <param name="condition">The trek condition to use for initializing the state. Cannot be null.</param> |
| | 1078 | 68 | | public TransitState(TrekCondition condition) => Update(condition, null); |
| | | 69 | | |
| | | 70 | | /// <summary> |
| | | 71 | | /// Initializes a new instance of the TransitState class with the specified current and previous trek conditions. |
| | | 72 | | /// </summary> |
| | | 73 | | /// <param name="condition">The current trek condition to represent. Cannot be null.</param> |
| | | 74 | | /// <param name="previous">The previous trek condition, or null if there is no previous condition.</param> |
| | 2 | 75 | | public TransitState(TrekCondition condition, TrekCondition? previous) => Update(condition, previous); |
| | | 76 | | |
| | | 77 | | #endregion |
| | | 78 | | |
| | | 79 | | #region State Update |
| | | 80 | | |
| | | 81 | | /// <summary> |
| | | 82 | | /// Updates the traversal state and retains the previous state for transition tracking. |
| | | 83 | | /// </summary> |
| | | 84 | | public void Update(TrekCondition condition, TrekCondition? previous) |
| | | 85 | | { |
| | 3050 | 86 | | PreviousState = previous; |
| | 3050 | 87 | | Medium = condition.Medium; |
| | 3050 | 88 | | SurfaceLevel = condition.SurfaceLevel; |
| | 3050 | 89 | | GroundState = condition.GroundState; |
| | | 90 | | |
| | 3050 | 91 | | if (Medium == TraversalMedium.Solid) |
| | | 92 | | { |
| | 848 | 93 | | SurfaceNormal = GroundState?.GroundNormal ?? Vector3d.Zero; |
| | 848 | 94 | | SlopeAngle = Vector3d.Angle(Vector3d.Up, SurfaceNormal); |
| | | 95 | | } |
| | | 96 | | else |
| | | 97 | | { |
| | 2202 | 98 | | SurfaceNormal = Vector3d.Up; |
| | 2202 | 99 | | SlopeAngle = Fixed64.Zero; |
| | | 100 | | } |
| | | 101 | | |
| | 3050 | 102 | | CeilingLevel = condition.CeilingLevel; |
| | 3050 | 103 | | } |
| | | 104 | | |
| | | 105 | | #endregion |
| | | 106 | | |
| | | 107 | | #region Utility Methods |
| | | 108 | | |
| | | 109 | | /// <summary> |
| | | 110 | | /// Calculates the signed angle of the current surface slope relative to the specified movement direction. |
| | | 111 | | /// </summary> |
| | | 112 | | /// <param name="moveDirection"> |
| | | 113 | | /// The movement direction vector used to determine the orientation relative to the slope. |
| | | 114 | | /// If this value is <see cref="Vector3d.Zero"/>, the method treats the movement as oriented downhill. |
| | | 115 | | /// </param> |
| | | 116 | | /// <returns> |
| | | 117 | | /// The signed slope angle in degrees. |
| | | 118 | | /// A negative value indicates movement downhill, and a positive value indicates movement uphill. |
| | | 119 | | /// Returns zero if the surface is flat. |
| | | 120 | | /// </returns> |
| | | 121 | | public Fixed64 GetSignedSlopeAngle(Vector3d moveDirection) |
| | | 122 | | { |
| | 2184 | 123 | | if (SlopeAngle == Fixed64.Zero) |
| | 2100 | 124 | | return Fixed64.Zero; |
| | | 125 | | |
| | | 126 | | // Treat downhill as gravity-biased when idle |
| | 84 | 127 | | if (moveDirection == Vector3d.Zero) |
| | 30 | 128 | | moveDirection = Vector3d.Backward; |
| | | 129 | | |
| | 84 | 130 | | bool isDownhill = Vector3d.Dot(moveDirection.Normalized, SurfaceNormal) < Fixed64.Zero; |
| | 84 | 131 | | return isDownhill ? -SlopeAngle : SlopeAngle; |
| | | 132 | | } |
| | | 133 | | |
| | | 134 | | /// <summary> |
| | | 135 | | /// Returns a new TraversalCondition instance reflecting the current state. |
| | | 136 | | /// </summary> |
| | | 137 | | public TrekCondition ToTrekCondition() |
| | | 138 | | { |
| | 2362 | 139 | | return new TrekCondition() |
| | 2362 | 140 | | { |
| | 2362 | 141 | | Medium = Medium, |
| | 2362 | 142 | | SurfaceLevel = SurfaceLevel, |
| | 2362 | 143 | | GroundState = GroundState, |
| | 2362 | 144 | | CeilingLevel = CeilingLevel |
| | 2362 | 145 | | }; |
| | | 146 | | } |
| | | 147 | | |
| | | 148 | | #endregion |
| | | 149 | | } |