| | | 1 | | using System.Runtime.CompilerServices; |
| | | 2 | | |
| | | 3 | | namespace FixedMathSharp; |
| | | 4 | | |
| | | 5 | | /// <summary> |
| | | 6 | | /// Provides extension methods for performing operations and comparisons on FixedQuaternion instances, including |
| | | 7 | | /// approximate equality checks and angular velocity calculations. |
| | | 8 | | /// </summary> |
| | | 9 | | public static partial class FixedQuaternionExtensions |
| | | 10 | | { |
| | | 11 | | /// <inheritdoc cref="FixedQuaternion.ToAngularVelocity" /> |
| | | 12 | | public static Vector3d ToAngularVelocity( |
| | | 13 | | this FixedQuaternion currentRotation, |
| | | 14 | | FixedQuaternion previousRotation, |
| | | 15 | | Fixed64 deltaTime) |
| | 1 | 16 | | { |
| | 1 | 17 | | return FixedQuaternion.ToAngularVelocity(currentRotation, previousRotation, deltaTime); |
| | 1 | 18 | | } |
| | | 19 | | |
| | | 20 | | #region Equality |
| | | 21 | | |
| | | 22 | | /// <summary> |
| | | 23 | | /// Compares two quaternions for approximate equality, allowing a fixed absolute difference between components. |
| | | 24 | | /// </summary> |
| | | 25 | | /// <param name="q1">The current quaternion.</param> |
| | | 26 | | /// <param name="q2">The quaternion to compare against.</param> |
| | | 27 | | /// <param name="allowedDifference">The allowed absolute difference between each component.</param> |
| | | 28 | | /// <returns>True if the components are within the allowed difference, false otherwise.</returns> |
| | | 29 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 30 | | public static bool FuzzyEqualAbsolute(this FixedQuaternion q1, FixedQuaternion q2, Fixed64 allowedDifference) |
| | 6 | 31 | | { |
| | 6 | 32 | | return (q1.x - q2.x).Abs() <= allowedDifference && |
| | 6 | 33 | | (q1.y - q2.y).Abs() <= allowedDifference && |
| | 6 | 34 | | (q1.z - q2.z).Abs() <= allowedDifference && |
| | 6 | 35 | | (q1.w - q2.w).Abs() <= allowedDifference; |
| | 6 | 36 | | } |
| | | 37 | | |
| | | 38 | | /// <summary> |
| | | 39 | | /// Compares two quaternions for approximate equality, allowing a fractional percentage (defaults to ~1%) difference |
| | | 40 | | /// </summary> |
| | | 41 | | /// <param name="q1">The current quaternion.</param> |
| | | 42 | | /// <param name="q2">The quaternion to compare against.</param> |
| | | 43 | | /// <param name="percentage">The allowed fractional difference (percentage) for each component.</param> |
| | | 44 | | /// <returns>True if the components are within the allowed percentage difference, false otherwise.</returns> |
| | | 45 | | public static bool FuzzyEqual(this FixedQuaternion q1, FixedQuaternion q2, Fixed64? percentage = null) |
| | 37 | 46 | | { |
| | 37 | 47 | | Fixed64 p = percentage ?? Fixed64.Epsilon; |
| | 37 | 48 | | return q1.x.FuzzyComponentEqual(q2.x, p) && |
| | 37 | 49 | | q1.y.FuzzyComponentEqual(q2.y, p) && |
| | 37 | 50 | | q1.z.FuzzyComponentEqual(q2.z, p) && |
| | 37 | 51 | | q1.w.FuzzyComponentEqual(q2.w, p); |
| | 37 | 52 | | } |
| | | 53 | | |
| | | 54 | | #endregion |
| | | 55 | | } |