From 242b38ee9ebcbbdc8089748f701ccdfc51f48573 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 14 Jul 2020 09:21:01 +0200
Subject: [PATCH] Quaternion: remove deprecated axis_angle conversions

---
 matrix/Quaternion.hpp | 93 -------------------------------------------
 test/attitude.cpp     | 18 +++++----
 2 files changed, 11 insertions(+), 100 deletions(-)

diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index 24e8c7d806..1e8ca2b6ca 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -224,7 +224,6 @@ public:
         q.normalize();
     }
 
-
     /**
      * Constructor from quaternion values
      *
@@ -358,7 +357,6 @@ public:
         *this = this->canonical();
     }
 
-
     /**
      * Return canonical form of the quaternion
      *
@@ -420,82 +418,6 @@ public:
         return Vector3<Type>(res(1), res(2), res(3));
     }
 
-    /**
-     * Rotation quaternion from vector
-     *
-     * The axis of rotation is given by vector direction and
-     * the angle is given by the norm.
-     *
-     * @param vec rotation vector
-     * @return quaternion representing the rotation
-     */
-    void from_axis_angle(Vector<Type, 3> vec)
-    {
-        Quaternion &q = *this;
-        Type theta = vec.norm();
-
-        if (theta < Type(1e-10)) {
-            q(0) = Type(1);
-            q(1) = q(2) = q(3) = Type(0);
-            return;
-        }
-
-        vec /= theta;
-        from_axis_angle(vec, theta);
-    }
-
-    /**
-     * Rotation quaternion from axis and angle
-     * XXX DEPRECATED, use AxisAngle class
-     *
-     * @param axis axis of rotation
-     * @param theta scalar describing angle of rotation
-     * @return quaternion representing the rotation
-     */
-    void from_axis_angle(const Vector<Type, 3> &axis, Type theta)
-    {
-        Quaternion &q = *this;
-
-        if (theta < Type(1e-10)) {
-            q(0) = Type(1);
-            q(1) = q(2) = q(3) = Type(0);
-        }
-
-        Type magnitude = sin(theta / 2.0f);
-
-        q(0) = cos(theta / 2.0f);
-        q(1) = axis(0) * magnitude;
-        q(2) = axis(1) * magnitude;
-        q(3) = axis(2) * magnitude;
-    }
-
-
-    /**
-     * Rotation vector from quaternion
-     * XXX DEPRECATED, use AxisAngle class
-     *
-     * The axis of rotation is given by vector direction and
-     * the angle is given by the norm.
-     *
-     * @return vector, direction representing rotation axis and norm representing angle
-     */
-    Vector<Type, 3> to_axis_angle() const
-    {
-        const Quaternion &q = *this;
-        Type axis_magnitude = Type(sqrt(q(1) * q(1) + q(2) * q(2) + q(3) * q(3)));
-        Vector<Type, 3> vec;
-        vec(0) = q(1);
-        vec(1) = q(2);
-        vec(2) = q(3);
-
-        if (axis_magnitude >= Type(1e-10)) {
-            vec = vec / axis_magnitude;
-            vec = vec * wrap_pi(Type(2) * atan2(axis_magnitude, q(0)));
-        }
-
-        return vec;
-    }
-
     /**
      * Imaginary components of quaternion
      */
@@ -525,21 +447,6 @@ public:
         R_z(2) = a * a - b * b - c * c + d * d;
         return R_z;
     }
-
-    /**
-     * XXX DEPRECATED, can use assignment or ctor
-     */
-    Quaternion from_dcm(const Matrix<Type, 3, 3>& dcm) {
-        return Quaternion(Dcm<Type>(dcm));
-    }
-
-    /**
-     * XXX DEPRECATED, can use assignment or ctor
-     */
-    Dcm<Type> to_dcm() {
-        return Dcm<Type>(*this);
-    }
-
 };
 
 typedef Quaternion<float> Quatf;
diff --git a/test/attitude.cpp b/test/attitude.cpp
index 3acab686f5..3e3c1d164c 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -341,29 +341,29 @@ int main()
 
     // get rotation axis from quaternion (nonzero rotation)
     q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f);
-    rot = q.to_axis_angle();
+    rot = AxisAnglef(q);
     TEST(fabs(rot(0)) < eps);
     TEST(fabs(rot(1) - 1.0f) < eps);
     TEST(fabs(rot(2)) < eps);
 
     // get rotation axis from quaternion (zero rotation)
     q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
-    rot = q.to_axis_angle();
+    rot = AxisAnglef(q);
     TEST(fabs(rot(0)) < eps);
     TEST(fabs(rot(1)) < eps);
     TEST(fabs(rot(2)) < eps);
 
     // from axis angle (zero rotation)
     rot(0) = rot(1) = rot(2) = 0.0f;
-    q.from_axis_angle(rot, 0.0f);
+    q = Quatf(AxisAnglef(rot));
     q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
     TEST(isEqual(q, q_true));
 
     // from axis angle, with length of vector the rotation
     float n = float(sqrt(4*M_PI*M_PI/3));
-    q.from_axis_angle(Vector3f(n, n, n));
+    q = AxisAnglef(n, n, n);
     TEST(isEqual(q, Quatf(-1, 0, 0, 0)));
-    q.from_axis_angle(Vector3f(0, 0, 0));
+    q = AxisAnglef(0, 0, 0);
     TEST(isEqual(q, Quatf(1, 0, 0, 0)));
 
     // Quaternion initialisation per array
@@ -388,13 +388,17 @@ int main()
     TEST(isEqualF(aa_norm_check.angle(), 0.0f));
 
     q = Quatf(-0.29555112749297824f, 0.25532186f,  0.51064372f,  0.76596558f);
+    float r_array[9] = {-0.6949206f, 0.713521f, 0.089292854f, -0.19200698f, -0.30378509f, 0.93319237f, 0.69297814f, 0.63134968f, 0.34810752f};
+    R = Dcmf(r_array);
     TEST(isEqual(q.imag(), Vector3f(0.25532186f,  0.51064372f,  0.76596558f)));
 
     // from dcm
-    TEST(isEqual(Eulerf(q.from_dcm(Dcmf(q))), Eulerf(q)));
+    TEST(isEqual(Quatf(R), q));
+    TEST(isEqual(Quatf(Dcmf(q)), q));
 
     // to dcm
-    TEST(isEqual(Dcmf(q), q.to_dcm()));
+    TEST(isEqual(Dcmf(q), R));
+    TEST(isEqual(Dcmf(Quatf(R)), R));
 
     // conjugate
     v = Vector3f(1.5f, 2.2f, 3.2f);