Normalizing directions

Angle.cpp

@@ -112,7 +112,7 @@ float AngleOf<T>::InRadians() const {
 // AngleOf<float> AngleOf<float>::Deg2Rad = (pi * 2) / 360.0f;
 
 template <typename T>
-bool AngleOf<T>::operator==(AngleOf<T> a) {
+bool AngleOf<T>::operator==(const AngleOf<T> a) const {
   return this->value == a.value;
 }
 

Angle.h

@@ -33,7 +33,7 @@ class AngleOf {
 
   // static AngleOf<T> pi;
 
-  bool operator==(AngleOf<T> a);
+  bool operator==(const AngleOf<T> a) const;
   bool operator>(AngleOf<T> a);
   bool operator>=(AngleOf<T> a);
   bool operator<(AngleOf<T> a);

CMakeLists.txt

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

Direction.cpp

@@ -19,6 +19,7 @@ template <typename T>
 DirectionOf<T>::DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical) {
   this->horizontal = horizontal;
   this->vertical = vertical;
+  Normalize();
 };
 
 template <typename T>
@@ -26,6 +27,7 @@ DirectionOf<T>::DirectionOf(Vector3 v) {
   this->horizontal =
       atan2f(v.Right(), v.Forward()) * Passer::LinearAlgebra::Rad2Deg;
   this->vertical = 90 - acosf(v.Up()) * Passer::LinearAlgebra::Rad2Deg;
+  Normalize();
 }
 
 template <typename T>
@@ -41,28 +43,33 @@ const DirectionOf<T> DirectionOf<T>::left = DirectionOf<T>(-90.0f, 0.0f);
 template <typename T>
 const DirectionOf<T> DirectionOf<T>::right = DirectionOf<T>(90.0f, 0.0f);
 
+template <typename T>
+DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Degrees(float horizontal,
+                                                              float vertical) {
+  return DirectionOf<T>(AngleOf<T>::Degrees(horizontal),
+                        AngleOf<T>::Degrees(vertical));
+}
+
+template <typename T>
+DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Radians(float horizontal,
+                                                              float vertical) {
+  return DirectionOf<T>(AngleOf<T>::Radians(horizontal),
+                        AngleOf<T>::Radians(vertical));
+}
+
+template <typename T>
+bool Passer::LinearAlgebra::DirectionOf<T>::operator==(
+    const DirectionOf<T> d) const {
+  return (this->horizontal == d.horizontal) && (this->vertical == d.vertical);
+}
+
 template <typename T>
 DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator-() const {
-  DirectionOf<T> r = DirectionOf<T>(-this->horizontal, -this->vertical);
+  DirectionOf<T> r = DirectionOf<T>(this->horizontal + AngleOf<T>::Degrees(180),
+                                    -this->vertical);
   return r;
 }
 
-template <typename T>
-DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator+(
-    const DirectionOf<T>& v) const {
-  DirectionOf<T> r = DirectionOf<T>(this->horizontal + v.horizontal,
-                                    this->vertical + v.vertical);
-  return r;
-}
-
-template <typename T>
-DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator+=(
-    const DirectionOf<T>& v) {
-  this->horizontal += v.horizontal;
-  this->vertical += v.vertical;
-  return *this;
-}
-
 template <typename T>
 Vector3 DirectionOf<T>::ToVector3() {
   Vector3 v = Quaternion::Euler(-(this->vertical.InDegrees()),
@@ -71,5 +78,14 @@ Vector3 DirectionOf<T>::ToVector3() {
   return v;
 }
 
+template <typename T>
+void DirectionOf<T>::Normalize() {
+  if (this->vertical > AngleOf<T>::Degrees(90) ||
+      this->vertical < AngleOf<T>::Degrees(-90)) {
+    this->horizontal += AngleOf<T>::Degrees(180);
+    this->vertical = AngleOf<T>::Degrees(180) - this->vertical;
+  }
+}
+
 template class DirectionOf<float>;
 template class DirectionOf<signed short>;

Direction.h

@@ -15,13 +15,18 @@ struct Vector3;
 template <typename T>
 class DirectionOf {
  public:
+  /// @brief horizontal angle, range= (-180..180]
   AngleOf<T> horizontal;
+  /// @brief vertical angle, range in degrees = (-90..90]
   AngleOf<T> vertical;
 
   DirectionOf<T>();
   DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
   DirectionOf<T>(Vector3 v);
 
+  static DirectionOf<T> Degrees(float horizontal, float vertical);
+  static DirectionOf<T> Radians(float horizontal, float vertical);
+
   const static DirectionOf forward;
   const static DirectionOf back;
   const static DirectionOf up;
@@ -29,11 +34,14 @@ class DirectionOf {
   const static DirectionOf left;
   const static DirectionOf right;
 
+  bool operator==(const DirectionOf<T> d) const;
+
   DirectionOf<T> operator-() const;
-  DirectionOf<T> operator+(const DirectionOf<T>& v) const;
-  DirectionOf<T> operator+=(const DirectionOf<T>& v);
 
   Vector3 ToVector3();
+
+ protected:
+  void Normalize();
 };
 
 using DirectionSingle = DirectionOf<float>;

SwingTwist.cpp

@@ -79,9 +79,8 @@ SwingTwistOf<T> SwingTwistOf<T>::operator*(
 
 template <typename T>
 SwingTwistOf<T> SwingTwistOf<T>::operator*=(const SwingTwistOf<T>& rotation) {
-  // this->swing.horizontal += rotation.swing.horizontal;
+  this->swing.horizontal += rotation.swing.horizontal;
-  // this->swing.vertical += rotation.swing.vertical;
+  this->swing.vertical += rotation.swing.vertical;
-  this->swing += rotation.swing;
   this->twist += rotation.twist;
   return *this;
 }

test/Direction_test.cc

@@ -0,0 +1,56 @@
+#if GTEST
+#include <gtest/gtest.h>
+
+#include <math.h>
+#include <limits>
+
+#include "Direction.h"
+
+#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
+
+TEST(Direction16, Compare) {
+  Direction16 d = Direction16::Degrees(45, 135);
+  bool r;
+  r = (d == Direction16(Angle16::Degrees(45), Angle16::Degrees(135)));
+  EXPECT_TRUE(r) << "45,135 == 45, 135";
+
+  r = (d ==
+       Direction16(Angle16::Degrees(45 + 360), Angle16::Degrees(135 - 360)));
+  EXPECT_TRUE(r) << "45+360, 135-360 == 45, 135";
+}
+
+TEST(Direction16, Inverse) {
+  Direction16 d;
+  Direction16 r;
+
+  d = Direction16::Degrees(45, 135);
+  r = -d;
+  EXPECT_EQ(r, Direction16::Degrees(-135, -135)) << "-(45, 135)";
+
+  d = Direction16::Degrees(-45, -135);
+  r = -d;
+  EXPECT_EQ(r, Direction16::Degrees(135, 135)) << "-(-45, -135)";
+
+  d = Direction16::Degrees(0, 0);
+  r = -d;
+  EXPECT_EQ(r, Direction16::Degrees(180, 0)) << "-(0, 0)";
+
+  d = Direction16::Degrees(0, 45);
+  r = -d;
+  EXPECT_EQ(r, Direction16::Degrees(180, -45)) << "-(0, 45)";
+}
+
+TEST(Direction16, Equality) {
+  Direction16 d;
+  d = Direction16::Degrees(135, 45);
+  EXPECT_EQ(d, Direction16::Degrees(135, 45)) << "(135, 45) == (135, 45)";
+  EXPECT_EQ(d, Direction16::Degrees(135 + 360, 45))
+      << "(135, 45) == (135 + 360, 45) ";
+  EXPECT_EQ(d, Direction16::Degrees(135 - 360, 45))
+      << "(135, 135) == (135 - 360, 45) ";
+
+  d = Direction16::Degrees(0, 45 + 180);
+  EXPECT_EQ(d, Direction16::Degrees(180, -45)) << "(0, 45+180) == (180, -45)";
+}
+
+#endif