From fd01740d76fd6ffc797b63124ef2991aee16506f Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Tue, 3 Jun 2025 17:49:51 +0200
Subject: [PATCH] Squashed 'LinearAlgebra/' changes from 9b2a047..493c19c

493c19c Fix ToVector3
35b6830 Support sending poses from Unity changes
6d74649 Compatibility

git-subtree-dir: LinearAlgebra
git-subtree-split: 493c19cbe22465ead382a42ff952b06a24f60dda
---
 src/Spherical.cs      | 2 +-
 src/SwingTwist.cs     | 9 ++++++++-
 test/DirectionTest.cs | 4 +++-
 3 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/src/Spherical.cs b/src/Spherical.cs
index 183aa01..e138e7c 100644
--- a/src/Spherical.cs
+++ b/src/Spherical.cs
@@ -106,7 +106,7 @@ namespace LinearAlgebra {
         // }
 
         public Vector3 ToVector3() {
-            float verticalRad = (float)(Math.PI / 2 - this.direction.vertical) * Angle.Deg2Rad;
+            float verticalRad = (float)(90 - this.direction.vertical) * Angle.Deg2Rad;
             float horizontalRad = this.direction.horizontal * Angle.Deg2Rad;
             float cosVertical = (float)Math.Cos(verticalRad);
             float sinVertical = (float)Math.Sin(verticalRad);
diff --git a/src/SwingTwist.cs b/src/SwingTwist.cs
index 22eb0bb..58c1a1a 100644
--- a/src/SwingTwist.cs
+++ b/src/SwingTwist.cs
@@ -29,11 +29,18 @@ namespace LinearAlgebra {
         }
 
 #if UNITY_5_3_OR_NEWER
+        public static SwingTwist FromQuaternion(Quaternion q) {
+            // q.ToAngles(out float right, out float up, out float forward);
+            UnityEngine.Vector3 angles = q.eulerAngles;
+            SwingTwist r = new SwingTwist(angles.y, angles.x, angles.z);
+            return r;
+        }
+
         public Quaternion ToQuaternion() {
             Quaternion q = Quaternion.Euler(-this.swing.vertical,
                                    this.swing.horizontal,
                                    this.twist);
-                                   return q;
+            return q;
         }
 #endif        
     }
diff --git a/test/DirectionTest.cs b/test/DirectionTest.cs
index e31af4c..3cebe1a 100644
--- a/test/DirectionTest.cs
+++ b/test/DirectionTest.cs
@@ -1,3 +1,4 @@
+#if !UNITY_5_6_OR_NEWER
 using NUnit.Framework;
 
 namespace LinearAlgebra.Test {
@@ -14,4 +15,5 @@ namespace LinearAlgebra.Test {
             Assert.True(r);
         }
     };
-}
\ No newline at end of file
+}
+#endif
\ No newline at end of file