diff --git a/Angle.cpp b/Angle.cpp
index 00ea7cb..4588736 100644
--- a/Angle.cpp
+++ b/Angle.cpp
@@ -142,22 +142,22 @@ bool AngleOf<T>::operator==(const AngleOf<T> a) const {
 }
 
 template <typename T>
-bool AngleOf<T>::operator>(AngleOf<T> a) {
+bool AngleOf<T>::operator>(AngleOf<T> a) const {
   return this->value > a.value;
 }
 
 template <typename T>
-bool AngleOf<T>::operator>=(AngleOf<T> a) {
+bool AngleOf<T>::operator>=(AngleOf<T> a) const {
   return this->value >= a.value;
 }
 
 template <typename T>
-bool AngleOf<T>::operator<(AngleOf<T> a) {
+bool AngleOf<T>::operator<(AngleOf<T> a) const {
   return this->value < a.value;
 }
 
 template <typename T>
-bool AngleOf<T>::operator<=(AngleOf<T> a) {
+bool AngleOf<T>::operator<=(AngleOf<T> a) const {
   return this->value <= a.value;
 }
 
diff --git a/Angle.h b/Angle.h
index 384c66f..8b02505 100644
--- a/Angle.h
+++ b/Angle.h
@@ -31,10 +31,10 @@ class AngleOf {
   inline T GetBinary() const { return this->value; }
 
   bool operator==(const AngleOf<T> a) const;
-  bool operator>(AngleOf<T> a);
-  bool operator>=(AngleOf<T> a);
-  bool operator<(AngleOf<T> a);
-  bool operator<=(AngleOf<T> a);
+  bool operator>(AngleOf<T> a) const;
+  bool operator>=(AngleOf<T> a) const;
+  bool operator<(AngleOf<T> a) const;
+  bool operator<=(AngleOf<T> a) const;
 
   static signed int Sign(AngleOf<T> a);
   static AngleOf<T> Abs(AngleOf<T> a);
diff --git a/Direction.h b/Direction.h
index 5b5d0fb..79ca814 100644
--- a/Direction.h
+++ b/Direction.h
@@ -22,7 +22,6 @@ class DirectionOf {
 
   DirectionOf<T>();
   DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
-  // DirectionOf<T>(Vector3 v);
 
   Vector3 ToVector3() const;
   static DirectionOf<T> FromVector3(Vector3 v);
diff --git a/SwingTwist.cpp b/SwingTwist.cpp
index 0971b78..459efee 100644
--- a/SwingTwist.cpp
+++ b/SwingTwist.cpp
@@ -11,8 +11,7 @@ SwingTwistOf<T>::SwingTwistOf() {
 }
 
 template <typename T>
-Passer::LinearAlgebra::SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing,
-                                                     AngleOf<T> twist) {
+SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist) {
   this->swing = swing;
   this->twist = twist;
 }
@@ -46,8 +45,7 @@ Quaternion SwingTwistOf<T>::ToQuaternion() const {
 }
 
 template <typename T>
-SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion(
-    Quaternion q) {
+SwingTwistOf<T> SwingTwistOf<T>::FromQuaternion(Quaternion q) {
   Vector3 angles = Quaternion::ToAngles(q);
   SwingTwistOf<T> r =
       SwingTwistOf<T>::Degrees(angles.Up(), angles.Right(), angles.Forward());
@@ -56,7 +54,7 @@ SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion(
 }
 
 template <typename T>
-SphericalOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::ToAngleAxis() const {
+SphericalOf<T> SwingTwistOf<T>::ToAngleAxis() const {
   Quaternion q = this->ToQuaternion();
   float angle;
   Vector3 axis;
@@ -68,13 +66,17 @@ SphericalOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::ToAngleAxis() const {
 }
 
 template <typename T>
-SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromAngleAxis(
-    SphericalOf<T> aa) {
+SwingTwistOf<T> SwingTwistOf<T>::FromAngleAxis(SphericalOf<T> aa) {
   Vector3 vectorAxis = aa.direction.ToVector3();
   Quaternion q = Quaternion::AngleAxis(aa.distance, vectorAxis);
   return SwingTwistOf<T>();
 }
 
+template <typename T>
+bool SwingTwistOf<T>::operator==(const SwingTwistOf<T> s) const {
+  return (this->swing == s.swing) && (this->twist == s.twist);
+}
+
 template <typename T>
 const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();
 
@@ -120,5 +122,14 @@ SwingTwistOf<T> SwingTwistOf<T>::AngleAxis(float angle,
   return r;
 }
 
+template <typename T>
+AngleOf<T> SwingTwistOf<T>::Angle(const SwingTwistOf<T>& r1,
+                                  const SwingTwistOf<T>& r2) {
+  Quaternion q1 = r1.ToQuaternion();
+  Quaternion q2 = r2.ToQuaternion();
+  float angle = Quaternion::Angle(q1, q2);
+  return AngleOf<T>::Degrees(angle);
+}
+
 template class SwingTwistOf<float>;
 template class SwingTwistOf<signed short>;
\ No newline at end of file
diff --git a/SwingTwist.h b/SwingTwist.h
index d5e27a0..0558bbb 100644
--- a/SwingTwist.h
+++ b/SwingTwist.h
@@ -26,6 +26,7 @@ class SwingTwistOf {
   static SwingTwistOf<T> Degrees(float horizontal,
                                  float vertical = 0,
                                  float twist = 0);
+
   Quaternion ToQuaternion() const;
   static SwingTwistOf<T> FromQuaternion(Quaternion q);
 
@@ -34,6 +35,8 @@ class SwingTwistOf {
 
   const static SwingTwistOf<T> identity;
 
+  bool operator==(const SwingTwistOf<T> d) const;
+
   /// <summary>
   /// Rotate a vector using this rotation
   /// </summary>
@@ -59,6 +62,8 @@ class SwingTwistOf {
   /// <param name="axis">The axis</param>
   /// <returns>The resulting quaternion</returns>
   static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T>& axis);
+
+  static AngleOf<T> Angle(const SwingTwistOf<T>& r1, const SwingTwistOf<T>& r2);
 };
 
 using SwingTwistSingle = SwingTwistOf<float>;
diff --git a/test/Angle_test.cc b/test/Angle_test.cc
deleted file mode 100644
index 6cda76e..0000000
--- a/test/Angle_test.cc
+++ /dev/null
@@ -1,224 +0,0 @@
-/*
-#if GTEST
-#include <gtest/gtest.h>
-
-#include <math.h>
-#include <limits>
-
-#include "Angle.h"
-
-#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
-
-TEST(Angle16, Compare) {
-  Angle16 a = Angle16::Degrees(45);
-  bool r = false;
-
-  r = a > Angle16::Degrees(0);
-  EXPECT_TRUE(r) << "45 > 0";
-
-  r = a > Angle16::Degrees(90);
-  EXPECT_FALSE(r) << "45 > 90";
-
-  r = a > Angle16::Degrees(-90);
-  EXPECT_TRUE(r) << "45 > -90";
-}
-
-TEST(AngleSingle, Compare) {
-  AngleSingle a = AngleSingle::Degrees(45);
-  bool r = false;
-
-  r = a > AngleSingle::Degrees(0);
-  EXPECT_TRUE(r) << "45 > 0";
-
-  r = a > AngleSingle::Degrees(90);
-  EXPECT_FALSE(r) << "45 > 90";
-
-  r = a > AngleSingle::Degrees(-90);
-  EXPECT_TRUE(r) << "45 > -90";
-}
-
-TEST(Angle, Normalize) {
-  Angle r = AngleSingle();
-
-  r = Angle::Normalize(AngleSingle::Degrees(90.0f));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
-
-  r = Angle::Normalize(AngleSingle::Degrees(-90));
-  EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
-
-  r = Angle::Normalize(AngleSingle::Degrees(270));
-  EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
-
-  r = Angle::Normalize(AngleSingle::Degrees(270 + 360));
-  EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
-
-  r = Angle::Normalize(AngleSingle::Degrees(-270));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
-
-  r = Angle::Normalize(AngleSingle::Degrees(-270 - 360));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
-
-  r = Angle::Normalize(AngleSingle::Degrees(0));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
-
-  if (std::numeric_limits<float>::is_iec559) {
-    r = Angle::Normalize(AngleSingle::Degrees(FLOAT_INFINITY));
-    EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
-
-    r = Angle::Normalize(AngleSingle::Degrees(-FLOAT_INFINITY));
-    EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
-  }
-}
-
-TEST(Angle, Clamp) {
-  Angle r = AngleSingle();
-
-  r = Angle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
-                   AngleSingle::Degrees(2));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 2";
-
-  r = Angle::Clamp(AngleSingle::Degrees(-1), AngleSingle::Degrees(0),
-                   AngleSingle::Degrees(2));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -1 0 2";
-
-  r = Angle::Clamp(AngleSingle::Degrees(3), AngleSingle::Degrees(0),
-                   AngleSingle::Degrees(2));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 2) << "Clamp 3 0 2";
-
-  r = Angle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
-                   AngleSingle::Degrees(0));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 1 0 0";
-
-  r = Angle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
-                   AngleSingle::Degrees(0));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
-
-  r = Angle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(1),
-                   AngleSingle::Degrees(-1));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 0 1 -1";
-
-  if (std::numeric_limits<float>::is_iec559) {
-    r = Angle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
-                     AngleSingle::Degrees(FLOAT_INFINITY));
-    EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 INFINITY";
-
-    r = Angle::Clamp(AngleSingle::Degrees(1),
-                     AngleSingle::Degrees(-FLOAT_INFINITY),
-                     AngleSingle::Degrees(1));
-    EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 -INFINITY 1";
-  }
-}
-
-// TEST(Angle, Difference) {
-//   Angle r = 0;
-
-//   r = Angle::Difference(0, 90);
-//   EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
-
-//   r = Angle::Difference(0, -90);
-//   EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
-
-//   r = Angle::Difference(0, 270);
-//   EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
-
-//   r = Angle::Difference(0, -270);
-//   EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
-
-//   r = Angle::Difference(90, 0);
-//   EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
-
-//   r = Angle::Difference(-90, 0);
-//   EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
-
-//   r = Angle::Difference(0, 0);
-//   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
-
-//   r = Angle::Difference(90, 90);
-//   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
-
-//   if (std::numeric_limits<float>::is_iec559) {
-//     r = Angle::Difference(0, INFINITY);
-//     EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
-
-//     r = Angle::Difference(0, -INFINITY);
-//     EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
-
-//     r = Angle::Difference(-INFINITY, INFINITY);
-//     EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
-//     INFINITY";
-//   }
-// }
-
-TEST(Angle, MoveTowards) {
-  Angle r = Angle();
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
-                         Angle::Degrees(30));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 90 30";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
-                         Angle::Degrees(90));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
-                         Angle::Degrees(180));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 180";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
-                         Angle::Degrees(270));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
-                         Angle::Degrees(-30));
-  EXPECT_FLOAT_EQ(r.InDegrees(), -30) << "MoveTowards 0 90 -30";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
-                         Angle::Degrees(-30));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -90 -30";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
-                         Angle::Degrees(-90));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -90";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
-                         Angle::Degrees(-180));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 180) << "MoveTowards 0 -90 -180";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
-                         Angle::Degrees(-270));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 270) << "MoveTowards 0 -90 -270";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
-                         Angle::Degrees(0));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(0),
-                         Angle::Degrees(0));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
-
-  r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(0),
-                         Angle::Degrees(30));
-  EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
-
-  if (std::numeric_limits<float>::is_iec559) {
-    r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
-                           Angle::Degrees(FLOAT_INFINITY));
-    EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
-
-    r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(FLOAT_INFINITY),
-                           Angle::Degrees(30));
-    EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
-
-    r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
-                           Angle::Degrees(-FLOAT_INFINITY));
-    EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY)
-        << "MoveTowards 0 -90 -FLOAT_INFINITY";
-
-    r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-FLOAT_INFINITY),
-                           Angle::Degrees(-30));
-    EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
-  }
-}
-
-#endif
-*/
\ No newline at end of file
diff --git a/test/SwingTwistSingle_test.cc b/test/SwingTwistSingle_test.cc
index 49040e3..5ed238b 100644
--- a/test/SwingTwistSingle_test.cc
+++ b/test/SwingTwistSingle_test.cc
@@ -43,4 +43,56 @@ TEST(SwingTwistSingle, Quaternion) {
   EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
 }
 
+TEST(SwingTwistSingle, AngleAxis) {
+  SwingTwistSingle s;
+  SwingTwistSingle r;
+
+  s = SwingTwistSingle::AngleAxis(0, DirectionSingle::up);
+  EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 up";
+
+  r = SwingTwistSingle::AngleAxis(90, DirectionSingle::up);
+  s = SwingTwistSingle::Degrees(90, 0, 0);
+  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
+      << "90 up";
+
+  r = SwingTwistSingle::AngleAxis(180, DirectionSingle::up);
+  s = SwingTwistSingle::Degrees(180, 0, 0);
+  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
+      << "180 up";
+
+  r = SwingTwistSingle::AngleAxis(270, DirectionSingle::up);
+  s = SwingTwistSingle::Degrees(-90, 0, 0);
+  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
+      << "270 up";
+
+  r = SwingTwistSingle::AngleAxis(90, DirectionSingle::right);
+  s = SwingTwistSingle::Degrees(0, 90, 0);
+  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
+      << "90 right";
+
+  r = SwingTwistSingle::AngleAxis(180, DirectionSingle::right);
+  s = SwingTwistSingle::Degrees(0, 180, 0);
+  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
+      << "180 right";
+  r = SwingTwistSingle::AngleAxis(270, DirectionSingle::right);
+  s = SwingTwistSingle::Degrees(0, -90, 0);
+  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
+      << "270 right";
+
+  r = SwingTwistSingle::AngleAxis(90, DirectionSingle::forward);
+  s = SwingTwistSingle::Degrees(0, 0, 90);
+  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
+      << "90 up";
+
+  r = SwingTwistSingle::AngleAxis(180, DirectionSingle::forward);
+  s = SwingTwistSingle::Degrees(0, 0, 180);
+  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
+      << "180 up";
+
+  r = SwingTwistSingle::AngleAxis(270, DirectionSingle::forward);
+  s = SwingTwistSingle::Degrees(0, 0, -90);
+  EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
+      << "270 up";
+}
+
 #endif
\ No newline at end of file