| | | 1 | | //======================================================================= |
| | | 2 | | // JointMotor3D.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 | | /// Deterministic angular target-drive payload for a 3D joint. |
| | | 14 | | /// </summary> |
| | | 15 | | public readonly struct JointMotor3D |
| | | 16 | | { |
| | | 17 | | /// <summary> |
| | | 18 | | /// Creates an angular motor payload. |
| | | 19 | | /// </summary> |
| | | 20 | | public JointMotor3D( |
| | | 21 | | FixedQuaternion targetLocalRotation, |
| | | 22 | | Fixed64 angularDriveStrength, |
| | | 23 | | Fixed64 angularDriveDamping, |
| | | 24 | | Fixed64 maximumMotorImpulse) |
| | | 25 | | { |
| | 18 | 26 | | TargetLocalRotation = targetLocalRotation.Normalized; |
| | 18 | 27 | | AngularDriveStrength = angularDriveStrength; |
| | 18 | 28 | | AngularDriveDamping = angularDriveDamping; |
| | 18 | 29 | | MaximumMotorImpulse = maximumMotorImpulse; |
| | 18 | 30 | | Validate(); |
| | 18 | 31 | | } |
| | | 32 | | |
| | | 33 | | /// <summary> |
| | | 34 | | /// Gets a disabled motor payload. |
| | | 35 | | /// </summary> |
| | 302 | 36 | | public static JointMotor3D Disabled => default; |
| | | 37 | | |
| | | 38 | | /// <summary> |
| | | 39 | | /// Gets the desired B-relative-to-A joint-space rotation. |
| | | 40 | | /// </summary> |
| | | 41 | | public FixedQuaternion TargetLocalRotation { get; } |
| | | 42 | | |
| | | 43 | | /// <summary> |
| | | 44 | | /// Gets the deterministic spring-like angular drive strength. |
| | | 45 | | /// </summary> |
| | | 46 | | public Fixed64 AngularDriveStrength { get; } |
| | | 47 | | |
| | | 48 | | /// <summary> |
| | | 49 | | /// Gets angular damping applied against relative angular velocity. |
| | | 50 | | /// </summary> |
| | | 51 | | public Fixed64 AngularDriveDamping { get; } |
| | | 52 | | |
| | | 53 | | /// <summary> |
| | | 54 | | /// Gets the maximum absolute impulse this motor may apply per row and iteration. |
| | | 55 | | /// </summary> |
| | | 56 | | public Fixed64 MaximumMotorImpulse { get; } |
| | | 57 | | |
| | | 58 | | /// <summary> |
| | | 59 | | /// Gets whether this motor can emit solver rows. |
| | | 60 | | /// </summary> |
| | | 61 | | public bool IsEnabled => |
| | 49703 | 62 | | AngularDriveStrength > Fixed64.Zero |
| | 49703 | 63 | | && MaximumMotorImpulse > Fixed64.Zero; |
| | | 64 | | |
| | | 65 | | internal void Validate() |
| | | 66 | | { |
| | 394 | 67 | | SwiftThrowHelper.ThrowIfArgument( |
| | 394 | 68 | | AngularDriveStrength < Fixed64.Zero, |
| | 394 | 69 | | nameof(AngularDriveStrength), |
| | 394 | 70 | | "Angular drive strength cannot be negative."); |
| | 394 | 71 | | SwiftThrowHelper.ThrowIfArgument( |
| | 394 | 72 | | AngularDriveDamping < Fixed64.Zero, |
| | 394 | 73 | | nameof(AngularDriveDamping), |
| | 394 | 74 | | "Angular drive damping cannot be negative."); |
| | 394 | 75 | | SwiftThrowHelper.ThrowIfArgument( |
| | 394 | 76 | | MaximumMotorImpulse < Fixed64.Zero, |
| | 394 | 77 | | nameof(MaximumMotorImpulse), |
| | 394 | 78 | | "Maximum motor impulse cannot be negative."); |
| | 394 | 79 | | } |
| | | 80 | | } |