| | | 1 | | //======================================================================= |
| | | 2 | | // JointLimit3D.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 | | |
| | | 10 | | namespace Gravitas.Constraints; |
| | | 11 | | |
| | | 12 | | /// <summary> |
| | | 13 | | /// Optional deterministic angular limit payload for a 3D joint. |
| | | 14 | | /// </summary> |
| | | 15 | | public readonly struct JointLimit3D |
| | | 16 | | { |
| | | 17 | | private JointLimit3D( |
| | | 18 | | JointLimitKind3D kind, |
| | | 19 | | Fixed64 maxHingeAngle, |
| | | 20 | | Fixed64 maxConeAngle, |
| | | 21 | | Fixed64 maxTwistAngle) |
| | | 22 | | { |
| | 55 | 23 | | Kind = kind; |
| | 55 | 24 | | MaxHingeAngle = maxHingeAngle; |
| | 55 | 25 | | MaxConeAngle = maxConeAngle; |
| | 55 | 26 | | MaxTwistAngle = maxTwistAngle; |
| | 55 | 27 | | } |
| | | 28 | | |
| | | 29 | | /// <summary> |
| | | 30 | | /// Gets an unrestricted limit payload. |
| | | 31 | | /// </summary> |
| | 272 | 32 | | public static JointLimit3D Unrestricted => default; |
| | | 33 | | |
| | | 34 | | /// <summary> |
| | | 35 | | /// Gets the active limit family. |
| | | 36 | | /// </summary> |
| | | 37 | | public JointLimitKind3D Kind { get; } |
| | | 38 | | |
| | | 39 | | /// <summary> |
| | | 40 | | /// Gets the maximum absolute hinge angle in radians. |
| | | 41 | | /// </summary> |
| | | 42 | | public Fixed64 MaxHingeAngle { get; } |
| | | 43 | | |
| | | 44 | | /// <summary> |
| | | 45 | | /// Gets the maximum cone swing angle in radians. |
| | | 46 | | /// </summary> |
| | | 47 | | public Fixed64 MaxConeAngle { get; } |
| | | 48 | | |
| | | 49 | | /// <summary> |
| | | 50 | | /// Gets the maximum absolute twist angle in radians. |
| | | 51 | | /// </summary> |
| | | 52 | | public Fixed64 MaxTwistAngle { get; } |
| | | 53 | | |
| | | 54 | | /// <summary> |
| | | 55 | | /// Creates a hinge limit around the local X axis. |
| | | 56 | | /// </summary> |
| | | 57 | | public static JointLimit3D Hinge(Fixed64 maxAngle) |
| | | 58 | | { |
| | 38 | 59 | | ValidateNonNegative(maxAngle, nameof(maxAngle)); |
| | 38 | 60 | | return new JointLimit3D(JointLimitKind3D.Hinge, maxAngle, Fixed64.Zero, Fixed64.Zero); |
| | | 61 | | } |
| | | 62 | | |
| | | 63 | | /// <summary> |
| | | 64 | | /// Creates a cone-twist limit around the local Z axis. |
| | | 65 | | /// </summary> |
| | | 66 | | public static JointLimit3D ConeTwist(Fixed64 maxConeAngle, Fixed64 maxTwistAngle) |
| | | 67 | | { |
| | 17 | 68 | | ValidateNonNegative(maxConeAngle, nameof(maxConeAngle)); |
| | 17 | 69 | | ValidateNonNegative(maxTwistAngle, nameof(maxTwistAngle)); |
| | 17 | 70 | | return new JointLimit3D(JointLimitKind3D.ConeTwist, Fixed64.Zero, maxConeAngle, maxTwistAngle); |
| | | 71 | | } |
| | | 72 | | |
| | | 73 | | internal void Validate() |
| | | 74 | | { |
| | 373 | 75 | | SwiftThrowHelper.ThrowIfArgument( |
| | 373 | 76 | | Kind != JointLimitKind3D.Unrestricted |
| | 373 | 77 | | && Kind != JointLimitKind3D.Hinge |
| | 373 | 78 | | && Kind != JointLimitKind3D.ConeTwist, |
| | 373 | 79 | | nameof(Kind), |
| | 373 | 80 | | "Unsupported joint limit kind."); |
| | 373 | 81 | | ValidateNonNegative(MaxHingeAngle, nameof(MaxHingeAngle)); |
| | 373 | 82 | | ValidateNonNegative(MaxConeAngle, nameof(MaxConeAngle)); |
| | 373 | 83 | | ValidateNonNegative(MaxTwistAngle, nameof(MaxTwistAngle)); |
| | 373 | 84 | | } |
| | | 85 | | |
| | | 86 | | private static void ValidateNonNegative(Fixed64 value, string name) => |
| | 1191 | 87 | | SwiftThrowHelper.ThrowIfArgument(value < Fixed64.Zero, name, "Joint limit angles cannot be negative."); |
| | | 88 | | } |