Normalized swing twist

Angle.cpp

@@ -208,12 +208,32 @@ AngleOf<T> AngleOf<T>::operator+(const AngleOf<T>& a) const {
   return angle;
 }
 
+template <>
+AngleOf<float> AngleOf<float>::operator+=(const AngleOf<float>& a) {
+  this->value += a.value;
+  this->Normalize();
+  return *this;
+}
+
 template <typename T>
 AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& a) {
   this->value += a.value;
   return *this;
 }
 
+template <typename T>
+void AngleOf<T>::Normalize() {
+  float angleValue = this->InDegrees();
+  if (!isfinite(angleValue))
+    return;
+
+  while (angleValue <= -180)
+    angleValue += 360;
+  while (angleValue > 180)
+    angleValue -= 360;
+  *this = AngleOf::Degrees(angleValue);
+}
+
 template <typename T>
 AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
   float angleValue = angle.InDegrees();

Angle.h

@@ -51,6 +51,7 @@ class AngleOf {
     return AngleOf::Degrees((float)f * a.InDegrees());
   }
 
+  void Normalize();
   static AngleOf<T> Normalize(AngleOf<T> a);
   static AngleOf<T> Clamp(AngleOf<T> a, AngleOf<T> min, AngleOf<T> max);
   // static AngleOf<T> Difference(AngleOf<T> a, AngleOf<T> b) {

Direction.cpp

@@ -22,18 +22,6 @@ DirectionOf<T>::DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical) {
   Normalize();
 };
 
-// template <typename T>
-// DirectionOf<T>::DirectionOf(Vector3 v) {
-//   this->horizontal = AngleOf<T>::Atan2(
-//       v.Right(),
-//       v.Forward());  // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
-//   this->vertical =
-//       -AngleOf<T>::deg90 -
-//       AngleOf<T>::Acos(
-//           v.Up());  // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
-//   Normalize();
-// }
-
 template <typename T>
 const DirectionOf<T> DirectionOf<T>::forward =
     DirectionOf<T>(AngleOf<T>(), AngleOf<T>());

SwingTwist.cpp

@@ -12,6 +12,16 @@ SwingTwistOf<T>::SwingTwistOf() {
 
 template <typename T>
 SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist) {
+  // Normalize angles
+  AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
+  AngleOf<T> deg180 = AngleOf<T>::Degrees(180);
+
+  if (swing.vertical > deg90 || swing.vertical < -deg90) {
+    swing.horizontal += deg180;
+    swing.vertical = deg180 - swing.vertical;
+    twist += deg180;
+  }
+
   this->swing = swing;
   this->twist = twist;
 }
@@ -20,8 +30,16 @@ template <typename T>
 SwingTwistOf<T>::SwingTwistOf(AngleOf<T> horizontal,
                               AngleOf<T> vertical,
                               AngleOf<T> twist) {
-  // this->horizontal = horizontal;
-  // this->vertical = vertical;
+  // Normalize angles
+  AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
+  AngleOf<T> deg180 = AngleOf<T>::Degrees(180);
+
+  if (vertical > deg90 || vertical < -deg90) {
+    horizontal += deg180;
+    vertical = deg180 - vertical;
+    twist += deg180;
+  }
+
   this->swing = DirectionOf<T>(horizontal, vertical);
   this->twist = twist;
 }
@@ -30,9 +48,12 @@ template <typename T>
 SwingTwistOf<T> SwingTwistOf<T>::Degrees(float horizontal,
                                          float vertical,
                                          float twist) {
-  DirectionOf<T> swing = DirectionOf<T>::Degrees(horizontal, vertical);
-  AngleOf<T> twistAngle = AngleOf<T>::Degrees(twist);
-  SwingTwistOf<T> orientation = SwingTwistOf(swing, twistAngle);
+  SwingTwistOf<T> orientation = SwingTwistOf<T>(AngleOf<T>::Degrees(horizontal),
+                                                AngleOf<T>::Degrees(vertical),
+                                                AngleOf<T>::Degrees(twist));
+  // DirectionOf<T> swing = DirectionOf<T>::Degrees(horizontal, vertical);
+  // AngleOf<T> twistAngle = AngleOf<T>::Degrees(twist);
+  // SwingTwistOf<T> orientation = SwingTwistOf(swing, twistAngle);
   return orientation;
 }
 
@@ -49,7 +70,7 @@ SwingTwistOf<T> SwingTwistOf<T>::FromQuaternion(Quaternion q) {
   Vector3 angles = Quaternion::ToAngles(q);
   SwingTwistOf<T> r =
       SwingTwistOf<T>::Degrees(angles.Up(), angles.Right(), angles.Forward());
-
+  r.Normalize();
   return r;
 }
 
@@ -131,5 +152,17 @@ AngleOf<T> SwingTwistOf<T>::Angle(const SwingTwistOf<T>& r1,
   return AngleOf<T>::Degrees(angle);
 }
 
+template <typename T>
+void SwingTwistOf<T>::Normalize() {
+  AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
+  AngleOf<T> deg180 = AngleOf<T>::Degrees(180);
+
+  if (this->swing.vertical > deg90 || this->swing.vertical < -deg90) {
+    this->swing.horizontal += deg180;
+    this->swing.vertical = deg180 - this->swing.vertical;
+    this->twist += deg180;
+  }
+}
+
 template class SwingTwistOf<float>;
 template class SwingTwistOf<signed short>;

SwingTwist.h

@@ -64,6 +64,8 @@ class SwingTwistOf {
   static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T>& axis);
 
   static AngleOf<T> Angle(const SwingTwistOf<T>& r1, const SwingTwistOf<T>& r2);
+
+  void Normalize();
 };
 
 using SwingTwistSingle = SwingTwistOf<float>;

test/SwingTwistSingle_test.cc

@@ -100,4 +100,32 @@ TEST(SwingTwistSingle, AngleAxis) {
       << "270 up";
 }
 
+TEST(SwingTwistSingle, Normalize) {
+  SwingTwistSingle s;
+
+  s = SwingTwistSingle::Degrees(0, 0, 0);
+  EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 0 0 Normalized";
+
+  s = SwingTwistSingle::Degrees(0, 180, 0);
+  EXPECT_EQ(s, SwingTwistSingle::Degrees(180, 0, 180)) << "0 180 0 Normalized";
+
+  s = SwingTwistSingle::Degrees(0, 180, 180);
+  EXPECT_EQ(s, SwingTwistSingle::Degrees(180, 0, 0)) << "0 180 180 Normalized";
+
+  s = SwingTwistSingle::Degrees(270, 90, 0);
+  EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, 90, 0)) << "270 90 0 Normalized";
+
+  s = SwingTwistSingle::Degrees(270, 270, 0);
+  EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, -90, 0))
+      << "270 270 0 Normalized";
+
+  s = SwingTwistSingle::Degrees(270, 225, 0);
+  EXPECT_EQ(s, SwingTwistSingle::Degrees(90, -45, -180))
+      << "270 225 0 Normalized";
+
+  s = SwingTwistSingle::Degrees(270, 0, 225);
+  EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, 0, -135))
+      << "270 0 225 Normalized";
+}
+
 #endif