Extended unit tests (plus fixes)

AngleAxis.cpp

@@ -19,7 +19,7 @@ AngleAxisOf<T>::AngleAxisOf(float angle, DirectionOf<T> axis) {
 template <typename T>
 AngleAxisOf<T>::AngleAxisOf(float angle, Vector3 axis) {
   this->angle = angle;
-  this->axis = DirectionOf<T>(axis);
+  this->axis = DirectionOf<T>::FromVector3(axis);
 }
 
 template <typename T>
@@ -28,7 +28,7 @@ AngleAxisOf<T>::AngleAxisOf(Quaternion q) {
   Vector3 axis;
   q.ToAngleAxis(&angle, &axis);
   this->angle = angle;
-  this->axis = DirectionOf<T>(axis);
+  this->axis = DirectionOf<T>::FromVector3(axis);
 }
 
 template <typename T>

CMakeLists.txt

@@ -40,21 +40,10 @@ else()
 
     enable_testing()
 
-    file(GLOB_RECURSE test_srcs test/*.cc)
+    file(GLOB_RECURSE test_srcs test/*_test.cc)
     add_executable(
         LinearAlgebraTest
         ${test_srcs}
-        # "test/Angle_test.cc"
-        # "test/Direction_test.cc"
-        # "test/DiscreteAngle_test.cc"
-        # "test/FloatSingle_test.cc"
-        # "test/Matrix_test.cc"
-        # "test/Polar_test.cc"
-        # "test/Quaternion_test.cc"
-        # "test/Spherical_test.cc"
-        # "test/Spherical16_test.cc"
-        # "test/Vector2_test.cc"
-        # "test/Vector3_test.cc"
     )
     target_link_libraries(
         LinearAlgebraTest

Direction.cpp

@@ -22,17 +22,17 @@ DirectionOf<T>::DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical) {
   Normalize();
 };
 
-template <typename T>
+// template <typename T>
-DirectionOf<T>::DirectionOf(Vector3 v) {
+// DirectionOf<T>::DirectionOf(Vector3 v) {
-  this->horizontal = AngleOf<T>::Atan2(
+//   this->horizontal = AngleOf<T>::Atan2(
-      v.Right(),
+//       v.Right(),
-      v.Forward());  // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
+//       v.Forward());  // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
-  this->vertical =
+//   this->vertical =
-      -AngleOf<T>::deg90 -
+//       -AngleOf<T>::deg90 -
-      AngleOf<T>::Acos(
+//       AngleOf<T>::Acos(
-          v.Up());  // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
+//           v.Up());  // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
-  Normalize();
+//   Normalize();
-}
+// }
 
 template <typename T>
 const DirectionOf<T> DirectionOf<T>::forward =
@@ -53,6 +53,28 @@ template <typename T>
 const DirectionOf<T> DirectionOf<T>::right =
     DirectionOf<T>(AngleOf<T>::deg90, AngleOf<T>());
 
+template <typename T>
+Vector3 Passer::LinearAlgebra::DirectionOf<T>::ToVector3() const {
+  Quaternion q = Quaternion::Euler(-this->vertical.InDegrees(),
+                                   this->horizontal.InDegrees(), 0);
+  Vector3 v = q * Vector3::forward;
+  return v;
+}
+
+template <typename T>
+DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::FromVector3(Vector3 v) {
+  DirectionOf<T> d;
+  d.horizontal = AngleOf<T>::Atan2(
+      v.Right(),
+      v.Forward());  // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
+  d.vertical =
+      -AngleOf<T>::deg90 -
+      AngleOf<T>::Acos(
+          v.Up());  // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
+  d.Normalize();
+  return d;
+}
+
 template <typename T>
 DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Degrees(float horizontal,
                                                               float vertical) {

Direction.h

@@ -22,7 +22,10 @@ class DirectionOf {
 
   DirectionOf<T>();
   DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
-  DirectionOf<T>(Vector3 v);
+  // DirectionOf<T>(Vector3 v);
+
+  Vector3 ToVector3() const;
+  static DirectionOf<T> FromVector3(Vector3 v);
 
   static DirectionOf<T> Degrees(float horizontal, float vertical);
   static DirectionOf<T> Radians(float horizontal, float vertical);

Quaternion.cpp

@@ -136,7 +136,7 @@ Vector3 Quaternion::operator*(const Vector3& p) const {
   return result;
 }
 
-bool Quaternion::operator==(const Quaternion& q) {
+bool Quaternion::operator==(const Quaternion& q) const {
   return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w);
 }
 

Quaternion.h

@@ -39,7 +39,7 @@ typedef struct Quat {
 /// A quaternion
 /// </summary>
 struct Quaternion : Quat {
-public:
+ public:
   /// <summary>
   /// Create a new identity quaternion
   /// </summary>
@@ -80,7 +80,7 @@ public:
   /// <returns>A unit quaternion</returns>
   /// This will preserve the orientation,
   /// but ensures that it is a unit quaternion.
-  static Quaternion Normalize(const Quaternion &q);
+  static Quaternion Normalize(const Quaternion& q);
 
   /// <summary>
   /// Convert to euler angles
@@ -88,14 +88,14 @@ public:
   /// <param name="q">The quaternion to convert</param>
   /// <returns>A vector containing euler angles</returns>
   /// The euler angles performed in the order: Z, X, Y
-  static Vector3 ToAngles(const Quaternion &q);
+  static Vector3 ToAngles(const Quaternion& q);
 
   /// <summary>
   /// Rotate a vector using this quaterion
   /// </summary>
   /// <param name="vector">The vector to rotate</param>
   /// <returns>The rotated vector</returns>
-  Vector3 operator*(const Vector3 &vector) const;
+  Vector3 operator*(const Vector3& vector) const;
   /// <summary>
   /// Multiply this quaternion with another quaternion
   /// </summary>
@@ -103,7 +103,7 @@ public:
   /// <returns>The resulting rotation</returns>
   /// The result will be this quaternion rotated according to
   /// the give rotation.
-  Quaternion operator*(const Quaternion &rotation) const;
+  Quaternion operator*(const Quaternion& rotation) const;
 
   /// <summary>
   /// Check the equality of two quaternions
@@ -114,7 +114,7 @@ public:
   /// themselves. Two quaternions with the same rotational effect may have
   /// different components. Use Quaternion::Angle to check if the rotations are
   /// the same.
-  bool operator==(const Quaternion &quaternion);
+  bool operator==(const Quaternion& quaternion) const;
 
   /// <summary>
   /// The inverse of quaterion
@@ -129,8 +129,8 @@ public:
   /// <param name="forward">The look direction</param>
   /// <param name="upwards">The up direction</param>
   /// <returns>The look rotation</returns>
-  static Quaternion LookRotation(const Vector3 &forward,
+  static Quaternion LookRotation(const Vector3& forward,
-                                 const Vector3 &upwards);
+                                 const Vector3& upwards);
   /// <summary>
   /// Creates a quaternion with the given forward direction with up =
   /// Vector3::up
@@ -140,7 +140,7 @@ public:
   /// For the rotation, Vector::up is used for the up direction.
   /// Note: if the forward direction == Vector3::up, the result is
   /// Quaternion::identity
-  static Quaternion LookRotation(const Vector3 &forward);
+  static Quaternion LookRotation(const Vector3& forward);
 
   /// <summary>
   /// Calculat the rotation from on vector to another
@@ -157,7 +157,8 @@ public:
   /// <param name="to">The destination rotation</param>
   /// <param name="maxDegreesDelta">The maximum amount of degrees to
   /// rotate</param> <returns>The possibly limited rotation</returns>
-  static Quaternion RotateTowards(const Quaternion &from, const Quaternion &to,
+  static Quaternion RotateTowards(const Quaternion& from,
+                                  const Quaternion& to,
                                   float maxDegreesDelta);
 
   /// <summary>
@@ -166,13 +167,13 @@ public:
   /// <param name="angle">The angle</param>
   /// <param name="axis">The axis</param>
   /// <returns>The resulting quaternion</returns>
-  static Quaternion AngleAxis(float angle, const Vector3 &axis);
+  static Quaternion AngleAxis(float angle, const Vector3& axis);
   /// <summary>
   /// Convert this quaternion to angle/axis representation
   /// </summary>
   /// <param name="angle">A pointer to the angle for the result</param>
   /// <param name="axis">A pointer to the axis for the result</param>
-  void ToAngleAxis(float *angle, Vector3 *axis);
+  void ToAngleAxis(float* angle, Vector3* axis);
 
   /// <summary>
   /// Get the angle between two orientations
@@ -190,8 +191,9 @@ public:
   /// <param name="factor">The factor between 0 and 1.</param>
   /// <returns>The resulting rotation</returns>
   /// A factor 0 returns rotation1, factor1 returns rotation2.
-  static Quaternion Slerp(const Quaternion &rotation1,
+  static Quaternion Slerp(const Quaternion& rotation1,
-                          const Quaternion &rotation2, float factor);
+                          const Quaternion& rotation2,
+                          float factor);
   /// <summary>
   /// Unclamped sherical lerp between two rotations
   /// </summary>
@@ -201,8 +203,9 @@ public:
   /// <returns>The resulting rotation</returns>
   /// A factor 0 returns rotation1, factor1 returns rotation2.
   /// Values outside the 0..1 range will result in extrapolated rotations
-  static Quaternion SlerpUnclamped(const Quaternion &rotation1,
+  static Quaternion SlerpUnclamped(const Quaternion& rotation1,
-                                   const Quaternion &rotation2, float factor);
+                                   const Quaternion& rotation2,
+                                   float factor);
 
   /// <summary>
   /// Create a rotation from euler angles
@@ -260,8 +263,10 @@ public:
   /// <param name="swing">A pointer to the quaternion for the swing
   /// result</param> <param name="twist">A pointer to the quaternion for the
   /// twist result</param>
-  static void GetSwingTwist(Vector3 axis, Quaternion rotation,
+  static void GetSwingTwist(Vector3 axis,
-                            Quaternion *swing, Quaternion *twist);
+                            Quaternion rotation,
+                            Quaternion* swing,
+                            Quaternion* twist);
 
   /// <summary>
   /// Calculate the dot product of two quaternions
@@ -271,12 +276,12 @@ public:
   /// <returns></returns>
   static float Dot(Quaternion rotation1, Quaternion rotation2);
 
-private:
+ private:
   float GetLength() const;
   float GetLengthSquared() const;
-  static float GetLengthSquared(const Quaternion &q);
+  static float GetLengthSquared(const Quaternion& q);
 
-  void ToAxisAngleRad(const Quaternion &q, Vector3 *const axis, float *angle);
+  void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
   static Quaternion FromEulerRad(Vector3 euler);
   static Quaternion FromEulerRadXYZ(Vector3 euler);
 

SwingTwist.cpp

@@ -55,6 +55,26 @@ SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion(
   return r;
 }
 
+template <typename T>
+SphericalOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::ToAngleAxis() const {
+  Quaternion q = this->ToQuaternion();
+  float angle;
+  Vector3 axis;
+  q.ToAngleAxis(&angle, &axis);
+  DirectionOf<T> direction = DirectionOf<T>::FromVector3(axis);
+
+  SphericalOf<T> aa = SphericalOf<T>(angle, direction);
+  return aa;
+}
+
+template <typename T>
+SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromAngleAxis(
+    SphericalOf<T> aa) {
+  Vector3 vectorAxis = aa.direction.ToVector3();
+  Quaternion q = Quaternion::AngleAxis(aa.distance, vectorAxis);
+  return SwingTwistOf<T>();
+}
+
 template <typename T>
 const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();
 

SwingTwist.h

@@ -29,6 +29,9 @@ class SwingTwistOf {
   Quaternion ToQuaternion() const;
   static SwingTwistOf<T> FromQuaternion(Quaternion q);
 
+  SphericalOf<T> ToAngleAxis() const;
+  static SwingTwistOf<T> FromAngleAxis(SphericalOf<T> aa);
+
   const static SwingTwistOf<T> identity;
 
   /// <summary>

Vector3.cpp

@@ -153,7 +153,7 @@ float Vector3::Dot(const Vector3& v1, const Vector3& v2) {
   return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
 }
 
-bool Vector3::operator==(const Vector3& v) {
+bool Vector3::operator==(const Vector3& v) const {
   return (this->x == v.x && this->y == v.y && this->z == v.z);
 }
 

Vector3.h

@@ -90,7 +90,7 @@ struct Vector3 : Vec3 {
   /// @return true if it is identical to the given vector
   /// @note This uses float comparison to check equality which may have strange
   /// effects. Equality on floats should be avoided.
-  bool operator==(const Vector3& v);
+  bool operator==(const Vector3& v) const;
 
   /// @brief The vector length
   /// @param v The vector for which you need the length

test/Spherical16_test.cc

@@ -33,6 +33,18 @@ TEST(Spherical16, FromVector3) {
   EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0";
 }
 
+TEST(Spherical16, Vector3) {
+  Vector3 v = Vector3(1, 2, 3);
+  Spherical16 rd = Spherical16::FromVector3(v);
+  Vector3 rv = rd.ToVector3();
+  EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical";
+
+  v = Vector3(1, 2, -3);
+  rd = Spherical16::FromVector3(v);
+  rv = rd.ToVector3();
+  EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical";
+}
+
 // TEST(Spherical16, FromPolar) {
 //   Polar p = Polar(1, 0);
 //   Spherical16 s = Spherical16::FromPolar(p);

test/SphericalSingle_test.cc

@@ -7,32 +7,32 @@
 
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
-TEST(Spherical, FromVector3) {
+TEST(SphericalSingle, FromVector3) {
   Vector3 v = Vector3(0, 0, 1);
-  Spherical s = Spherical::FromVector3(v);
+  SphericalSingle s = SphericalSingle ::FromVector3(v);
 
   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
   EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 0 1";
   EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 0 0 1";
 
   v = Vector3(0, 1, 0);
-  s = Spherical::FromVector3(v);
+  s = SphericalSingle ::FromVector3(v);
 
   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
   EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0";
   EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0";
 
   v = Vector3(1, 0, 0);
-  s = Spherical::FromVector3(v);
+  s = SphericalSingle ::FromVector3(v);
 
   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
   EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0";
   EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0";
 }
 
-TEST(Spherical, FromPolar) {
+TEST(SphericalSingle, FromPolar) {
   Polar p = Polar(1, Angle::Degrees(0));
-  Spherical s = Spherical::FromPolar(p);
+  SphericalSingle s = SphericalSingle ::FromPolar(p);
 
   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)";
   EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
@@ -41,7 +41,7 @@ TEST(Spherical, FromPolar) {
       << "s.vert Polar(1 0)";
 
   p = Polar(1, Angle::Degrees(45));
-  s = Spherical::FromPolar(p);
+  s = SphericalSingle ::FromPolar(p);
 
   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)";
   EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 45.0F)
@@ -50,7 +50,7 @@ TEST(Spherical, FromPolar) {
       << "s.vert Polar(1 45)";
 
   p = Polar(1, Angle::Degrees(-45));
-  s = Spherical::FromPolar(p);
+  s = SphericalSingle ::FromPolar(p);
 
   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)";
   EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -45.0F)
@@ -59,7 +59,7 @@ TEST(Spherical, FromPolar) {
       << "s.vert Polar(1 -45)";
 
   p = Polar(0, Angle::Degrees(0));
-  s = Spherical::FromPolar(p);
+  s = SphericalSingle ::FromPolar(p);
 
   EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)";
   EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
@@ -68,7 +68,7 @@ TEST(Spherical, FromPolar) {
       << "s.vert Polar(0 0)";
 
   p = Polar(-1, Angle::Degrees(0));
-  s = Spherical::FromPolar(p);
+  s = SphericalSingle ::FromPolar(p);
 
   EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)";
   EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -180.0F)
@@ -77,12 +77,12 @@ TEST(Spherical, FromPolar) {
       << "s.vert Polar(-1 0)";
 }
 
-TEST(Spherical, Incident1) {
+TEST(SphericalSingle, Incident1) {
   Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
-  Spherical s = Spherical::FromVector3(v);
+  SphericalSingle s = SphericalSingle ::FromVector3(v);
 
-  Spherical sr =
+  SphericalSingle sr =
-      Spherical(2.49F, Angle::Degrees(98.18f), Angle::Degrees(24.4F));
+      SphericalSingle(2.49F, Angle::Degrees(98.18f), Angle::Degrees(24.4F));
   EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
   EXPECT_NEAR(s.direction.horizontal.InDegrees(),
               sr.direction.horizontal.InDegrees(), 1.0e-02);
@@ -95,12 +95,12 @@ TEST(Spherical, Incident1) {
   EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0";
 }
 
-TEST(Spherical, Incident2) {
+TEST(SphericalSingle, Incident2) {
   Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
-  Spherical s = Spherical::FromVector3(v);
+  SphericalSingle s = SphericalSingle ::FromVector3(v);
 
-  Spherical sr =
+  SphericalSingle sr = SphericalSingle(1.4142135623F, Angle::Degrees(45.0f),
-      Spherical(1.4142135623F, Angle::Degrees(45.0f), Angle::Degrees(0.0F));
+                                       Angle::Degrees(0.0F));
   EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
   EXPECT_NEAR(s.direction.horizontal.InDegrees(),
               sr.direction.horizontal.InDegrees(), 1.0e-05);
@@ -113,9 +113,10 @@ TEST(Spherical, Incident2) {
   EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
 
   v = Vector3(0.0f, 1.0f, 1.0f);
-  s = Spherical::FromVector3(v);
+  s = SphericalSingle ::FromVector3(v);
 
-  sr = Spherical(1.4142135623F, Angle::Degrees(0.0f), Angle::Degrees(45.0F));
+  sr = SphericalSingle(1.4142135623F, Angle::Degrees(0.0f),
+                       Angle::Degrees(45.0F));
   EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
   EXPECT_NEAR(s.direction.horizontal.InDegrees(),
               sr.direction.horizontal.InDegrees(), 1.0e-05);
@@ -128,7 +129,7 @@ TEST(Spherical, Incident2) {
   EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
 
   v = Vector3(1.0f, 1.0f, 1.0f);
-  s = Spherical::FromVector3(v);
+  s = SphericalSingle ::FromVector3(v);
   r = Vector3(s);
 
   EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
@@ -139,17 +140,19 @@ TEST(Spherical, Incident2) {
   EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
   EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
 
-  // s = Spherical(10, 45, 45);
+  // s = SphericalSingle
+  (10, 45, 45);
   // r = s.ToVector3();
   // EXPECT_NEAR(r.x, 5, 1.0e-06);
   // EXPECT_NEAR(r.y, 7.07, 1.0e-06);
   // EXPECT_NEAR(r.z, 5, 1.0e-06);
 }
 
-TEST(Spherical, Addition) {
+TEST(SphericalSingle, Addition) {
-  Spherical v1 = Spherical(1, Angle::Degrees(45), Angle::Degrees(0));
+  SphericalSingle v1 =
-  Spherical v2 = Spherical::zero;
+      SphericalSingle(1, Angle::Degrees(45), Angle::Degrees(0));
-  Spherical r = Spherical::zero;
+  SphericalSingle v2 = SphericalSingle ::zero;
+  SphericalSingle r = SphericalSingle ::zero;
 
   r = v1 + v2;
   EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
@@ -158,13 +161,13 @@ TEST(Spherical, Addition) {
   r += v2;
   EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
 
-  v2 = Spherical(1, Angle::Degrees(-45), Angle::Degrees(0));
+  v2 = SphericalSingle(1, Angle::Degrees(-45), Angle::Degrees(0));
   r = v1 + v2;
   EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)";
   EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)";
   EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)";
 
-  v2 = Spherical(1, Angle::Degrees(0), Angle::Degrees(90));
+  v2 = SphericalSingle(1, Angle::Degrees(0), Angle::Degrees(90));
   r = v1 + v2;
   EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)";
   EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)";

test/SwingTwistSingle_test.cc

@@ -0,0 +1,46 @@
+#if GTEST
+#include <gtest/gtest.h>
+#include <math.h>
+#include <limits>
+
+#include "SwingTwist.h"
+
+#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
+
+TEST(SwingTwistSingle, Quaternion) {
+  Quaternion q;
+  SwingTwistSingle s;
+  Quaternion rq;
+
+  q = Quaternion::identity;
+  s = SwingTwistSingle::FromQuaternion(q);
+  rq = s.ToQuaternion();
+  EXPECT_EQ(q, rq) << " 0 0 0 1 <-> SwingTwist";
+
+  q = Quaternion::Euler(90, 0, 0);
+  s = SwingTwistSingle::FromQuaternion(q);
+  rq = s.ToQuaternion();
+  EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 90 0 0 <-> SwingTwist";
+
+  q = Quaternion::Euler(0, 90, 0);
+  s = SwingTwistSingle::FromQuaternion(q);
+  rq = s.ToQuaternion();
+  EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
+
+  q = Quaternion::Euler(0, 0, 90);
+  s = SwingTwistSingle::FromQuaternion(q);
+  rq = s.ToQuaternion();
+  EXPECT_EQ(q, rq) << " Euler 0 0 90 <-> SwingTwist";
+
+  q = Quaternion::Euler(0, 180, 0);  // ==> spherical S(180 0)T0
+  s = SwingTwistSingle::FromQuaternion(q);
+  rq = s.ToQuaternion();
+  EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
+
+  q = Quaternion::Euler(0, 135, 0);  // ==> spherical S(180 45)T0
+  s = SwingTwistSingle::FromQuaternion(q);
+  rq = s.ToQuaternion();
+  EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
+}
+
+#endif