diff --git a/Assets/NanoBrain/LinearAlgebra/src/Direction.cs b/Assets/NanoBrain/LinearAlgebra/src/Direction.cs index ed82901..a962589 100644 --- a/Assets/NanoBrain/LinearAlgebra/src/Direction.cs +++ b/Assets/NanoBrain/LinearAlgebra/src/Direction.cs @@ -102,17 +102,7 @@ namespace LinearAlgebra { } } -#if !UNITY_5_3_OR_NEWER - /// - /// Convert the direction into a carthesian vector - /// - /// The carthesian vector corresponding to this direction. - public Vector3Float ToVector3Float() { - Quaternion q = Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0); - Vector3Float v = q * Vector3Float.forward; - return v; - } -#else +#if UNITY_5_3_OR_NEWER /// /// Convert the direction into a carthesian vector /// @@ -122,7 +112,30 @@ namespace LinearAlgebra { UnityEngine.Vector3 v = q * UnityEngine.Vector3.forward; return v; } -#endif + + /// + /// Convert a carthesian vector into a direction + /// + /// The carthesian vector + /// The direction + /// Information about the length of the carthesian vector is not + /// included in this transformation + public static Direction FromVector3(UnityEngine.Vector3 v) { + AngleFloat horizontal = AngleFloat.Atan2(v.x, v.z); + AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.y); + Direction d = new(horizontal, vertical); + return d; + } +#else + /// + /// Convert the direction into a carthesian vector + /// + /// The carthesian vector corresponding to this direction. + public Vector3Float ToVector3() { + Quaternion q = Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0); + Vector3Float v = q * Vector3Float.forward; + return v; + } /// /// Convert a carthesian vector into a direction @@ -137,6 +150,8 @@ namespace LinearAlgebra { Direction d = new(horizontal, vertical); return d; } +#endif + /// /// Tests the equality of two directions diff --git a/Assets/NanoBrain/LinearAlgebra/src/SwingTwist.cs b/Assets/NanoBrain/LinearAlgebra/src/SwingTwist.cs index 4437c4f..df6e048 100644 --- a/Assets/NanoBrain/LinearAlgebra/src/SwingTwist.cs +++ b/Assets/NanoBrain/LinearAlgebra/src/SwingTwist.cs @@ -1,6 +1,6 @@ -#if !UNITY_5_3_OR_NEWER -using UnityEngine; -#endif +// #if !UNITY_5_3_OR_NEWER +// using UnityEngine; +// #endif namespace LinearAlgebra { @@ -46,7 +46,45 @@ namespace LinearAlgebra { return s; } -#if !UNITY_5_3_OR_NEWER +#if UNITY_5_3_OR_NEWER + /// + /// A zero angle rotation + /// + public static readonly SwingTwist zero = Degrees(0, 0, 0); + + public Spherical ToAngleAxis() { + UnityEngine.Quaternion q = this.ToQuaternion(); + q.ToAngleAxis(out float angle, out UnityEngine.Vector3 axis); + Direction direction = Direction.FromVector3(axis); + + Spherical r = new(angle, direction); + return r; + } + + public static SwingTwist FromAngleAxis(Spherical r) { + UnityEngine.Vector3 vectorAxis = r.direction.ToVector3(); + UnityEngine.Quaternion q = UnityEngine.Quaternion.AngleAxis(r.distance, vectorAxis); + return FromQuaternion(q); + } + + /// + /// Convert a quaternion in a swing/twist rotation + /// + /// The quaternion to convert + /// The swing/twist rotation + public static SwingTwist FromQuaternion(UnityEngine.Quaternion q) { + UnityEngine.Vector3 angles = q.eulerAngles; + SwingTwist r = Degrees(angles.y, -angles.x, -angles.z); + return r; + } + + public UnityEngine.Quaternion ToQuaternion() { + UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(this.swing.vertical.inDegrees, + this.swing.horizontal.inDegrees, + this.twist.inDegrees); + return q; + } +#else /// /// A zero angle rotation /// @@ -92,25 +130,7 @@ namespace LinearAlgebra { return r; } #endif -#if UNITY_5_3_OR_NEWER - /// - /// Convert a quaternion in a swing/twist rotation - /// - /// The quaternion to convert - /// The swing/twist rotation - public static SwingTwist FromQuaternion(UnityEngine.Quaternion q) { - UnityEngine.Vector3 angles = q.eulerAngles; - SwingTwist r = Degrees(angles.y, -angles.x, -angles.z); - return r; - } - - public UnityEngine.Quaternion ToUnityQuaternion() { - UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(this.swing.vertical.inDegrees, - this.swing.horizontal.inDegrees, - this.twist.inDegrees); - return q; - } -#endif + } } \ No newline at end of file