| | | 1 | | //======================================================================= |
| | | 2 | | // ClimbLocomotion.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 Chronicler; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using Trailblazer.Support; |
| | | 11 | | |
| | | 12 | | namespace Trailblazer.Navigation.Motor; |
| | | 13 | | |
| | | 14 | | /// <summary> |
| | | 15 | | /// Stores climb locomotion configuration and runtime attachment state. |
| | | 16 | | /// </summary> |
| | | 17 | | public class ClimbLocomotion : ILocomotion |
| | | 18 | | { |
| | | 19 | | /// <summary> |
| | | 20 | | /// Default maximum speed while climbing. |
| | | 21 | | /// </summary> |
| | 1 | 22 | | public static readonly Fixed64 DefaultMaxClimbSpeed = Fixed64.One; |
| | | 23 | | |
| | | 24 | | /// <summary> |
| | | 25 | | /// Default maximum acceleration while climbing. |
| | | 26 | | /// </summary> |
| | 1 | 27 | | public static readonly Fixed64 DefaultMaxClimbAcceleration = (Fixed64)8; |
| | | 28 | | |
| | | 29 | | /// <summary> |
| | | 30 | | /// Default gravity compensation applied while attached to a climb affordance. |
| | | 31 | | /// </summary> |
| | 1 | 32 | | public static readonly Fixed64 DefaultGravityCompensationWhileClimbing = Fixed64.One; |
| | | 33 | | |
| | | 34 | | /// <summary> |
| | | 35 | | /// Default positional tolerance used when validating attachment continuity without a stable affordance id. |
| | | 36 | | /// </summary> |
| | 1 | 37 | | public static readonly Fixed64 DefaultClimbStartTolerance = Fixed64.Half; |
| | | 38 | | |
| | 1157 | 39 | | private bool _isEnabled = true; |
| | | 40 | | |
| | | 41 | | /// <inheritdoc cref="ILocomotion.IsEnabled"/> |
| | | 42 | | public bool IsEnabled |
| | | 43 | | { |
| | 4409 | 44 | | get => _isEnabled; |
| | | 45 | | set |
| | | 46 | | { |
| | 3 | 47 | | _isEnabled = value; |
| | 3 | 48 | | if (!_isEnabled) |
| | 2 | 49 | | this.ClearTransientState(); |
| | 3 | 50 | | } |
| | | 51 | | } |
| | | 52 | | |
| | | 53 | | /// <summary> |
| | | 54 | | /// Determines whether this locomotion may engage with climb affordances. |
| | | 55 | | /// </summary> |
| | 1157 | 56 | | public bool CanClimb = true; |
| | | 57 | | |
| | | 58 | | /// <summary> |
| | | 59 | | /// Maximum climb speed while attached. |
| | | 60 | | /// </summary> |
| | 1157 | 61 | | public Fixed64 MaxClimbSpeed = DefaultMaxClimbSpeed; |
| | | 62 | | |
| | | 63 | | /// <summary> |
| | | 64 | | /// Maximum acceleration while attached. |
| | | 65 | | /// </summary> |
| | 1157 | 66 | | public Fixed64 MaxClimbAcceleration = DefaultMaxClimbAcceleration; |
| | | 67 | | |
| | | 68 | | /// <summary> |
| | | 69 | | /// Amount of gravity canceled while actively climbing. |
| | | 70 | | /// </summary> |
| | 1157 | 71 | | public Fixed64 GravityCompensationWhileClimbing = DefaultGravityCompensationWhileClimbing; |
| | | 72 | | |
| | | 73 | | /// <summary> |
| | | 74 | | /// Positional tolerance used for initial attach and attachment continuity checks. |
| | | 75 | | /// </summary> |
| | 1157 | 76 | | public Fixed64 ClimbStartTolerance = DefaultClimbStartTolerance; |
| | | 77 | | |
| | | 78 | | /// <summary> |
| | | 79 | | /// Whether lateral traverse across a climb surface is allowed. |
| | | 80 | | /// </summary> |
| | 1157 | 81 | | public bool AllowLateralTraverse = true; |
| | | 82 | | |
| | | 83 | | /// <summary> |
| | | 84 | | /// Whether active mantle should query an optional host validator each frame. |
| | | 85 | | /// </summary> |
| | | 86 | | public bool ValidateActiveMantleWithHost; |
| | | 87 | | |
| | | 88 | | /// <summary> |
| | | 89 | | /// Optional host-owned resolver that supplies deterministic climb affordance snapshots. |
| | | 90 | | /// </summary> |
| | | 91 | | public IClimbAffordanceResolver? ClimbResolver; |
| | | 92 | | |
| | | 93 | | /// <summary> |
| | | 94 | | /// Whether active climb movement is currently attached to an affordance. |
| | | 95 | | /// </summary> |
| | | 96 | | [Transient] |
| | | 97 | | public bool IsClimbing { get; set; } |
| | | 98 | | |
| | | 99 | | /// <summary> |
| | | 100 | | /// Whether the locomotion is currently in a mantle/top-out phase. |
| | | 101 | | /// </summary> |
| | | 102 | | [Transient] |
| | | 103 | | public bool IsMantling { get; set; } |
| | | 104 | | |
| | | 105 | | /// <summary> |
| | | 106 | | /// Kind of affordance currently attached, when active. |
| | | 107 | | /// </summary> |
| | | 108 | | [Transient] |
| | | 109 | | public ClimbAffordanceKind ActiveClimbKind { get; set; } |
| | | 110 | | |
| | | 111 | | /// <summary> |
| | | 112 | | /// Stable host affordance identity when available. |
| | | 113 | | /// </summary> |
| | | 114 | | [Transient] |
| | | 115 | | public int? AttachmentId { get; set; } |
| | | 116 | | |
| | | 117 | | /// <summary> |
| | | 118 | | /// Stored attachment point for the current climb interaction. |
| | | 119 | | /// </summary> |
| | | 120 | | [Transient] |
| | | 121 | | public Vector3d AttachmentPoint { get; set; } |
| | | 122 | | |
| | | 123 | | /// <summary> |
| | | 124 | | /// Stored attachment surface normal for the current climb interaction. |
| | | 125 | | /// </summary> |
| | | 126 | | [Transient] |
| | | 127 | | public Vector3d AttachedSurfaceNormal { get; set; } |
| | | 128 | | |
| | | 129 | | /// <summary> |
| | | 130 | | /// Stored climb up direction for the current climb interaction. |
| | | 131 | | /// </summary> |
| | | 132 | | [Transient] |
| | | 133 | | public Vector3d AttachedUpDirection { get; set; } |
| | | 134 | | |
| | | 135 | | /// <summary> |
| | | 136 | | /// Whether lateral movement is allowed for the active frame snapshot. |
| | | 137 | | /// </summary> |
| | | 138 | | [Transient] |
| | | 139 | | public bool ActiveAllowLateralTraverse { get; set; } |
| | | 140 | | |
| | | 141 | | /// <summary> |
| | | 142 | | /// Whether descent is allowed for the active frame snapshot. |
| | | 143 | | /// </summary> |
| | | 144 | | [Transient] |
| | | 145 | | public bool ActiveAllowDescent { get; set; } |
| | | 146 | | |
| | | 147 | | /// <summary> |
| | | 148 | | /// Whether detaching via jump is allowed for the active frame snapshot. |
| | | 149 | | /// </summary> |
| | | 150 | | [Transient] |
| | | 151 | | public bool ActiveAllowDetachJump { get; set; } |
| | | 152 | | |
| | | 153 | | /// <summary> |
| | | 154 | | /// Whether mantling is allowed for the active frame snapshot. |
| | | 155 | | /// </summary> |
| | | 156 | | [Transient] |
| | | 157 | | public bool ActiveAllowMantle { get; set; } |
| | | 158 | | |
| | | 159 | | /// <summary> |
| | | 160 | | /// Current mantle target when a ledge affordance supplies one. |
| | | 161 | | /// </summary> |
| | | 162 | | [Transient] |
| | | 163 | | public Vector3d? MantleTargetPosition { get; set; } |
| | | 164 | | |
| | | 165 | | /// <summary> |
| | | 166 | | /// Applies a climb affordance snapshot to update the active attachment state for the current frame. |
| | | 167 | | /// </summary> |
| | | 168 | | /// <param name="snapshot"></param> |
| | | 169 | | public void ApplyClimbSnapshot(ClimbAffordanceSnapshot snapshot) |
| | | 170 | | { |
| | 64 | 171 | | ActiveClimbKind = snapshot.Kind; |
| | 64 | 172 | | AttachmentId = snapshot.AffordanceId; |
| | 64 | 173 | | AttachmentPoint = snapshot.AttachmentPoint; |
| | 64 | 174 | | AttachedSurfaceNormal = snapshot.SurfaceNormal; |
| | 64 | 175 | | AttachedUpDirection = snapshot.UpDirection; |
| | 64 | 176 | | ActiveAllowLateralTraverse = snapshot.AllowLateralTraverse && AllowLateralTraverse; |
| | 64 | 177 | | ActiveAllowDescent = snapshot.AllowDescent; |
| | 64 | 178 | | ActiveAllowDetachJump = snapshot.AllowDetachJump; |
| | 64 | 179 | | ActiveAllowMantle = snapshot.AllowMantle && snapshot.MantleTargetPosition.HasValue; |
| | 64 | 180 | | MantleTargetPosition = snapshot.MantleTargetPosition; |
| | 64 | 181 | | } |
| | | 182 | | |
| | | 183 | | /// <summary> |
| | | 184 | | /// Creates a readonly snapshot for optional active mantle validation. |
| | | 185 | | /// </summary> |
| | | 186 | | public ActiveMantleState CreateActiveMantleState() |
| | | 187 | | { |
| | 6 | 188 | | return new ActiveMantleState( |
| | 6 | 189 | | ActiveClimbKind, |
| | 6 | 190 | | AttachmentId, |
| | 6 | 191 | | AttachmentPoint, |
| | 6 | 192 | | AttachedSurfaceNormal, |
| | 6 | 193 | | AttachedUpDirection, |
| | 6 | 194 | | MantleTargetPosition ?? AttachmentPoint); |
| | | 195 | | } |
| | | 196 | | |
| | | 197 | | /// <inheritdoc /> |
| | | 198 | | public void RecordData(IChronicler chronicler) |
| | | 199 | | { |
| | 162 | 200 | | RecordValues.Look(chronicler, ref _isEnabled, "IsEnabled", true); |
| | 162 | 201 | | RecordValues.Look(chronicler, ref CanClimb, "CanClimb", true); |
| | 162 | 202 | | RecordValues.Look(chronicler, ref MaxClimbSpeed, "MaxClimbSpeed", DefaultMaxClimbSpeed); |
| | 162 | 203 | | RecordValues.Look(chronicler, ref MaxClimbAcceleration, "MaxClimbAcceleration", DefaultMaxClimbAcceleration); |
| | 162 | 204 | | RecordValues.Look(chronicler, ref GravityCompensationWhileClimbing, "GravityCompensationWhileClimbing", DefaultG |
| | 162 | 205 | | RecordValues.Look(chronicler, ref ClimbStartTolerance, "ClimbStartTolerance", DefaultClimbStartTolerance); |
| | 162 | 206 | | RecordValues.Look(chronicler, ref AllowLateralTraverse, "AllowLateralTraverse", true); |
| | 162 | 207 | | RecordValues.Look(chronicler, ref ValidateActiveMantleWithHost, "ValidateActiveMantleWithHost", false); |
| | | 208 | | |
| | 162 | 209 | | bool isClimbing = IsClimbing; |
| | 162 | 210 | | bool isMantling = IsMantling; |
| | 162 | 211 | | ClimbAffordanceKind activeClimbKind = ActiveClimbKind; |
| | 162 | 212 | | int? attachmentId = AttachmentId; |
| | 162 | 213 | | Vector3d attachmentPoint = AttachmentPoint; |
| | 162 | 214 | | Vector3d attachedSurfaceNormal = AttachedSurfaceNormal; |
| | 162 | 215 | | Vector3d attachedUpDirection = AttachedUpDirection; |
| | 162 | 216 | | bool activeAllowLateralTraverse = ActiveAllowLateralTraverse; |
| | 162 | 217 | | bool activeAllowDescent = ActiveAllowDescent; |
| | 162 | 218 | | bool activeAllowDetachJump = ActiveAllowDetachJump; |
| | 162 | 219 | | bool activeAllowMantle = ActiveAllowMantle; |
| | 162 | 220 | | Vector3d? mantleTargetPosition = MantleTargetPosition; |
| | | 221 | | |
| | 162 | 222 | | RecordValues.Look(chronicler, ref isClimbing, "IsClimbing", false); |
| | 162 | 223 | | RecordValues.Look(chronicler, ref isMantling, "IsMantling", false); |
| | 162 | 224 | | RecordValues.Look(chronicler, ref activeClimbKind, "ActiveClimbKind", ClimbAffordanceKind.None); |
| | 162 | 225 | | RecordValues.Look(chronicler, ref attachmentId, "AttachmentId", null); |
| | 162 | 226 | | RecordValues.Look(chronicler, ref attachmentPoint, "AttachmentPoint", Vector3d.Zero); |
| | 162 | 227 | | RecordValues.Look(chronicler, ref attachedSurfaceNormal, "AttachedSurfaceNormal", Vector3d.Zero); |
| | 162 | 228 | | RecordValues.Look(chronicler, ref attachedUpDirection, "AttachedUpDirection", Vector3d.Zero); |
| | 162 | 229 | | RecordValues.Look(chronicler, ref activeAllowLateralTraverse, "ActiveAllowLateralTraverse", false); |
| | 162 | 230 | | RecordValues.Look(chronicler, ref activeAllowDescent, "ActiveAllowDescent", true); |
| | 162 | 231 | | RecordValues.Look(chronicler, ref activeAllowDetachJump, "ActiveAllowDetachJump", true); |
| | 162 | 232 | | RecordValues.Look(chronicler, ref activeAllowMantle, "ActiveAllowMantle", false); |
| | 162 | 233 | | RecordValues.Look(chronicler, ref mantleTargetPosition, "MantleTargetPosition", null); |
| | | 234 | | |
| | 162 | 235 | | if (chronicler.Mode == SerializationMode.Loading) |
| | | 236 | | { |
| | 76 | 237 | | IsClimbing = isClimbing; |
| | 76 | 238 | | IsMantling = isMantling; |
| | 76 | 239 | | ActiveClimbKind = activeClimbKind; |
| | 76 | 240 | | AttachmentId = attachmentId; |
| | 76 | 241 | | AttachmentPoint = attachmentPoint; |
| | 76 | 242 | | AttachedSurfaceNormal = attachedSurfaceNormal; |
| | 76 | 243 | | AttachedUpDirection = attachedUpDirection; |
| | 76 | 244 | | ActiveAllowLateralTraverse = activeAllowLateralTraverse; |
| | 76 | 245 | | ActiveAllowDescent = activeAllowDescent; |
| | 76 | 246 | | ActiveAllowDetachJump = activeAllowDetachJump; |
| | 76 | 247 | | ActiveAllowMantle = activeAllowMantle; |
| | 76 | 248 | | MantleTargetPosition = mantleTargetPosition; |
| | | 249 | | |
| | 76 | 250 | | if (!_isEnabled) |
| | 2 | 251 | | this.ClearTransientState(); |
| | | 252 | | } |
| | 162 | 253 | | } |
| | | 254 | | } |