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