Fix Matrix unit test

Angle.cpp

@@ -3,15 +3,15 @@
 // file, You can obtain one at https ://mozilla.org/MPL/2.0/.
 
 #include "Angle.h"
-#include "FloatSingle.h"
 #include <math.h>
+#include "FloatSingle.h"
 
-const float Rad2Deg = 57.29578F;
+namespace LinearAlgebra {
-const float Deg2Rad = 0.0174532924F;
 
 //===== AngleSingle, AngleOf<float>
 
-template <> AngleOf<float> Passer::LinearAlgebra::AngleOf<float>::Degrees(float degrees) {
+template <>
+AngleOf<float> AngleOf<float>::Degrees(float degrees) {
   if (isfinite(degrees)) {
     while (degrees < -180)
       degrees += 360;
@@ -22,7 +22,8 @@ template <> AngleOf<float> Passer::LinearAlgebra::AngleOf<float>::Degrees(float
   return AngleOf<float>(degrees);
 }
 
-template <> AngleOf<float> AngleOf<float>::Radians(float radians) {
+template <>
+AngleOf<float> AngleOf<float>::Radians(float radians) {
   if (isfinite(radians)) {
     while (radians <= -pi)
       radians += 2 * pi;
@@ -33,9 +34,13 @@ template <> AngleOf<float> AngleOf<float>::Radians(float radians) {
   return Binary(radians * Rad2Deg);
 }
 
-template <> float AngleOf<float>::InDegrees() const { return this->value; }
+template <>
+float AngleOf<float>::InDegrees() const {
+  return this->value;
+}
 
-template <> float AngleOf<float>::InRadians() const {
+template <>
+float AngleOf<float>::InRadians() const {
   return this->value * Deg2Rad;
 }
 
@@ -58,25 +63,29 @@ AngleOf<signed short> AngleOf<signed short>::Radians(float radians) {
   return Binary(value);
 }
 
-template <> float AngleOf<signed short>::InDegrees() const {
+template <>
+float AngleOf<signed short>::InDegrees() const {
   float degrees = this->value / 65536.0f * 360.0f;
   return degrees;
 }
 
-template <> float AngleOf<signed short>::InRadians() const {
+template <>
+float AngleOf<signed short>::InRadians() const {
   float radians = this->value / 65536.0f * (2 * pi);
   return radians;
 }
 
 //===== Angle8, AngleOf<signed char>
 
-template <> AngleOf<signed char> AngleOf<signed char>::Degrees(float degrees) {
+template <>
+AngleOf<signed char> AngleOf<signed char>::Degrees(float degrees) {
   // map float [-180..180) to integer [-128..127)
   signed char value = (signed char)roundf(degrees / 360.0F * 256.0F);
   return Binary(value);
 }
 
-template <> AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
+template <>
+AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
   if (!isfinite(radians))
     return AngleOf<signed char>::zero;
 
@@ -85,32 +94,42 @@ template <> AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
   return Binary(value);
 }
 
-template <> float AngleOf<signed char>::InDegrees() const {
+template <>
+float AngleOf<signed char>::InDegrees() const {
   float degrees = this->value / 256.0f * 360.0f;
   return degrees;
 }
 
-template <> float AngleOf<signed char>::InRadians() const {
+template <>
+float AngleOf<signed char>::InRadians() const {
   float radians = this->value / 128.0f * pi;
   return radians;
 }
 
 //===== Generic
 
-template <typename T> AngleOf<T>::AngleOf() : value(0) {}
+template <typename T>
+AngleOf<T>::AngleOf() : value(0) {}
 
-template <typename T> AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
+template <typename T>
+AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
 
-template <typename T> const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
+template <typename T>
+const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
 
-template <typename T> AngleOf<T> AngleOf<T>::Binary(T rawValue) {
+template <typename T>
+AngleOf<T> AngleOf<T>::Binary(T rawValue) {
   AngleOf<T> angle = AngleOf<T>();
   angle.SetBinary(rawValue);
   return angle;
 }
 
-template <typename T> T AngleOf<T>::GetBinary() const { return this->value; }
+template <typename T>
-template <typename T> void AngleOf<T>::SetBinary(T rawValue) {
+T AngleOf<T>::GetBinary() const {
+  return this->value;
+}
+template <typename T>
+void AngleOf<T>::SetBinary(T rawValue) {
   this->value = rawValue;
 }
 
@@ -119,24 +138,28 @@ bool AngleOf<T>::operator==(const AngleOf<T> angle) const {
   return this->value == angle.value;
 }
 
-template <typename T> bool AngleOf<T>::operator>(AngleOf<T> angle) const {
+template <typename T>
+bool AngleOf<T>::operator>(AngleOf<T> angle) const {
   return this->value > angle.value;
 }
 
-template <typename T> bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
+template <typename T>
+bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
   return this->value >= angle.value;
 }
 
-template <typename T> bool AngleOf<T>::operator<(AngleOf<T> angle) const {
+template <typename T>
+bool AngleOf<T>::operator<(AngleOf<T> angle) const {
   return this->value < angle.value;
 }
 
-template <typename T> bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
+template <typename T>
+bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
   return this->value <= angle.value;
 }
 
 template <typename T>
-signed int Passer::LinearAlgebra::AngleOf<T>::Sign(AngleOf<T> angle) {
+signed int AngleOf<T>::Sign(AngleOf<T> angle) {
   if (angle.value < 0)
     return -1;
   if (angle.value > 0)
@@ -145,51 +168,52 @@ signed int Passer::LinearAlgebra::AngleOf<T>::Sign(AngleOf<T> angle) {
 }
 
 template <typename T>
-AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Abs(AngleOf<T> angle) {
+AngleOf<T> AngleOf<T>::Abs(AngleOf<T> angle) {
   if (Sign(angle) < 0)
     return -angle;
   else
     return angle;
 }
 
-template <typename T> AngleOf<T> AngleOf<T>::operator-() const {
+template <typename T>
+AngleOf<T> AngleOf<T>::operator-() const {
   AngleOf<T> angle = Binary(-this->value);
   return angle;
 }
 
 template <>
-AngleOf<float> AngleOf<float>::operator-(const AngleOf<float> &angle) const {
+AngleOf<float> AngleOf<float>::operator-(const AngleOf<float>& angle) const {
   AngleOf<float> r = Binary(this->value - angle.value);
   r = Normalize(r);
   return r;
 }
 template <typename T>
-AngleOf<T> AngleOf<T>::operator-(const AngleOf<T> &angle) const {
+AngleOf<T> AngleOf<T>::operator-(const AngleOf<T>& angle) const {
   AngleOf<T> r = Binary(this->value - angle.value);
   return r;
 }
 
 template <>
-AngleOf<float> AngleOf<float>::operator+(const AngleOf<float> &angle) const {
+AngleOf<float> AngleOf<float>::operator+(const AngleOf<float>& angle) const {
   AngleOf<float> r = Binary(this->value + angle.value);
   r = Normalize(r);
   return r;
 }
 template <typename T>
-AngleOf<T> AngleOf<T>::operator+(const AngleOf<T> &angle) const {
+AngleOf<T> AngleOf<T>::operator+(const AngleOf<T>& angle) const {
   AngleOf<T> r = Binary(this->value + angle.value);
   return r;
 }
 
 template <>
-AngleOf<float> AngleOf<float>::operator+=(const AngleOf<float> &angle) {
+AngleOf<float> AngleOf<float>::operator+=(const AngleOf<float>& angle) {
   this->value += angle.value;
   this->Normalize();
   return *this;
 }
 
 template <typename T>
-AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T> &angle) {
+AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& angle) {
   this->value += angle.value;
   return *this;
 }
@@ -206,7 +230,8 @@ AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T> &angle) {
 //   return AngleOf::Degrees((float)factor * angle.InDegrees());
 // }
 
-template <typename T> void AngleOf<T>::Normalize() {
+template <typename T>
+void AngleOf<T>::Normalize() {
   float angleValue = this->InDegrees();
   if (!isfinite(angleValue))
     return;
@@ -218,7 +243,8 @@ template <typename T> void AngleOf<T>::Normalize() {
   *this = AngleOf::Degrees(angleValue);
 }
 
-template <typename T> AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
+template <typename T>
+AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
   float angleValue = angle.InDegrees();
   if (!isfinite(angleValue))
     return angle;
@@ -237,9 +263,10 @@ AngleOf<T> AngleOf<T>::Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max) {
 }
 
 template <typename T>
-AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
+AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
+                                   AngleOf<T> toAngle,
                                    float maxDegrees) {
-  maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances
+  maxDegrees = fmaxf(0, maxDegrees);  // filter out negative distances
   AngleOf<T> d = toAngle - fromAngle;
   float dDegrees = Abs(d).InDegrees();
   d = AngleOf<T>::Degrees(Float::Clamp(dDegrees, 0, maxDegrees));
@@ -249,28 +276,34 @@ AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
   return fromAngle + d;
 }
 
-template <typename T> float AngleOf<T>::Cos(AngleOf<T> angle) {
+template <typename T>
+float AngleOf<T>::Cos(AngleOf<T> angle) {
   return cosf(angle.InRadians());
 }
-template <typename T> float AngleOf<T>::Sin(AngleOf<T> angle) {
+template <typename T>
+float AngleOf<T>::Sin(AngleOf<T> angle) {
   return sinf(angle.InRadians());
 }
-template <typename T> float AngleOf<T>::Tan(AngleOf<T> angle) {
+template <typename T>
+float AngleOf<T>::Tan(AngleOf<T> angle) {
   return tanf(angle.InRadians());
 }
 
-template <typename T> AngleOf<T> AngleOf<T>::Acos(float f) {
+template <typename T>
+AngleOf<T> AngleOf<T>::Acos(float f) {
   return AngleOf<T>::Radians(acosf(f));
 }
-template <typename T> AngleOf<T> AngleOf<T>::Asin(float f) {
+template <typename T>
+AngleOf<T> AngleOf<T>::Asin(float f) {
   return AngleOf<T>::Radians(asinf(f));
 }
-template <typename T> AngleOf<T> AngleOf<T>::Atan(float f) {
+template <typename T>
+AngleOf<T> AngleOf<T>::Atan(float f) {
   return AngleOf<T>::Radians(atanf(f));
 }
 
 template <typename T>
-AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Atan2(float y, float x) {
+AngleOf<T> AngleOf<T>::Atan2(float y, float x) {
   return AngleOf<T>::Radians(atan2f(y, x));
 }
 
@@ -297,7 +330,7 @@ float AngleOf<T>::CosineRuleSide(float a, float b, AngleOf<T> gamma) {
   float b2 = b * b;
   float d =
       a2 + b2 -
-      2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
+      2 * a * b * Cos(gamma);  // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
   // Catch edge cases where float inacuracies lead tot nans
   if (d < 0)
     return 0;
@@ -354,6 +387,8 @@ AngleOf<T> AngleOf<T>::SineRuleAngle(float a, AngleOf<T> beta, float b) {
   return alpha;
 }
 
-template class Passer::LinearAlgebra::AngleOf<float>;
+template class AngleOf<float>;
-template class Passer::LinearAlgebra::AngleOf<signed char>;
+template class AngleOf<signed char>;
-template class Passer::LinearAlgebra::AngleOf<signed short>;
+template class AngleOf<signed short>;
+
+}  // namespace LinearAlgebra

Angle.h

@@ -5,7 +5,6 @@
 #ifndef ANGLE_H
 #define ANGLE_H
 
-namespace Passer {
 namespace LinearAlgebra {
 
 static float pi = 3.1415927410125732421875F;
@@ -18,10 +17,11 @@ static float Deg2Rad = (pi * 2) / 360.0f;
 /// The angle is internally limited to (-180..180] degrees or (-PI...PI]
 /// radians. When an angle exceeds this range, it is normalized to a value
 /// within the range.
-template <typename T> class AngleOf {
+template <typename T>
-public:
+class AngleOf {
+ public:
   /// @brief Create a new angle with a zero value
-  AngleOf<T>();
+  AngleOf();
 
   /// @brief An zero value angle
   const static AngleOf<T> zero;
@@ -100,28 +100,28 @@ public:
   /// @brief Substract another angle from this angle
   /// @param angle The angle to subtract from this angle
   /// @return The result of the subtraction
-  AngleOf<T> operator-(const AngleOf<T> &angle) const;
+  AngleOf<T> operator-(const AngleOf<T>& angle) const;
   /// @brief Add another angle from this angle
   /// @param angle The angle to add to this angle
   /// @return The result of the addition
-  AngleOf<T> operator+(const AngleOf<T> &angle) const;
+  AngleOf<T> operator+(const AngleOf<T>& angle) const;
   /// @brief Add another angle to this angle
   /// @param angle The angle to add to this angle
   /// @return The result of the addition
-  AngleOf<T> operator+=(const AngleOf<T> &angle);
+  AngleOf<T> operator+=(const AngleOf<T>& angle);
 
   /// @brief Mutliplies the angle
   /// @param angle The angle to multiply
   /// @param factor The factor by which the angle is multiplied
   /// @return The multiplied angle
-  friend AngleOf<T> operator*(const AngleOf<T> &angle, float factor) {
+  friend AngleOf<T> operator*(const AngleOf<T>& angle, float factor) {
     return AngleOf::Degrees((float)angle.InDegrees() * factor);
   }
   /// @brief Multiplies the angle
   /// @param factor The factor by which the angle is multiplies
   /// @param angle The angle to multiply
   /// @return The multiplied angle
-  friend AngleOf<T> operator*(float factor, const AngleOf<T> &angle) {
+  friend AngleOf<T> operator*(float factor, const AngleOf<T>& angle) {
     return AngleOf::Degrees((float)factor * angle.InDegrees());
   }
 
@@ -150,7 +150,8 @@ public:
   /// @param toAngle The angle to rotate towards
   /// @param maxAngle The maximum angle to rotate
   /// @return The rotated angle
-  static AngleOf<T> MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
+  static AngleOf<T> MoveTowards(AngleOf<T> fromAngle,
+                                AngleOf<T> toAngle,
                                 float maxAngle);
 
   /// @brief Calculates the cosine of an angle
@@ -205,18 +206,22 @@ public:
   /// @return The angle of the corner opposing side A
   static AngleOf<T> SineRuleAngle(float a, AngleOf<T> beta, float c);
 
-private:
+ private:
   T value;
 
-  AngleOf<T>(T rawValue);
+  AngleOf(T rawValue);
 };
 
 using AngleSingle = AngleOf<float>;
 using Angle16 = AngleOf<signed short>;
 using Angle8 = AngleOf<signed char>;
 
-} // namespace LinearAlgebra
+#if defined(ARDUINO)
-} // namespace Passer
+using Angle = Angle16;
-using namespace Passer::LinearAlgebra;
+#else
+using Angle = AngleSingle;
+#endif
+
+}  // namespace LinearAlgebra
 
 #endif

Direction.cpp

@@ -9,7 +9,9 @@
 
 #include <math.h>
 
-template <typename T> DirectionOf<T>::DirectionOf() {
+namespace LinearAlgebra {
+template <typename T>
+DirectionOf<T>::DirectionOf() {
   this->horizontal = AngleOf<T>();
   this->vertical = AngleOf<T>();
 }
@@ -41,7 +43,7 @@ const DirectionOf<T> DirectionOf<T>::right =
     DirectionOf<T>(AngleOf<T>::Degrees(90), AngleOf<T>());
 
 template <typename T>
-Vector3 Passer::LinearAlgebra::DirectionOf<T>::ToVector3() const {
+Vector3 DirectionOf<T>::ToVector3() const {
   Quaternion q = Quaternion::Euler(-this->vertical.InDegrees(),
                                    this->horizontal.InDegrees(), 0);
   Vector3 v = q * Vector3::forward;
@@ -49,49 +51,47 @@ Vector3 Passer::LinearAlgebra::DirectionOf<T>::ToVector3() const {
 }
 
 template <typename T>
-DirectionOf<T>
+DirectionOf<T> DirectionOf<T>::FromVector3(Vector3 vector) {
-Passer::LinearAlgebra::DirectionOf<T>::FromVector3(Vector3 vector) {
   DirectionOf<T> d;
   d.horizontal = AngleOf<T>::Atan2(
       vector.Right(),
-      vector.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
+      vector
+          .Forward());  // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
   d.vertical =
       AngleOf<T>::Degrees(-90) -
       AngleOf<T>::Acos(
-          vector.Up()); // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
+          vector.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,
+DirectionOf<T> DirectionOf<T>::Degrees(float horizontal, float vertical) {
-                                                              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,
+DirectionOf<T> DirectionOf<T>::Radians(float horizontal, float vertical) {
-                                                              float vertical) {
   return DirectionOf<T>(AngleOf<T>::Radians(horizontal),
                         AngleOf<T>::Radians(vertical));
 }
 
 template <typename T>
-bool Passer::LinearAlgebra::DirectionOf<T>::operator==(
+bool DirectionOf<T>::operator==(const DirectionOf<T> direction) const {
-    const DirectionOf<T> direction) const {
   return (this->horizontal == direction.horizontal) &&
          (this->vertical == direction.vertical);
 }
 
 template <typename T>
-DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator-() const {
+DirectionOf<T> DirectionOf<T>::operator-() const {
   DirectionOf<T> r = DirectionOf<T>(this->horizontal + AngleOf<T>::Degrees(180),
                                     -this->vertical);
   return r;
 }
 
-template <typename T> void DirectionOf<T>::Normalize() {
+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);
@@ -99,5 +99,6 @@ template <typename T> void DirectionOf<T>::Normalize() {
   }
 }
 
-template class Passer::LinearAlgebra::DirectionOf<float>;
+template class LinearAlgebra::DirectionOf<float>;
-template class Passer::LinearAlgebra::DirectionOf<signed short>;
+template class LinearAlgebra::DirectionOf<signed short>;
+}

Direction.h

@@ -7,7 +7,6 @@
 
 #include "Angle.h"
 
-namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector3;
@@ -22,20 +21,21 @@ struct Vector3;
 /// rotation has been applied.
 /// The angles are automatically normalized to stay within the abovenmentioned
 /// ranges.
-template <typename T> class DirectionOf {
+template <typename T>
-public:
+class DirectionOf {
-  /// @brief Create a new direction with zero angles
+ public:
-  DirectionOf<T>();
-  /// @brief Create a new direction
-  /// @param horizontal The horizontal angle
-  /// @param vertical The vertical angle.
-  DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
-
   /// @brief horizontal angle, range= (-180..180]
   AngleOf<T> horizontal;
   /// @brief vertical angle, range in degrees = (-90..90]
   AngleOf<T> vertical;
-  
+
+  /// @brief Create a new direction with zero angles
+  DirectionOf();
+  /// @brief Create a new direction
+  /// @param horizontal The horizontal angle
+  /// @param vertical The vertical angle.
+  DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical);
+
   /// @brief Convert the direction into a carthesian vector
   /// @return The carthesian vector corresponding to this direction.
   Vector3 ToVector3() const;
@@ -83,7 +83,7 @@ public:
   /// @return The reversed direction.
   DirectionOf<T> operator-() const;
 
-protected:
+ protected:
   /// @brief Normalize this vector to the specified ranges
   void Normalize();
 };
@@ -91,8 +91,12 @@ protected:
 using DirectionSingle = DirectionOf<float>;
 using Direction16 = DirectionOf<signed short>;
 
-} // namespace LinearAlgebra
+#if defined(ARDUINO)
-} // namespace Passer
+using Direction = Direction16;
-using namespace Passer::LinearAlgebra;
+#else
+using Direction = DirectionSingle;
+#endif
+
+}  // namespace LinearAlgebra
 
 #endif

FloatSingle.h

@@ -5,19 +5,18 @@
 #ifndef FLOAT_H
 #define FLOAT_H
 
-namespace Passer {
 namespace LinearAlgebra {
 
 class Float {
-public:
+ public:
   static const float epsilon;
   static const float sqrEpsilon;
 
   static float Clamp(float f, float min, float max);
 };
 
-} // namespace LinearAlgebra
+}  // namespace LinearAlgebra
-} // namespace Passer
+
-using namespace Passer::LinearAlgebra;
+using namespace LinearAlgebra;
 
 #endif

Matrix.cpp

@@ -1,6 +1,290 @@
 #include "Matrix.h"
+#if !defined(NO_STD) 
+#include <iostream>
+#endif
 
-template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
+namespace LinearAlgebra {
+
+#pragma region Matrix1
+
+Matrix1::Matrix1(int size) : size(size) {
+  if (this->size == 0)
+    data = nullptr;
+  else {
+    this->data = new float[size]();
+    this->externalData = false;
+  }
+}
+
+Matrix1::Matrix1(float* data, int size) : data(data), size(size) {
+  this->externalData = true;
+}
+
+Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
+  Matrix1 r = Matrix1(4);
+  float* data = r.data;
+  data[0] = q.x;
+  data[1] = q.y;
+  data[2] = q.z;
+  data[3] = q.w;
+  return r;
+}
+
+Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
+  return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
+}
+
+// Matrix1
+#pragma endregion
+
+#pragma region Matrix2
+
+Matrix2::Matrix2() {}
+
+Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
+  this->nValues = nRows * nCols;
+  if (this->nValues == 0)
+    this->data = nullptr;
+  else {
+    this->data = new float[this->nValues];
+    this->externalData = false;
+  }
+}
+
+Matrix2::Matrix2(float* data, int nRows, int nCols)
+    : nRows(nRows), nCols(nCols), data(data) {
+  this->nValues = nRows * nCols;
+  this->externalData = true;
+}
+
+Matrix2::Matrix2(const Matrix2& m)
+    : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) {
+  if (this->nValues == 0)
+    this->data = nullptr;
+  else {
+    this->data = new float[this->nValues];
+
+    for (int ix = 0; ix < this->nValues; ++ix)
+      this->data[ix] = m.data[ix];
+  }
+}
+
+Matrix2& Matrix2::operator=(const Matrix2& m) {
+  if (this != &m) {
+    delete[] this->data;  // Free the current memory
+
+    this->nRows = m.nRows;
+    this->nCols = m.nCols;
+    this->nValues = m.nValues;
+    if (this->nValues == 0)
+      this->data = nullptr;
+    else {
+      this->data = new float[this->nValues];
+      for (int ix = 0; ix < this->nValues; ++ix)
+        this->data[ix] = m.data[ix];
+    }
+  }
+  return *this;
+}
+
+Matrix2::~Matrix2() {
+  if (!this->externalData)
+    delete[] data;
+}
+
+Matrix2 Matrix2::Clone() const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  for (int ix = 0; ix < this->nValues; ++ix)
+    r.data[ix] = this->data[ix];
+  return r;
+}
+
+// Move constructor
+Matrix2::Matrix2(Matrix2&& other) noexcept
+    : nRows(other.nRows),
+      nCols(other.nCols),
+      nValues(other.nValues),
+      data(other.data) {
+  other.data = nullptr;  // Set the other object's pointer to nullptr to avoid
+                         // double deletion
+}
+
+// Move assignment operator
+Matrix2& Matrix2::operator=(Matrix2&& other) noexcept {
+  if (this != &other) {
+    delete[] data;  // Clean up current data
+    nRows = other.nRows;
+    nCols = other.nCols;
+    nValues = other.nValues;
+    data = other.data;
+    other.data = nullptr;  // Avoid double deletion
+  }
+  return *this;
+}
+
+Matrix2 Matrix2::Zero(int nRows, int nCols) {
+  Matrix2 r = Matrix2(nRows, nCols);
+  for (int ix = 0; ix < r.nValues; ix++)
+    r.data[ix] = 0;
+  return r;
+}
+
+void Matrix2::Clear() {
+  for (int ix = 0; ix < this->nValues; ix++)
+    this->data[ix] = 0;
+}
+
+Matrix2 Matrix2::Identity(int size) {
+  return Diagonal(1, size);
+}
+
+Matrix2 Matrix2::Diagonal(float f, int size) {
+  Matrix2 r = Matrix2::Zero(size, size);
+  float* data = r.data;
+  int valueIx = 0;
+  for (int ix = 0; ix < size; ix++) {
+    data[valueIx] = f;
+    valueIx += size + 1;
+  }
+  return r;
+}
+
+Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
+  Matrix2 r = Matrix2(3, 3);
+  float* data = r.data;
+  data[0 * 3 + 1] = -v.z;  // result(0, 1)
+  data[0 * 3 + 2] = v.y;   // result(0, 2)
+  data[1 * 3 + 0] = v.z;   // result(1, 0)
+  data[1 * 3 + 2] = -v.x;  // result(1, 2)
+  data[2 * 3 + 0] = -v.y;  // result(2, 0)
+  data[2 * 3 + 1] = v.x;   // result(2, 1)
+  return r;
+}
+
+Matrix2 Matrix2::Transpose() const {
+  Matrix2 r = Matrix2(this->nCols, this->nRows);
+
+  for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
+    for (int colIx = 0; colIx < this->nCols; colIx++)
+      r.data[colIx * this->nCols + rowIx] =
+          this->data[rowIx * this->nCols + colIx];
+  }
+  return r;
+}
+
+Matrix2 LinearAlgebra::Matrix2::operator-() const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  for (int ix = 0; ix < r.nValues; ix++)
+    r.data[ix] = -this->data[ix];
+  return r;
+}
+
+Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  for (int ix = 0; ix < r.nValues; ix++)
+    r.data[ix] = this->data[ix] + v.data[ix];
+  return r;
+}
+
+Matrix2 Matrix2::operator+=(const Matrix2& v) {
+  for (int ix = 0; ix < this->nValues; ix++)
+    this->data[ix] += v.data[ix];
+  return *this;
+}
+
+Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
+  Matrix2 r = Matrix2(this->nRows, B.nCols);
+
+  int ACols = this->nCols;
+  int BCols = B.nCols;
+  int ARows = this->nRows;
+  // int BRows = B.nRows;
+
+  for (int i = 0; i < ARows; ++i) {
+    // Pre-compute row offsets
+    int ARowOffset = i * ACols;  // ARowOffset is constant for each row of A
+    int BColOffset = i * BCols;  // BColOffset is constant for each row of B
+    for (int j = 0; j < BCols; ++j) {
+      float sum = 0;
+      std::cout << " 0";
+      int BIndex = j;
+      for (int k = 0; k < ACols; ++k) {
+        std::cout << " + " << this->data[ARowOffset + k] << " * "
+                  << B.data[BIndex];
+        sum += this->data[ARowOffset + k] * B.data[BIndex];
+        BIndex += BCols;
+      }
+      r.data[BColOffset + j] = sum;
+      std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
+    }
+  }
+  return r;
+}
+
+Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
+  Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
+
+  int resultRowIx = 0;
+  int resultColIx = 0;
+  for (int i = rowStart; i < rowStop; i++) {
+    for (int j = colStart; j < colStop; j++)
+      r.data[resultRowIx * r.nCols + resultColIx] =
+          this->data[i * this->nCols + j];
+  }
+  return r;
+}
+
+void Matrix2::UpdateSlice(int rowStart,
+                          int rowStop,
+                          int colStart,
+                          int colStop,
+                          const Matrix2& m) const {
+  // for (int i = rowStart; i < rowStop; i++) {
+  //   for (int j = colStart; j < colStop; j++)
+  //     this->data[i * this->nCols + j] =
+  //         m.data[(i - rowStart) * m.nCols + (j - colStart)];
+  // }
+
+  int rRowDataIx = rowStart * this->nCols;
+  int mRowDataIx = 0;
+  for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
+    rRowDataIx = rowIx * this->nCols;
+    // rRowDataIx += this->nCols;
+    mRowDataIx += m.nCols;
+    for (int colIx = colStart; colIx < colStop; colIx++) {
+      this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
+    }
+  }
+}
+
+/// @brief Compute the Omega matrix of a 3D vector
+/// @param v The vector
+/// @return 4x4 Omega matrix
+Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) {
+  Matrix2 r = Matrix2::Zero(4, 4);
+  r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));
+
+  // set last row to -v
+  int ix = 3 * 4;
+  r.data[ix++] = -v.x;
+  r.data[ix++] = -v.y;
+  r.data[ix] = -v.z;
+
+  // Set last column to v
+  ix = 3;
+  r.data[ix += 4] = v.x;
+  r.data[ix += 4] = v.y;
+  r.data[ix] = v.z;
+  return r;
+}
+
+// Matrix2
+#pragma endregion
+
+}  // namespace LinearAlgebra
+
+template <>
+MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   if (rows <= 0 || cols <= 0) {
     this->rows = 0;
     this->cols = 0;
@@ -14,15 +298,17 @@ template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   this->data = new float[matrixSize]{0.0f};
 }
 
-template <> MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
+template <>
+MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
   Set(0, 0, v.Right());
   Set(1, 0, v.Up());
   Set(2, 0, v.Forward());
 }
 
 template <>
-void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
+void MatrixOf<float>::Multiply(const MatrixOf<float>* m1,
-                               const MatrixOf<float> *m2, MatrixOf<float> *r) {
+                               const MatrixOf<float>* m2,
+                               MatrixOf<float>* r) {
   for (unsigned int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) {
     for (unsigned int colIx2 = 0; colIx2 < m2->cols; colIx2++) {
       unsigned int rDataIx = colIx2 * m2->cols + rowIx1;
@@ -37,7 +323,7 @@ void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
 }
 
 template <>
-Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
+Vector3 MatrixOf<float>::Multiply(const MatrixOf<float>* m, Vector3 v) {
   MatrixOf<float> v_m = MatrixOf<float>(v);
   MatrixOf<float> r_m = MatrixOf<float>(3, 1);
 
@@ -47,10 +333,11 @@ Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
   return r;
 }
 
-template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
+template <typename T>
-  float *vData = new float[3]{v.Right(), v.Up(), v.Forward()};
+Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
+  float* vData = new float[3]{v.Right(), v.Up(), v.Forward()};
   MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
-  float *rData = new float[3]{};
+  float* rData = new float[3]{};
   MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
 
   Multiply(this, &v_m, &r_m);

Matrix.h

@@ -1,20 +1,129 @@
 #ifndef MATRIX_H
 #define MATRIX_H
 
+#include "Quaternion.h"
 #include "Vector3.h"
 
-namespace Passer {
 namespace LinearAlgebra {
 
+/// @brief A 1-dimensional matrix or vector of arbitrary size
+class Matrix1 {
+ public:
+  float* data = nullptr;
+  int size = 0;
+
+  Matrix1(int size);
+  Matrix1(float* data, int size);
+
+  static Matrix1 FromQuaternion(Quaternion q);
+  Quaternion ToQuaternion();
+
+ private:
+  bool externalData = true;
+};
+
+/// @brief A 2-dimensional matrix of arbitrary size
+class Matrix2 {
+ public:
+  int nRows = 0;
+  int nCols = 0;
+  int nValues = 0;
+  float* data = nullptr;
+
+  Matrix2();
+  Matrix2(int nRows, int nCols);
+  Matrix2(float* data, int nRows, int nCols);
+  Matrix2(const Matrix2& m);
+  Matrix2& operator=(const Matrix2& other);
+
+  ~Matrix2();
+
+  Matrix2 Clone() const;
+
+  static Matrix2 Zero(int nRows, int nCols);
+  void Clear();
+
+  static Matrix2 Identity(int size);
+
+  static Matrix2 Diagonal(float f, int size);
+
+  static Matrix2 SkewMatrix(const Vector3& v);
+
+  Matrix2 Transpose() const;
+
+  Matrix2 operator-() const;
+
+  /// @brief Add a matrix to this matrix
+  /// @param m The matrix to add to this matrix
+  /// @return The result of the addition
+  Matrix2 operator+(const Matrix2& v) const;
+  Matrix2 operator+=(const Matrix2& v);
+
+  Matrix2 operator*(const Matrix2& m) const;
+  friend Matrix2 operator*(const Matrix2& m, float f) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = m.data[ix] * f;
+    return r;
+  }
+  friend Matrix2 operator*(float f, const Matrix2& m) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = f * m.data[ix];
+    return r;
+  }
+
+  friend Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
+    Matrix1 r = Matrix1(m.nRows);
+    for (int rowIx = 0; rowIx < m.nRows; rowIx++) {
+      int mRowIx = rowIx * m.nCols;
+      for (int colIx = 0; colIx < m.nCols; colIx++)
+        r.data[rowIx] += m.data[mRowIx + colIx] * v.data[rowIx];
+    }
+    return r;
+  }
+
+  friend Matrix2 operator/(const Matrix2& m, float f) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = m.data[ix] / f;
+    return r;
+  }
+  friend Matrix2 operator/(float f, const Matrix2& m) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = f / m.data[ix];
+    return r;
+  }
+
+  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
+
+  void UpdateSlice(int rowStart,
+                int rowStop,
+                int colStart,
+                int colStop,
+                const Matrix2& m) const;
+  // private:
+  //  move constructor and move assignment operator
+  Matrix2(Matrix2&& other) noexcept;
+  Matrix2& operator=(Matrix2&& other) noexcept;
+
+  static Matrix2 Omega(const Vector3& v);
+
+ private:
+  bool externalData = true;
+};
+
 /// @brief Single precision float matrix
-template <typename T> class MatrixOf {
+template <typename T>
-public:
+class MatrixOf {
+ public:
   MatrixOf(unsigned int rows, unsigned int cols);
-  MatrixOf(unsigned int rows, unsigned int cols, const T *source)
+  MatrixOf(unsigned int rows, unsigned int cols, const T* source)
       : MatrixOf(rows, cols) {
     Set(source);
   }
-  MatrixOf(Vector3 v); // creates a 3,1 matrix
+  MatrixOf(Vector3 v);  // creates a 3,1 matrix
 
   ~MatrixOf() {
     if (this->data == nullptr)
@@ -25,7 +134,7 @@ public:
 
   /// @brief Transpose with result in matrix m
   /// @param r The matrix in which the transposed matrix is stored
-  void Transpose(MatrixOf<T> *r) const {
+  void Transpose(MatrixOf<T>* r) const {
     // Check dimensions first
     // We dont care about the rows and cols (we overwrite them)
     // but the data size should be equal to avoid problems
@@ -54,13 +163,14 @@ public:
     }
   }
 
-  static void Multiply(const MatrixOf<T> *m1, const MatrixOf<T> *m2,
+  static void Multiply(const MatrixOf<T>* m1,
-                       MatrixOf<T> *r);
+                       const MatrixOf<T>* m2,
-  void Multiply(const MatrixOf<T> *m, MatrixOf<T> *r) const {
+                       MatrixOf<T>* r);
+  void Multiply(const MatrixOf<T>* m, MatrixOf<T>* r) const {
     Multiply(this, m, r);
   }
 
-  static Vector3 Multiply(const MatrixOf<T> *m, Vector3 v);
+  static Vector3 Multiply(const MatrixOf<T>* m, Vector3 v);
   Vector3 operator*(const Vector3 v) const;
 
   T Get(unsigned int rowIx, unsigned int colIx) const {
@@ -74,28 +184,28 @@ public:
   }
 
   // This function does not check on source size!
-  void Set(const T *source) {
+  void Set(const T* source) {
     unsigned int matrixSize = this->cols * this->rows;
     for (unsigned int dataIx = 0; dataIx < matrixSize; dataIx++)
       this->data[dataIx] = source[dataIx];
   }
 
   // This function does not check on source size!
-  void SetRow(unsigned int rowIx, const T *source) {
+  void SetRow(unsigned int rowIx, const T* source) {
     unsigned int dataIx = rowIx * this->cols;
     for (unsigned int sourceIx = 0; sourceIx < this->cols; dataIx++, sourceIx++)
       this->data[dataIx] = source[sourceIx];
   }
 
   // This function does not check on source size!
-  void SetCol(unsigned int colIx, const T *source) {
+  void SetCol(unsigned int colIx, const T* source) {
     unsigned int dataIx = colIx;
     for (unsigned int sourceIx = 0; sourceIx < this->cols;
          dataIx += this->cols, sourceIx++)
       this->data[dataIx] = source[sourceIx];
   }
 
-  void CopyFrom(const MatrixOf<T> *m) {
+  void CopyFrom(const MatrixOf<T>* m) {
     unsigned int thisMatrixSize = this->cols * this->rows;
     unsigned int mMatrixSize = m->cols * m->rows;
     if (mMatrixSize != thisMatrixSize)
@@ -108,14 +218,13 @@ public:
   unsigned int RowCount() const { return rows; }
   unsigned int ColCount() const { return cols; }
 
-private:
+ private:
   unsigned int rows;
   unsigned int cols;
-  T *data;
+  T* data;
 };
 
-} // namespace LinearAlgebra
+}  // namespace LinearAlgebra
-} // namespace Passer
+// using namespace LinearAlgebra;
-using namespace Passer::LinearAlgebra;
 
 #endif

Polar.cpp

@@ -3,11 +3,13 @@
 #include "Polar.h"
 #include "Vector2.h"
 
-template <typename T> PolarOf<T>::PolarOf() {
+template <typename T>
+PolarOf<T>::PolarOf() {
   this->distance = 0.0f;
   this->angle = AngleOf<T>();
 }
-template <typename T> PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
+template <typename T>
+PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
   // distance should always be 0 or greater
   if (distance < 0.0f) {
     this->distance = -distance;
@@ -34,16 +36,18 @@ PolarOf<T> PolarOf<T>::Radians(float distance, float radians) {
   return PolarOf<T>(distance, AngleOf<T>::Radians(radians));
 }
 
-template <typename T> PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
+template <typename T>
+PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
   float distance = v.magnitude();
   AngleOf<T> angle =
       AngleOf<T>::Degrees(Vector2::SignedAngle(Vector2::forward, v));
   PolarOf<T> p = PolarOf(distance, angle);
   return p;
 }
-template <typename T> PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
+template <typename T>
-  float distance = v.distance * cosf(v.direction.vertical.InDegrees() *
+PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
-                                     Passer::LinearAlgebra::Deg2Rad);
+  float distance =
+      v.distance * cosf(v.direction.vertical.InDegrees() * Deg2Rad);
   AngleOf<T> angle = v.direction.horizontal;
   PolarOf<T> p = PolarOf(distance, angle);
   return p;
@@ -60,31 +64,37 @@ const PolarOf<T> PolarOf<T>::right = PolarOf(1.0, AngleOf<T>::Degrees(90));
 template <typename T>
 const PolarOf<T> PolarOf<T>::left = PolarOf(1.0, AngleOf<T>::Degrees(-90));
 
-template <typename T> bool PolarOf<T>::operator==(const PolarOf &v) const {
+template <typename T>
+bool PolarOf<T>::operator==(const PolarOf& v) const {
   return (this->distance == v.distance &&
           this->angle.InDegrees() == v.angle.InDegrees());
 }
 
-template <typename T> PolarOf<T> PolarOf<T>::Normalize(const PolarOf &v) {
+template <typename T>
+PolarOf<T> PolarOf<T>::Normalize(const PolarOf& v) {
   PolarOf<T> r = PolarOf(1, v.angle);
   return r;
 }
-template <typename T> PolarOf<T> PolarOf<T>::normalized() const {
+template <typename T>
+PolarOf<T> PolarOf<T>::normalized() const {
   PolarOf<T> r = PolarOf(1, this->angle);
   return r;
 }
 
-template <typename T> PolarOf<T> PolarOf<T>::operator-() const {
+template <typename T>
+PolarOf<T> PolarOf<T>::operator-() const {
   PolarOf<T> v =
       PolarOf(this->distance, this->angle + AngleOf<T>::Degrees(180));
   return v;
 }
 
-template <typename T> PolarOf<T> PolarOf<T>::operator-(const PolarOf &v) const {
+template <typename T>
+PolarOf<T> PolarOf<T>::operator-(const PolarOf& v) const {
   PolarOf<T> r = -v;
   return *this + r;
 }
-template <typename T> PolarOf<T> PolarOf<T>::operator-=(const PolarOf &v) {
+template <typename T>
+PolarOf<T> PolarOf<T>::operator-=(const PolarOf& v) {
   *this = *this - v;
   return *this;
   // angle = AngleOf<T>::Normalize(newAngle);
@@ -105,7 +115,8 @@ template <typename T> PolarOf<T> PolarOf<T>::operator-=(const PolarOf &v) {
 //   return d;
 // }
 
-template <typename T> PolarOf<T> PolarOf<T>::operator+(const PolarOf &v) const {
+template <typename T>
+PolarOf<T> PolarOf<T>::operator+(const PolarOf& v) const {
   if (v.distance == 0)
     return PolarOf(this->distance, this->angle);
   if (this->distance == 0.0f)
@@ -133,33 +144,36 @@ template <typename T> PolarOf<T> PolarOf<T>::operator+(const PolarOf &v) const {
   PolarOf vector = PolarOf(newDistance, newAngleA);
   return vector;
 }
-template <typename T> PolarOf<T> PolarOf<T>::operator+=(const PolarOf &v) {
+template <typename T>
+PolarOf<T> PolarOf<T>::operator+=(const PolarOf& v) {
   *this = *this + v;
   return *this;
 }
 
-template <typename T> PolarOf<T> PolarOf<T>::operator*=(float f) {
+template <typename T>
+PolarOf<T> PolarOf<T>::operator*=(float f) {
   this->distance *= f;
   return *this;
 }
-template <typename T> PolarOf<T> PolarOf<T>::operator/=(float f) {
+template <typename T>
+PolarOf<T> PolarOf<T>::operator/=(float f) {
   this->distance /= f;
   return *this;
 }
 
 template <typename T>
-float PolarOf<T>::Distance(const PolarOf &v1, const PolarOf &v2) {
+float PolarOf<T>::Distance(const PolarOf& v1, const PolarOf& v2) {
   float d =
       AngleOf<T>::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
   return d;
 }
 
 template <typename T>
-PolarOf<T> PolarOf<T>::Rotate(const PolarOf &v, AngleOf<T> angle) {
+PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
   AngleOf<T> a = AngleOf<T>::Normalize(v.angle + angle);
   PolarOf<T> r = PolarOf(v.distance, a);
   return r;
 }
 
-template class Passer::LinearAlgebra::PolarOf<float>;
+template class LinearAlgebra::PolarOf<float>;
-template class Passer::LinearAlgebra::PolarOf<signed short>;
+template class LinearAlgebra::PolarOf<signed short>;

Polar.h

@@ -7,16 +7,17 @@
 
 #include "Angle.h"
 
-namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector2;
-template <typename T> class SphericalOf;
+template <typename T>
+class SphericalOf;
 
 /// @brief A polar vector using an angle in various representations
 /// @tparam T The implementation type used for the representation of the angle
-template <typename T> class PolarOf {
+template <typename T>
-public:
+class PolarOf {
+ public:
   /// @brief The distance in meters
   /// @remark The distance shall never be negative
   float distance;
@@ -76,12 +77,12 @@ public:
   /// @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 PolarOf &v) const;
+  bool operator==(const PolarOf& v) const;
 
   /// @brief The vector length
   /// @param v The vector for which you need the length
   /// @return The vector length;
-  inline static float Magnitude(const PolarOf &v) { return v.distance; }
+  inline static float Magnitude(const PolarOf& v) { return v.distance; }
   /// @brief The vector length
   /// @return The vector length
   inline float magnitude() const { return this->distance; }
@@ -89,7 +90,7 @@ public:
   /// @brief Convert the vector to a length of 1
   /// @param v The vector to convert
   /// @return The vector normalized to a length of 1
-  static PolarOf Normalize(const PolarOf &v);
+  static PolarOf Normalize(const PolarOf& v);
   /// @brief Convert the vector to a length of a
   /// @return The vector normalized to a length of 1
   PolarOf normalized() const;
@@ -102,23 +103,23 @@ public:
   /// @brief Subtract a polar vector from this vector
   /// @param v The vector to subtract
   /// @return The result of the subtraction
-  PolarOf operator-(const PolarOf &v) const;
+  PolarOf operator-(const PolarOf& v) const;
-  PolarOf operator-=(const PolarOf &v);
+  PolarOf operator-=(const PolarOf& v);
   /// @brief Add a polar vector to this vector
   /// @param v The vector to add
   /// @return The result of the addition
-  PolarOf operator+(const PolarOf &v) const;
+  PolarOf operator+(const PolarOf& v) const;
-  PolarOf operator+=(const PolarOf &v);
+  PolarOf operator+=(const PolarOf& v);
 
   /// @brief Scale the vector uniformly up
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark This operation will scale the distance of the vector. The angle
   /// will be unaffected.
-  friend PolarOf operator*(const PolarOf &v, float f) {
+  friend PolarOf operator*(const PolarOf& v, float f) {
     return PolarOf(v.distance * f, v.angle);
   }
-  friend PolarOf operator*(float f, const PolarOf &v) {
+  friend PolarOf operator*(float f, const PolarOf& v) {
     return PolarOf(f * v.distance, v.angle);
   }
   PolarOf operator*=(float f);
@@ -127,10 +128,10 @@ public:
   /// @return The scaled factor
   /// @remark This operation will scale the distance of the vector. The angle
   /// will be unaffected.
-  friend PolarOf operator/(const PolarOf &v, float f) {
+  friend PolarOf operator/(const PolarOf& v, float f) {
     return PolarOf(v.distance / f, v.angle);
   }
-  friend PolarOf operator/(float f, const PolarOf &v) {
+  friend PolarOf operator/(float f, const PolarOf& v) {
     return PolarOf(f / v.distance, v.angle);
   }
   PolarOf operator/=(float f);
@@ -139,22 +140,21 @@ public:
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The distance between the two vectors
-  static float Distance(const PolarOf &v1, const PolarOf &v2);
+  static float Distance(const PolarOf& v1, const PolarOf& v2);
 
   /// @brief Rotate a vector
   /// @param v The vector to rotate
   /// @param a The angle in degreesto rotate
   /// @return The rotated vector
-  static PolarOf Rotate(const PolarOf &v, AngleOf<T> a);
+  static PolarOf Rotate(const PolarOf& v, AngleOf<T> a);
 };
 
 using PolarSingle = PolarOf<float>;
 using Polar16 = PolarOf<signed short>;
 // using Polar = PolarSingle;
 
-} // namespace LinearAlgebra
+}  // namespace LinearAlgebra
-} // namespace Passer
+using namespace LinearAlgebra;
-using namespace Passer::LinearAlgebra;
 
 #include "Spherical.h"
 #include "Vector2.h"

Quaternion.cpp

@@ -6,6 +6,7 @@
 #include <float.h>
 #include <math.h>
 #include "Angle.h"
+#include "Matrix.h"
 #include "Vector3.h"
 
 void CopyQuat(const Quat& q1, Quat& q2) {
@@ -97,6 +98,28 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) {
   }
 }
 
+Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
+  Matrix2 r = Matrix2(3, 3);
+
+  float x = this->x;
+  float y = this->y;
+  float z = this->z;
+  float w = this->w;
+
+  float* data = r.data;
+  data[0 * 3 + 0] = 1 - 2 * (y * y + z * z);
+  data[0 * 3 + 1] = 2 * (x * y - w * z);
+  data[0 * 3 + 2] = 2 * (x * z + w * y);
+  data[1 * 3 + 0] = 2 * (x * y + w * z);
+  data[1 * 3 + 1] = 1 - 2 * (x * x + z * z);
+  data[1 * 3 + 2] = 2 * (y * z - w * x);
+  data[2 * 3 + 0] = 2 * (x * z - w * y);
+  data[2 * 3 + 1] = 2 * (y * z + w * x);
+  data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
+
+  return r;
+}
+
 Quaternion Quaternion::operator*(const Quaternion& r2) const {
   return Quaternion(
       this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x,

Quaternion.h

@@ -32,14 +32,15 @@ typedef struct Quat {
 } Quat;
 }
 
-namespace Passer {
 namespace LinearAlgebra {
 
+class Matrix2;
+
 /// <summary>
 /// A quaternion
 /// </summary>
 struct Quaternion : Quat {
-public:
+ public:
   /// <summary>
   /// Create a new identity quaternion
   /// </summary>
@@ -80,7 +81,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 +89,16 @@ 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);
+
+  Matrix2 ToRotationMatrix();
 
   /// <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 +106,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 +117,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) const;
+  bool operator==(const Quaternion& quaternion) const;
 
   /// <summary>
   /// The inverse of quaterion
@@ -129,8 +132,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 +143,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 +160,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 +170,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 +194,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 +206,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 +266,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,20 +279,19 @@ 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);
 
   Vector3 xyz() const;
 };
 
-} // namespace LinearAlgebra
+}  // namespace LinearAlgebra
-} // namespace Passer
+using namespace LinearAlgebra;
-using namespace Passer::LinearAlgebra;
 
 #endif

Spherical.cpp

@@ -5,13 +5,17 @@
 
 #include <math.h>
 
-template <typename T> SphericalOf<T>::SphericalOf() {
+namespace LinearAlgebra {
+
+template <typename T>
+SphericalOf<T>::SphericalOf() {
   this->distance = 0.0f;
   this->direction = DirectionOf<T>();
 }
 
 template <typename T>
-SphericalOf<T>::SphericalOf(float distance, AngleOf<T> horizontal,
+SphericalOf<T>::SphericalOf(float distance,
+                            AngleOf<T> horizontal,
                             AngleOf<T> vertical) {
   if (distance < 0) {
     this->distance = -distance;
@@ -34,7 +38,8 @@ SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) {
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::Degrees(float distance, float horizontal,
+SphericalOf<T> SphericalOf<T>::Degrees(float distance,
+                                       float horizontal,
                                        float vertical) {
   AngleOf<T> horizontalAngle = AngleOf<T>::Degrees(horizontal);
   AngleOf<T> verticalAngle = AngleOf<T>::Degrees(vertical);
@@ -43,7 +48,8 @@ SphericalOf<T> SphericalOf<T>::Degrees(float distance, float horizontal,
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::Radians(float distance, float horizontal,
+SphericalOf<T> SphericalOf<T>::Radians(float distance,
+                                       float horizontal,
                                        float vertical) {
   return SphericalOf<T>(distance, AngleOf<T>::Radians(horizontal),
                         AngleOf<T>::Radians(vertical));
@@ -57,7 +63,8 @@ SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
   return r;
 }
 
-template <typename T> SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
+template <typename T>
+SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
   float distance = v.magnitude();
   if (distance == 0.0f) {
     return SphericalOf(distance, AngleOf<T>(), AngleOf<T>());
@@ -81,7 +88,8 @@ template <typename T> SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
  * @tparam T The type of the distance and direction values.
  * @return Vector3 The 3D vector representation of the spherical coordinates.
  */
-template <typename T> Vector3 SphericalOf<T>::ToVector3() const {
+template <typename T>
+Vector3 SphericalOf<T>::ToVector3() const {
   float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
   float horizontalRad = this->direction.horizontal.InRadians();
 
@@ -126,7 +134,8 @@ SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
   return v;
 }
 
-template <typename T> SphericalOf<T> SphericalOf<T>::operator-() const {
+template <typename T>
+SphericalOf<T> SphericalOf<T>::operator-() const {
   SphericalOf<T> v = SphericalOf<T>(
       this->distance, this->direction.horizontal + AngleOf<T>::Degrees(180),
       this->direction.vertical + AngleOf<T>::Degrees(180));
@@ -134,7 +143,7 @@ template <typename T> SphericalOf<T> SphericalOf<T>::operator-() const {
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T> &s2) const {
+SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
   // let's do it the easy way...
   Vector3 v1 = this->ToVector3();
   Vector3 v2 = s2.ToVector3();
@@ -143,13 +152,13 @@ SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T> &s2) const {
   return r;
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T> &v) {
+SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T>& v) {
   *this = *this - v;
   return *this;
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T> &s2) const {
+SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
   // let's do it the easy way...
   Vector3 v1 = this->ToVector3();
   Vector3 v2 = s2.ToVector3();
@@ -204,17 +213,19 @@ SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T> &s2) const {
   */
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T> &v) {
+SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T>& v) {
   *this = *this + v;
   return *this;
 }
 
-template <typename T> SphericalOf<T> SphericalOf<T>::operator*=(float f) {
+template <typename T>
+SphericalOf<T> SphericalOf<T>::operator*=(float f) {
   this->distance *= f;
   return *this;
 }
 
-template <typename T> SphericalOf<T> SphericalOf<T>::operator/=(float f) {
+template <typename T>
+SphericalOf<T> SphericalOf<T>::operator/=(float f) {
   this->distance /= f;
   return *this;
 }
@@ -225,8 +236,8 @@ template <typename T> SphericalOf<T> SphericalOf<T>::operator/=(float f) {
 const float epsilon = 1E-05f;
 
 template <typename T>
-float SphericalOf<T>::DistanceBetween(const SphericalOf<T> &v1,
+float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
-                                      const SphericalOf<T> &v2) {
+                                      const SphericalOf<T>& v2) {
   // SphericalOf<T> difference = v1 - v2;
   // return difference.distance;
   Vector3 vec1 = v1.ToVector3();
@@ -236,8 +247,8 @@ float SphericalOf<T>::DistanceBetween(const SphericalOf<T> &v1,
 }
 
 template <typename T>
-AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf &v1,
+AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
-                                        const SphericalOf &v2) {
+                                        const SphericalOf& v2) {
   // float denominator = v1.distance * v2.distance;
   // if (denominator < epsilon)
   //   return 0.0f;
@@ -256,9 +267,9 @@ AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf &v1,
 }
 
 template <typename T>
-AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
+AngleOf<T> SphericalOf<T>::SignedAngleBetween(const SphericalOf<T>& v1,
-    const SphericalOf<T> &v1, const SphericalOf<T> &v2,
+                                              const SphericalOf<T>& v2,
-    const SphericalOf<T> &axis) {
+                                              const SphericalOf<T>& axis) {
   Vector3 v1_vector = v1.ToVector3();
   Vector3 v2_vector = v2.ToVector3();
   Vector3 axis_vector = axis.ToVector3();
@@ -267,7 +278,7 @@ AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T> &v,
+SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
                                       AngleOf<T> horizontalAngle,
                                       AngleOf<T> verticalAngle) {
   SphericalOf<T> r =
@@ -276,19 +287,21 @@ SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T> &v,
   return r;
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T> &v,
+SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T>& v,
                                                 AngleOf<T> a) {
   SphericalOf<T> r =
       SphericalOf(v.distance, v.direction.horizontal + a, v.direction.vertical);
   return r;
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T> &v,
+SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
                                               AngleOf<T> a) {
   SphericalOf<T> r =
       SphericalOf(v.distance, v.direction.horizontal, v.direction.vertical + a);
   return r;
 }
 
-template class Passer::LinearAlgebra::SphericalOf<float>;
+template class SphericalOf<float>;
-template class Passer::LinearAlgebra::SphericalOf<signed short>;
+template class SphericalOf<signed short>;
+
+}  // namespace LinearAlgebra

Spherical.h

@@ -7,30 +7,26 @@
 
 #include "Direction.h"
 
-namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector3;
-template <typename T> class PolarOf;
+template <typename T>
+class PolarOf;
 
 /// @brief A spherical vector using angles in various representations
 /// @tparam T The implementation type used for the representations of the agles
-template <typename T> class SphericalOf {
+template <typename T>
-public:
+class SphericalOf {
+ public:
   /// @brief The distance in meters
   /// @remark The distance should never be negative
   float distance;
-  /// @brief The angle in the horizontal plane in degrees, clockwise rotation
+  /// @brief The direction of the vector
-  /// @details The angle is automatically normalized to -180 .. 180
-  // AngleOf<T> horizontal;
-  /// @brief The angle in the vertical plane in degrees. Positive is upward.
-  /// @details The angle is automatically normalized to -180 .. 180
-  // AngleOf<T> vertical;
   DirectionOf<T> direction;
 
-  SphericalOf<T>();
+  SphericalOf();
-  SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
+  SphericalOf(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
-  SphericalOf<T>(float distance, DirectionOf<T> direction);
+  SphericalOf(float distance, DirectionOf<T> direction);
 
   /// @brief Create spherical vector without using AngleOf type. All given
   /// angles are in degrees
@@ -38,7 +34,8 @@ public:
   /// @param horizontal The horizontal angle in degrees
   /// @param vertical The vertical angle in degrees
   /// @return The spherical vector
-  static SphericalOf<T> Degrees(float distance, float horizontal,
+  static SphericalOf<T> Degrees(float distance,
+                                float horizontal,
                                 float vertical);
   /// @brief Short-hand Deg alias for the Degrees function
   constexpr static auto Deg = Degrees;
@@ -48,7 +45,8 @@ public:
   /// @param horizontal The horizontal angle in radians
   /// @param vertical The vertical angle in radians
   /// @return The spherical vectpr
-  static SphericalOf<T> Radians(float distance, float horizontal,
+  static SphericalOf<T> Radians(float distance,
+                                float horizontal,
                                 float vertical);
   // Short-hand Rad alias for the Radians function
   constexpr static auto Rad = Radians;
@@ -95,23 +93,23 @@ public:
   /// @brief Subtract a spherical vector from this vector
   /// @param v The vector to subtract
   /// @return The result of the subtraction
-  SphericalOf<T> operator-(const SphericalOf<T> &v) const;
+  SphericalOf<T> operator-(const SphericalOf<T>& v) const;
-  SphericalOf<T> operator-=(const SphericalOf<T> &v);
+  SphericalOf<T> operator-=(const SphericalOf<T>& v);
   /// @brief Add a spherical vector to this vector
   /// @param v The vector to add
   /// @return The result of the addition
-  SphericalOf<T> operator+(const SphericalOf<T> &v) const;
+  SphericalOf<T> operator+(const SphericalOf<T>& v) const;
-  SphericalOf<T> operator+=(const SphericalOf<T> &v);
+  SphericalOf<T> operator+=(const SphericalOf<T>& v);
 
   /// @brief Scale the vector uniformly up
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark This operation will scale the distance of the vector. The angle
   /// will be unaffected.
-  friend SphericalOf<T> operator*(const SphericalOf<T> &v, float f) {
+  friend SphericalOf<T> operator*(const SphericalOf<T>& v, float f) {
     return SphericalOf<T>(v.distance * f, v.direction);
   }
-  friend SphericalOf<T> operator*(float f, const SphericalOf<T> &v) {
+  friend SphericalOf<T> operator*(float f, const SphericalOf<T>& v) {
     return SphericalOf<T>(f * v.distance, v.direction);
   }
   SphericalOf<T> operator*=(float f);
@@ -120,10 +118,10 @@ public:
   /// @return The scaled factor
   /// @remark This operation will scale the distance of the vector. The angle
   /// will be unaffected.
-  friend SphericalOf<T> operator/(const SphericalOf<T> &v, float f) {
+  friend SphericalOf<T> operator/(const SphericalOf<T>& v, float f) {
     return SphericalOf<T>(v.distance / f, v.direction);
   }
-  friend SphericalOf<T> operator/(float f, const SphericalOf<T> &v) {
+  friend SphericalOf<T> operator/(float f, const SphericalOf<T>& v) {
     return SphericalOf<T>(f / v.distance, v.direction);
   }
   SphericalOf<T> operator/=(float f);
@@ -132,41 +130,42 @@ public:
   /// @param v1 The first coordinate
   /// @param v2 The second coordinate
   /// @return The distance between the coordinates in meters
-  static float DistanceBetween(const SphericalOf<T> &v1,
+  static float DistanceBetween(const SphericalOf<T>& v1,
-                               const SphericalOf<T> &v2);
+                               const SphericalOf<T>& v2);
   /// @brief Calculate the unsigned angle between two spherical vectors
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The unsigned angle between the vectors
-  static AngleOf<T> AngleBetween(const SphericalOf<T> &v1,
+  static AngleOf<T> AngleBetween(const SphericalOf<T>& v1,
-                                 const SphericalOf<T> &v2);
+                                 const SphericalOf<T>& v2);
   /// @brief Calculate the signed angle between two spherical vectors
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @param axis The axis are which the angle is calculated
   /// @return The signed angle between the vectors
-  static AngleOf<T> SignedAngleBetween(const SphericalOf<T> &v1,
+  static AngleOf<T> SignedAngleBetween(const SphericalOf<T>& v1,
-                                       const SphericalOf<T> &v2,
+                                       const SphericalOf<T>& v2,
-                                       const SphericalOf<T> &axis);
+                                       const SphericalOf<T>& axis);
 
   /// @brief Rotate a spherical vector
   /// @param v The vector to rotate
   /// @param horizontalAngle The horizontal rotation angle in local space
   /// @param verticalAngle The vertical rotation angle in local space
   /// @return The rotated vector
-  static SphericalOf<T> Rotate(const SphericalOf &v, AngleOf<T> horizontalAngle,
+  static SphericalOf<T> Rotate(const SphericalOf& v,
+                               AngleOf<T> horizontalAngle,
                                AngleOf<T> verticalAngle);
   /// @brief Rotate a spherical vector horizontally
   /// @param v The vector to rotate
   /// @param angle The horizontal rotation angle in local space
   /// @return The rotated vector
-  static SphericalOf<T> RotateHorizontal(const SphericalOf<T> &v,
+  static SphericalOf<T> RotateHorizontal(const SphericalOf<T>& v,
                                          AngleOf<T> angle);
   /// @brief Rotate a spherical vector vertically
   /// @param v The vector to rotate
   /// @param angle The vertical rotation angle in local space
   /// @return The rotated vector
-  static SphericalOf<T> RotateVertical(const SphericalOf<T> &v,
+  static SphericalOf<T> RotateVertical(const SphericalOf<T>& v,
                                        AngleOf<T> angle);
 };
 
@@ -180,9 +179,13 @@ using SphericalSingle = SphericalOf<float>;
 /// hardware
 using Spherical16 = SphericalOf<signed short>;
 
-} // namespace LinearAlgebra
+#if defined(ARDUINO)
-} // namespace Passer
+using Spherical = Spherical16;
-using namespace Passer::LinearAlgebra;
+#else
+using Spherical = SphericalSingle;
+#endif
+
+}  // namespace LinearAlgebra
 
 #include "Polar.h"
 #include "Vector3.h"

SwingTwist.cpp

@@ -4,6 +4,8 @@
 
 #include "SwingTwist.h"
 
+namespace LinearAlgebra {
+
 template <typename T>
 SwingTwistOf<T>::SwingTwistOf() {
   this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
@@ -164,5 +166,7 @@ void SwingTwistOf<T>::Normalize() {
   }
 }
 
-template class Passer::LinearAlgebra::SwingTwistOf<float>;
+template class SwingTwistOf<float>;
-template class Passer::LinearAlgebra::SwingTwistOf<signed short>;
+template class SwingTwistOf<signed short>;
+
+}

SwingTwist.h

@@ -10,22 +10,23 @@
 #include "Quaternion.h"
 #include "Spherical.h"
 
-namespace Passer {
 namespace LinearAlgebra {
 
 /// @brief An orientation using swing and twist angles in various
 /// representations
 /// @tparam T The implmentation type used for the representation of the angles
-template <typename T> class SwingTwistOf {
+template <typename T>
-public:
+class SwingTwistOf {
+ public:
   DirectionOf<T> swing;
   AngleOf<T> twist;
 
-  SwingTwistOf<T>();
+  SwingTwistOf();
-  SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
+  SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist);
-  SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
+  SwingTwistOf(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
 
-  static SwingTwistOf<T> Degrees(float horizontal, float vertical = 0,
+  static SwingTwistOf<T> Degrees(float horizontal,
+                                 float vertical = 0,
                                  float twist = 0);
 
   Quaternion ToQuaternion() const;
@@ -43,7 +44,7 @@ public:
   /// </summary>
   /// <param name="vector">The vector to rotate</param>
   /// <returns>The rotated vector</returns>
-  SphericalOf<T> operator*(const SphericalOf<T> &vector) const;
+  SphericalOf<T> operator*(const SphericalOf<T>& vector) const;
   /// <summary>
   /// Multiply this rotation with another rotation
   /// </summary>
@@ -51,8 +52,8 @@ public:
   /// <returns>The resulting swing/twist rotation</returns>
   /// The result will be this rotation rotated according to
   /// the give rotation.
-  SwingTwistOf<T> operator*(const SwingTwistOf<T> &rotation) const;
+  SwingTwistOf<T> operator*(const SwingTwistOf<T>& rotation) const;
-  SwingTwistOf<T> operator*=(const SwingTwistOf<T> &rotation);
+  SwingTwistOf<T> operator*=(const SwingTwistOf<T>& rotation);
 
   static SwingTwistOf<T> Inverse(SwingTwistOf<T> rotation);
 
@@ -62,9 +63,9 @@ public:
   /// <param name="angle">The angle</param>
   /// <param name="axis">The axis</param>
   /// <returns>The resulting quaternion</returns>
-  static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T> &axis);
+  static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T>& axis);
 
-  static AngleOf<T> Angle(const SwingTwistOf<T> &r1, const SwingTwistOf<T> &r2);
+  static AngleOf<T> Angle(const SwingTwistOf<T>& r1, const SwingTwistOf<T>& r2);
 
   void Normalize();
 };
@@ -72,8 +73,13 @@ public:
 using SwingTwistSingle = SwingTwistOf<float>;
 using SwingTwist16 = SwingTwistOf<signed short>;
 
-} // namespace LinearAlgebra
+#if defined(ARDUINO)
-} // namespace Passer
+using SwingTwist = SwingTwist16;
-using namespace Passer::LinearAlgebra;
+#else
+using SwingTwist = SwingTwistSingle;
+#endif
+
+}  // namespace LinearAlgebra
+using namespace LinearAlgebra;
 
 #endif

Vector2.cpp

@@ -26,11 +26,11 @@ Vector2::Vector2(float _x, float _y) {
 //   y = v.y;
 // }
 Vector2::Vector2(Vector3 v) {
-  x = v.Right();   // x;
+  x = v.Right();    // x;
-  y = v.Forward(); // z;
+  y = v.Forward();  // z;
 }
 Vector2::Vector2(PolarSingle p) {
-  float horizontalRad = p.angle.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
+  float horizontalRad = p.angle.InDegrees() * Deg2Rad;
   float cosHorizontal = cosf(horizontalRad);
   float sinHorizontal = sinf(horizontalRad);
 
@@ -49,18 +49,24 @@ const Vector2 Vector2::down = Vector2(0, -1);
 const Vector2 Vector2::forward = Vector2(0, 1);
 const Vector2 Vector2::back = Vector2(0, -1);
 
-bool Vector2::operator==(const Vector2 &v) {
+bool Vector2::operator==(const Vector2& v) {
   return (this->x == v.x && this->y == v.y);
 }
 
-float Vector2::Magnitude(const Vector2 &v) {
+float Vector2::Magnitude(const Vector2& v) {
   return sqrtf(v.x * v.x + v.y * v.y);
 }
-float Vector2::magnitude() const { return (float)sqrtf(x * x + y * y); }
+float Vector2::magnitude() const {
-float Vector2::SqrMagnitude(const Vector2 &v) { return v.x * v.x + v.y * v.y; }
+  return (float)sqrtf(x * x + y * y);
-float Vector2::sqrMagnitude() const { return (x * x + y * y); }
+}
+float Vector2::SqrMagnitude(const Vector2& v) {
+  return v.x * v.x + v.y * v.y;
+}
+float Vector2::sqrMagnitude() const {
+  return (x * x + y * y);
+}
 
-Vector2 Vector2::Normalize(const Vector2 &v) {
+Vector2 Vector2::Normalize(const Vector2& v) {
   float num = Vector2::Magnitude(v);
   Vector2 result = Vector2::zero;
   if (num > Float::epsilon) {
@@ -77,26 +83,28 @@ Vector2 Vector2::normalized() const {
   return result;
 }
 
-Vector2 Vector2::operator-() { return Vector2(-this->x, -this->y); }
+Vector2 Vector2::operator-() {
+  return Vector2(-this->x, -this->y);
+}
 
-Vector2 Vector2::operator-(const Vector2 &v) const {
+Vector2 Vector2::operator-(const Vector2& v) const {
   return Vector2(this->x - v.x, this->y - v.y);
 }
-Vector2 Vector2::operator-=(const Vector2 &v) {
+Vector2 Vector2::operator-=(const Vector2& v) {
   this->x -= v.x;
   this->y -= v.y;
   return *this;
 }
-Vector2 Vector2::operator+(const Vector2 &v) const {
+Vector2 Vector2::operator+(const Vector2& v) const {
   return Vector2(this->x + v.x, this->y + v.y);
 }
-Vector2 Vector2::operator+=(const Vector2 &v) {
+Vector2 Vector2::operator+=(const Vector2& v) {
   this->x += v.x;
   this->y += v.y;
   return *this;
 }
 
-Vector2 Vector2::Scale(const Vector2 &v1, const Vector2 &v2) {
+Vector2 Vector2::Scale(const Vector2& v1, const Vector2& v2) {
   return Vector2(v1.x * v2.x, v1.y * v2.y);
 }
 // Vector2 Passer::LinearAlgebra::operator*(const Vector2 &v, float f) {
@@ -122,18 +130,18 @@ Vector2 Vector2::operator/=(float f) {
   return *this;
 }
 
-float Vector2::Dot(const Vector2 &v1, const Vector2 &v2) {
+float Vector2::Dot(const Vector2& v1, const Vector2& v2) {
   return v1.x * v2.x + v1.y * v2.y;
 }
 
-float Vector2::Distance(const Vector2 &v1, const Vector2 &v2) {
+float Vector2::Distance(const Vector2& v1, const Vector2& v2) {
   return Magnitude(v1 - v2);
 }
 
-float Vector2::Angle(const Vector2 &v1, const Vector2 &v2) {
+float Vector2::Angle(const Vector2& v1, const Vector2& v2) {
   return (float)fabs(SignedAngle(v1, v2));
 }
-float Vector2::SignedAngle(const Vector2 &v1, const Vector2 &v2) {
+float Vector2::SignedAngle(const Vector2& v1, const Vector2& v2) {
   float sqrMagFrom = v1.sqrMagnitude();
   float sqrMagTo = v2.sqrMagnitude();
 
@@ -148,15 +156,14 @@ float Vector2::SignedAngle(const Vector2 &v1, const Vector2 &v2) {
 
   float angleFrom = atan2f(v1.y, v1.x);
   float angleTo = atan2f(v2.y, v2.x);
-  return -(angleTo - angleFrom) * Passer::LinearAlgebra::Rad2Deg;
+  return -(angleTo - angleFrom) * Rad2Deg;
 }
 
-Vector2 Vector2::Rotate(const Vector2 &v,
+Vector2 Vector2::Rotate(const Vector2& v, AngleSingle a) {
-                        Passer::LinearAlgebra::AngleSingle a) {
+  float angleRad = a.InDegrees() * Deg2Rad;
-  float angleRad = a.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
 #if defined(AVR)
   float sinValue = sin(angleRad);
-  float cosValue = cos(angleRad); // * Angle::Deg2Rad);
+  float cosValue = cos(angleRad);  // * Angle::Deg2Rad);
 #else
   float sinValue = (float)sinf(angleRad);
   float cosValue = (float)cosf(angleRad);
@@ -169,7 +176,7 @@ Vector2 Vector2::Rotate(const Vector2 &v,
   return r;
 }
 
-Vector2 Vector2::Lerp(const Vector2 &v1, const Vector2 &v2, float f) {
+Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) {
   Vector2 v = v1 + (v2 - v1) * f;
   return v;
 }

Vector2.h

@@ -26,11 +26,11 @@ typedef struct Vec2 {
 } Vec2;
 }
 
-namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector3;
-template <typename T> class PolarOf;
+template <typename T>
+class PolarOf;
 
 /// @brief A 2-dimensional vector
 /// @remark This uses the right=handed carthesian coordinate system.
@@ -38,7 +38,7 @@ template <typename T> class PolarOf;
 struct Vector2 : Vec2 {
   friend struct Vec2;
 
-public:
+ public:
   /// @brief A new 2-dimensional zero vector
   Vector2();
   /// @brief A new 2-dimensional vector
@@ -80,12 +80,12 @@ public:
   /// @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 Vector2 &v);
+  bool operator==(const Vector2& v);
 
   /// @brief The vector length
   /// @param v The vector for which you need the length
   /// @return The vector length
-  static float Magnitude(const Vector2 &v);
+  static float Magnitude(const Vector2& v);
   /// @brief The vector length
   /// @return The vector length
   float magnitude() const;
@@ -95,7 +95,7 @@ public:
   /// @remark The squared length is computationally simpler than the real
   /// length. Think of Pythagoras A^2 + B^2 = C^2. This prevents the calculation
   /// of the squared root of C.
-  static float SqrMagnitude(const Vector2 &v);
+  static float SqrMagnitude(const Vector2& v);
   /// @brief The squared vector length
   /// @return The squared vector length
   /// @remark The squared length is computationally simpler than the real
@@ -106,7 +106,7 @@ public:
   /// @brief Convert the vector to a length of 1
   /// @param v The vector to convert
   /// @return The vector normalized to a length of 1
-  static Vector2 Normalize(const Vector2 &v);
+  static Vector2 Normalize(const Vector2& v);
   /// @brief Convert the vector to a length 1
   /// @return The vector normalized to a length of 1
   Vector2 normalized() const;
@@ -118,13 +118,13 @@ public:
   /// @brief Subtract a vector from this vector
   /// @param v The vector to subtract from this vector
   /// @return The result of the subtraction
-  Vector2 operator-(const Vector2 &v) const;
+  Vector2 operator-(const Vector2& v) const;
-  Vector2 operator-=(const Vector2 &v);
+  Vector2 operator-=(const Vector2& v);
   /// @brief Add a vector to this vector
   /// @param v The vector to add to this vector
   /// @return The result of the addition
-  Vector2 operator+(const Vector2 &v) const;
+  Vector2 operator+(const Vector2& v) const;
-  Vector2 operator+=(const Vector2 &v);
+  Vector2 operator+=(const Vector2& v);
 
   /// @brief Scale the vector using another vector
   /// @param v1 The vector to scale
@@ -132,16 +132,16 @@ public:
   /// @return The scaled vector
   /// @remark Each component of the vector v1 will be multiplied with the
   /// matching component from the scaling vector v2.
-  static Vector2 Scale(const Vector2 &v1, const Vector2 &v2);
+  static Vector2 Scale(const Vector2& v1, const Vector2& v2);
   /// @brief Scale the vector uniformly up
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark Each component of the vector will be multipled with the same
   /// factor f.
-  friend Vector2 operator*(const Vector2 &v, float f) {
+  friend Vector2 operator*(const Vector2& v, float f) {
     return Vector2(v.x * f, v.y * f);
   }
-  friend Vector2 operator*(float f, const Vector2 &v) {
+  friend Vector2 operator*(float f, const Vector2& v) {
     return Vector2(v.x * f, v.y * f);
     // return Vector2(f * v.x, f * v.y);
   }
@@ -150,10 +150,10 @@ public:
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark Each componet of the vector will be divided by the same factor.
-  friend Vector2 operator/(const Vector2 &v, float f) {
+  friend Vector2 operator/(const Vector2& v, float f) {
     return Vector2(v.x / f, v.y / f);
   }
-  friend Vector2 operator/(float f, const Vector2 &v) {
+  friend Vector2 operator/(float f, const Vector2& v) {
     return Vector2(f / v.x, f / v.y);
   }
   Vector2 operator/=(float f);
@@ -162,13 +162,13 @@ public:
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The dot product of the two vectors
-  static float Dot(const Vector2 &v1, const Vector2 &v2);
+  static float Dot(const Vector2& v1, const Vector2& v2);
 
   /// @brief The distance between two vectors
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The distance between the two vectors
-  static float Distance(const Vector2 &v1, const Vector2 &v2);
+  static float Distance(const Vector2& v1, const Vector2& v2);
 
   /// @brief The angle between two vectors
   /// @param v1 The first vector
@@ -177,18 +177,18 @@ public:
   /// @remark This reterns an unsigned angle which is the shortest distance
   /// between the two vectors. Use Vector2::SignedAngle if a signed angle is
   /// needed.
-  static float Angle(const Vector2 &v1, const Vector2 &v2);
+  static float Angle(const Vector2& v1, const Vector2& v2);
   /// @brief The signed angle between two vectors
   /// @param v1 The starting vector
   /// @param v2 The ending vector
   /// @return The signed angle between the two vectors
-  static float SignedAngle(const Vector2 &v1, const Vector2 &v2);
+  static float SignedAngle(const Vector2& v1, const Vector2& v2);
 
   /// @brief Rotate the vector
   /// @param v The vector to rotate
   /// @param a The angle in degrees to rotate
   /// @return The rotated vector
-  static Vector2 Rotate(const Vector2 &v, Passer::LinearAlgebra::AngleSingle a);
+  static Vector2 Rotate(const Vector2& v, AngleSingle a);
 
   /// @brief Lerp (linear interpolation) between two vectors
   /// @param v1 The starting vector
@@ -198,12 +198,11 @@ public:
   /// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
   /// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference
   /// between *v1* and *v2* etc.
-  static Vector2 Lerp(const Vector2 &v1, const Vector2 &v2, float f);
+  static Vector2 Lerp(const Vector2& v1, const Vector2& v2, float f);
 };
 
-} // namespace LinearAlgebra
+}  // namespace LinearAlgebra
-} // namespace Passer
+using namespace LinearAlgebra;
-using namespace Passer::LinearAlgebra;
 
 #include "Polar.h"
 

Vector3.cpp

@@ -31,10 +31,8 @@ Vector3::Vector3(Vector2 v) {
 }
 
 Vector3::Vector3(SphericalOf<float> s) {
-  float verticalRad = (90.0f - s.direction.vertical.InDegrees()) *
+  float verticalRad = (90.0f - s.direction.vertical.InDegrees()) * Deg2Rad;
-                      Passer::LinearAlgebra::Deg2Rad;
+  float horizontalRad = s.direction.horizontal.InDegrees() * Deg2Rad;
-  float horizontalRad =
-      s.direction.horizontal.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
   float cosVertical = cosf(verticalRad);
   float sinVertical = sinf(verticalRad);
   float cosHorizontal = cosf(horizontalRad);
@@ -67,17 +65,21 @@ const Vector3 Vector3::back = Vector3(0, 0, -1);
 //   return Vector3(v.x, 0, v.y);
 // }
 
-float Vector3::Magnitude(const Vector3 &v) {
+float Vector3::Magnitude(const Vector3& v) {
   return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
 }
-float Vector3::magnitude() const { return (float)sqrtf(x * x + y * y + z * z); }
+float Vector3::magnitude() const {
+  return (float)sqrtf(x * x + y * y + z * z);
+}
 
-float Vector3::SqrMagnitude(const Vector3 &v) {
+float Vector3::SqrMagnitude(const Vector3& v) {
   return v.x * v.x + v.y * v.y + v.z * v.z;
 }
-float Vector3::sqrMagnitude() const { return (x * x + y * y + z * z); }
+float Vector3::sqrMagnitude() const {
+  return (x * x + y * y + z * z);
+}
 
-Vector3 Vector3::Normalize(const Vector3 &v) {
+Vector3 Vector3::Normalize(const Vector3& v) {
   float num = Vector3::Magnitude(v);
   Vector3 result = Vector3::zero;
   if (num > epsilon) {
@@ -98,26 +100,26 @@ Vector3 Vector3::operator-() const {
   return Vector3(-this->x, -this->y, -this->z);
 }
 
-Vector3 Vector3::operator-(const Vector3 &v) const {
+Vector3 Vector3::operator-(const Vector3& v) const {
   return Vector3(this->x - v.x, this->y - v.y, this->z - v.z);
 }
-Vector3 Vector3::operator-=(const Vector3 &v) {
+Vector3 Vector3::operator-=(const Vector3& v) {
   this->x -= v.x;
   this->y -= v.y;
   this->z -= v.z;
   return *this;
 }
-Vector3 Vector3::operator+(const Vector3 &v) const {
+Vector3 Vector3::operator+(const Vector3& v) const {
   return Vector3(this->x + v.x, this->y + v.y, this->z + v.z);
 }
-Vector3 Vector3::operator+=(const Vector3 &v) {
+Vector3 Vector3::operator+=(const Vector3& v) {
   this->x += v.x;
   this->y += v.y;
   this->z += v.z;
   return *this;
 }
 
-Vector3 Vector3::Scale(const Vector3 &v1, const Vector3 &v2) {
+Vector3 Vector3::Scale(const Vector3& v1, const Vector3& v2) {
   return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
 }
 // Vector3 Passer::LinearAlgebra::operator*(const Vector3 &v, float f) {
@@ -145,24 +147,24 @@ Vector3 Vector3::operator/=(float f) {
   return *this;
 }
 
-float Vector3::Dot(const Vector3 &v1, const Vector3 &v2) {
+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) const {
+bool Vector3::operator==(const Vector3& v) const {
   return (this->x == v.x && this->y == v.y && this->z == v.z);
 }
 
-float Vector3::Distance(const Vector3 &v1, const Vector3 &v2) {
+float Vector3::Distance(const Vector3& v1, const Vector3& v2) {
   return Magnitude(v1 - v2);
 }
 
-Vector3 Vector3::Cross(const Vector3 &v1, const Vector3 &v2) {
+Vector3 Vector3::Cross(const Vector3& v1, const Vector3& v2) {
   return Vector3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z,
                  v1.x * v2.y - v1.y * v2.x);
 }
 
-Vector3 Vector3::Project(const Vector3 &v, const Vector3 &n) {
+Vector3 Vector3::Project(const Vector3& v, const Vector3& n) {
   float sqrMagnitude = Dot(n, n);
   if (sqrMagnitude < epsilon)
     return Vector3::zero;
@@ -173,7 +175,7 @@ Vector3 Vector3::Project(const Vector3 &v, const Vector3 &n) {
   }
 }
 
-Vector3 Vector3::ProjectOnPlane(const Vector3 &v, const Vector3 &n) {
+Vector3 Vector3::ProjectOnPlane(const Vector3& v, const Vector3& n) {
   Vector3 r = v - Project(v, n);
   return r;
 }
@@ -184,7 +186,7 @@ float clamp(float x, float lower, float upper) {
   return upperClamp;
 }
 
-AngleOf<float> Vector3::Angle(const Vector3 &v1, const Vector3 &v2) {
+AngleOf<float> Vector3::Angle(const Vector3& v1, const Vector3& v2) {
   float denominator = sqrtf(v1.sqrMagnitude() * v2.sqrMagnitude());
   if (denominator < epsilon)
     return AngleOf<float>();
@@ -193,15 +195,16 @@ AngleOf<float> Vector3::Angle(const Vector3 &v1, const Vector3 &v2) {
   float fraction = dot / denominator;
   if (isnan(fraction))
     return AngleOf<float>::Degrees(
-        fraction); // short cut to returning NaN universally
+        fraction);  // short cut to returning NaN universally
 
   float cdot = clamp(fraction, -1.0, 1.0);
   float r = ((float)acos(cdot));
   return AngleOf<float>::Radians(r);
 }
 
-AngleOf<float> Vector3::SignedAngle(const Vector3 &v1, const Vector3 &v2,
+AngleOf<float> Vector3::SignedAngle(const Vector3& v1,
-                                    const Vector3 &axis) {
+                                    const Vector3& v2,
+                                    const Vector3& axis) {
   // angle in [0,180]
   AngleOf<float> angle = Vector3::Angle(v1, v2);
 
@@ -215,7 +218,7 @@ AngleOf<float> Vector3::SignedAngle(const Vector3 &v1, const Vector3 &v2,
   return AngleOf<float>(signed_angle);
 }
 
-Vector3 Vector3::Lerp(const Vector3 &v1, const Vector3 &v2, float f) {
+Vector3 Vector3::Lerp(const Vector3& v1, const Vector3& v2, float f) {
   Vector3 v = v1 + (v2 - v1) * f;
   return v;
 }

Vector3.h

@@ -14,7 +14,7 @@ extern "C" {
 /// This is a C-style implementation
 /// This uses the right-handed coordinate system.
 typedef struct Vec3 {
-protected:
+ public:
   /// <summary>
   /// The right axis of the vector
   /// </summary>
@@ -31,10 +31,10 @@ protected:
 } Vec3;
 }
 
-namespace Passer {
 namespace LinearAlgebra {
 
-template <typename T> class SphericalOf;
+template <typename T>
+class SphericalOf;
 
 /// @brief A 3-dimensional vector
 /// @remark This uses a right-handed carthesian coordinate system.
@@ -42,7 +42,7 @@ template <typename T> class SphericalOf;
 struct Vector3 : Vec3 {
   friend struct Vec3;
 
-public:
+ public:
   /// @brief A new 3-dimensional zero vector
   Vector3();
   /// @brief A new 3-dimensional vector
@@ -88,12 +88,12 @@ public:
   /// @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) const;
+  bool operator==(const Vector3& v) const;
 
   /// @brief The vector length
   /// @param v The vector for which you need the length
   /// @return The vector length
-  static float Magnitude(const Vector3 &v);
+  static float Magnitude(const Vector3& v);
   /// @brief The vector length
   /// @return The vector length
   float magnitude() const;
@@ -103,7 +103,7 @@ public:
   /// @remark The squared length is computationally simpler than the real
   /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
   /// calculation of the squared root of C.
-  static float SqrMagnitude(const Vector3 &v);
+  static float SqrMagnitude(const Vector3& v);
   /// @brief The squared vector length
   /// @return The squared vector length
   /// @remark The squared length is computationally simpler than the real
@@ -114,7 +114,7 @@ public:
   /// @brief Convert the vector to a length of 1
   /// @param v The vector to convert
   /// @return The vector normalized to a length of 1
-  static Vector3 Normalize(const Vector3 &v);
+  static Vector3 Normalize(const Vector3& v);
   /// @brief Convert the vector to a length of 1
   /// @return The vector normalized to a length of 1
   Vector3 normalized() const;
@@ -126,13 +126,13 @@ public:
   /// @brief Subtract a vector from this vector
   /// @param v The vector to subtract from this vector
   /// @return The result of this subtraction
-  Vector3 operator-(const Vector3 &v) const;
+  Vector3 operator-(const Vector3& v) const;
-  Vector3 operator-=(const Vector3 &v);
+  Vector3 operator-=(const Vector3& v);
   /// @brief Add a vector to this vector
   /// @param v The vector to add to this vector
   /// @return The result of the addition
-  Vector3 operator+(const Vector3 &v) const;
+  Vector3 operator+(const Vector3& v) const;
-  Vector3 operator+=(const Vector3 &v);
+  Vector3 operator+=(const Vector3& v);
 
   /// @brief Scale the vector using another vector
   /// @param v1 The vector to scale
@@ -140,16 +140,16 @@ public:
   /// @return The scaled vector
   /// @remark Each component of the vector v1 will be multiplied with the
   /// matching component from the scaling vector v2.
-  static Vector3 Scale(const Vector3 &v1, const Vector3 &v2);
+  static Vector3 Scale(const Vector3& v1, const Vector3& v2);
   /// @brief Scale the vector uniformly up
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark Each component of the vector will be multipled with the same
   /// factor f.
-  friend Vector3 operator*(const Vector3 &v, float f) {
+  friend Vector3 operator*(const Vector3& v, float f) {
     return Vector3(v.x * f, v.y * f, v.z * f);
   }
-  friend Vector3 operator*(float f, const Vector3 &v) {
+  friend Vector3 operator*(float f, const Vector3& v) {
     // return Vector3(f * v.x, f * v.y, f * v.z);
     return Vector3(v.x * f, v.y * f, v.z * f);
   }
@@ -158,10 +158,10 @@ public:
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark Each componet of the vector will be divided by the same factor.
-  friend Vector3 operator/(const Vector3 &v, float f) {
+  friend Vector3 operator/(const Vector3& v, float f) {
     return Vector3(v.x / f, v.y / f, v.z / f);
   }
-  friend Vector3 operator/(float f, const Vector3 &v) {
+  friend Vector3 operator/(float f, const Vector3& v) {
     // return Vector3(f / v.x, f / v.y, f / v.z);
     return Vector3(v.x / f, v.y / f, v.z / f);
   }
@@ -171,31 +171,31 @@ public:
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The distance between the two vectors
-  static float Distance(const Vector3 &v1, const Vector3 &v2);
+  static float Distance(const Vector3& v1, const Vector3& v2);
 
   /// @brief The dot product of two vectors
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The dot product of the two vectors
-  static float Dot(const Vector3 &v1, const Vector3 &v2);
+  static float Dot(const Vector3& v1, const Vector3& v2);
 
   /// @brief The cross product of two vectors
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The cross product of the two vectors
-  static Vector3 Cross(const Vector3 &v1, const Vector3 &v2);
+  static Vector3 Cross(const Vector3& v1, const Vector3& v2);
 
   /// @brief Project the vector on another vector
   /// @param v The vector to project
   /// @param n The normal vecto to project on
   /// @return The projected vector
-  static Vector3 Project(const Vector3 &v, const Vector3 &n);
+  static Vector3 Project(const Vector3& v, const Vector3& n);
   /// @brief Project the vector on a plane defined by a normal orthogonal to the
   /// plane.
   /// @param v The vector to project
   /// @param n The normal of the plane to project on
   /// @return Teh projected vector
-  static Vector3 ProjectOnPlane(const Vector3 &v, const Vector3 &n);
+  static Vector3 ProjectOnPlane(const Vector3& v, const Vector3& n);
 
   /// @brief The angle between two vectors
   /// @param v1 The first vector
@@ -204,14 +204,15 @@ public:
   /// @remark This reterns an unsigned angle which is the shortest distance
   /// between the two vectors. Use Vector3::SignedAngle if a signed angle is
   /// needed.
-  static AngleOf<float> Angle(const Vector3 &v1, const Vector3 &v2);
+  static AngleOf<float> Angle(const Vector3& v1, const Vector3& v2);
   /// @brief The signed angle between two vectors
   /// @param v1 The starting vector
   /// @param v2 The ending vector
   /// @param axis The axis to rotate around
   /// @return The signed angle between the two vectors
-  static AngleOf<float> SignedAngle(const Vector3 &v1, const Vector3 &v2,
+  static AngleOf<float> SignedAngle(const Vector3& v1,
-                                    const Vector3 &axis);
+                                    const Vector3& v2,
+                                    const Vector3& axis);
 
   /// @brief Lerp (linear interpolation) between two vectors
   /// @param v1 The starting vector
@@ -221,12 +222,11 @@ public:
   /// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
   /// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference
   /// between *v1* and *v2* etc.
-  static Vector3 Lerp(const Vector3 &v1, const Vector3 &v2, float f);
+  static Vector3 Lerp(const Vector3& v1, const Vector3& v2, float f);
 };
 
-} // namespace LinearAlgebra
+}  // namespace LinearAlgebra
-} // namespace Passer
+using namespace LinearAlgebra;
-using namespace Passer::LinearAlgebra;
 
 #include "Spherical.h"
 

test/Angle16_test.cc

@@ -1,11 +1,13 @@
 #if GTEST
 #include "gtest/gtest.h"
 
-#include <limits>
 #include <math.h>
+#include <limits>
 
 #include "Angle.h"
 
+using namespace LinearAlgebra;
+
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
 TEST(Angle16, Construct) {
@@ -86,7 +88,7 @@ TEST(Angle16, Normalize) {
   r = Angle16::Normalize(Angle16::Degrees(0));
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
 
-  if (false) { // std::numeric_limits<float>::is_iec559) {
+  if (false) {  // std::numeric_limits<float>::is_iec559) {
     // Infinites are not supported
     r = Angle16::Normalize(Angle16::Degrees(FLOAT_INFINITY));
     EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
@@ -125,7 +127,7 @@ TEST(Angle16, Clamp) {
                      Angle16::Degrees(-10));
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10";
 
-  if (false) { // std::numeric_limits<float>::is_iec559) {
+  if (false) {  // std::numeric_limits<float>::is_iec559) {
     // Infinites are not supported
     r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
                        Angle16::Degrees(FLOAT_INFINITY));
@@ -216,7 +218,7 @@ TEST(Angle16, MoveTowards) {
   r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(0), 30);
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
 
-  if (false) { // std::numeric_limits<float>::is_iec559) {
+  if (false) {  // std::numeric_limits<float>::is_iec559) {
     // infinites are not supported
     r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90),
                              FLOAT_INFINITY);

test/Angle8_test.cc

@@ -1,11 +1,13 @@
 #if GTEST
 #include <gtest/gtest.h>
 
-#include <limits>
 #include <math.h>
+#include <limits>
 
 #include "Angle.h"
 
+using namespace LinearAlgebra;
+
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
 TEST(Angle8, Construct) {
@@ -86,7 +88,7 @@ TEST(Angle8, Normalize) {
   r = Angle8::Normalize(Angle8::Degrees(0));
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
 
-  if (false) { // std::numeric_limits<float>::is_iec559) {
+  if (false) {  // std::numeric_limits<float>::is_iec559) {
     // Infinites are not supported
     r = Angle8::Normalize(Angle8::Degrees(FLOAT_INFINITY));
     EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
@@ -124,7 +126,7 @@ TEST(Angle8, Clamp) {
                     Angle8::Degrees(-10));
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10";
 
-  if (false) { // std::numeric_limits<float>::is_iec559) {
+  if (false) {  // std::numeric_limits<float>::is_iec559) {
     // Infinites are not supported
     r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
                       Angle8::Degrees(FLOAT_INFINITY));
@@ -215,7 +217,7 @@ TEST(Angle8, MoveTowards) {
   r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(0), 30);
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
 
-  if (false) { // std::numeric_limits<float>::is_iec559) {
+  if (false) {  // std::numeric_limits<float>::is_iec559) {
     // infinites are not supported
     r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90),
                             FLOAT_INFINITY);

test/AngleSingle_test.cc

@@ -1,11 +1,13 @@
 #if GTEST
 #include <gtest/gtest.h>
 
-#include <limits>
 #include <math.h>
+#include <limits>
 
 #include "Angle.h"
 
+using namespace LinearAlgebra;
+
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
 TEST(AngleSingle, Construct) {

test/Direction_test.cc

@@ -6,6 +6,8 @@
 
 #include "Direction.h"
 
+using namespace LinearAlgebra;
+
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
 TEST(Direction16, Compare) {

test/Matrix_test.cc

@@ -1,10 +1,90 @@
 #if GTEST
 #include <gtest/gtest.h>
-#include <limits>
 #include <math.h>
+#include <limits>
 
 #include "Matrix.h"
 
+TEST(Matrix2, Zero) {
+      // Test case 1: 2x2 zero matrix
+      Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
+      EXPECT_TRUE(zeroMatrix.nRows == 2);
+      EXPECT_TRUE(zeroMatrix.nCols == 2);
+      for (int i = 0; i < zeroMatrix.nValues; ++i) {
+        EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
+      }
+      std::cout << "Test case 1 passed: 2x2 zero matrix\n";
+  
+      // Test case 2: 3x3 zero matrix
+      zeroMatrix = Matrix2::Zero(3, 3);
+      EXPECT_TRUE(zeroMatrix.nRows == 3);
+      EXPECT_TRUE(zeroMatrix.nCols == 3);
+      for (int i = 0; i < zeroMatrix.nValues; ++i) {
+        EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
+      }
+      std::cout << "Test case 2 passed: 3x3 zero matrix\n";
+  
+      // Test case 3: 1x1 zero matrix
+      zeroMatrix = Matrix2::Zero(1, 1);
+      EXPECT_TRUE(zeroMatrix.nRows == 1);
+      EXPECT_TRUE(zeroMatrix.nCols == 1);
+      EXPECT_TRUE(zeroMatrix.data[0] == 0.0f);
+      std::cout << "Test case 3 passed: 1x1 zero matrix\n";
+  
+      // Test case 4: 0x0 matrix (edge case)
+      zeroMatrix = Matrix2::Zero(0, 0);
+      EXPECT_TRUE(zeroMatrix.nRows == 0);
+      EXPECT_TRUE(zeroMatrix.nCols == 0);
+      EXPECT_TRUE(zeroMatrix.data == nullptr);
+      std::cout << "Test case 4 passed: 0x0 matrix\n";
+}
+
+TEST(Matrix2, Multiplication) {
+  // Test 1: Multiplying two 2x2 matrices
+  float dataA[] = {1, 2, 3, 4};
+  float dataB[] = {5, 6, 7, 8};
+  Matrix2 A(dataA, 2, 2);
+  Matrix2 B(dataB, 2, 2);
+
+  Matrix2 result = A * B;
+
+  float expectedData[] = {19, 22, 43, 50};
+  for (int i = 0; i < 4; ++i) 
+    EXPECT_TRUE(result.data[i] == expectedData[i]);  
+  std::cout << "Test 1 passed: 2x2 matrix multiplication.\n";
+
+  
+  // Test 2: Multiplying a 3x2 matrix with a 2x3 matrix
+  float dataC[] = {1, 2, 3, 4, 5, 6};
+  float dataD[] = {7, 8, 9, 10, 11, 12};
+  Matrix2 C(dataC, 3, 2);
+  Matrix2 D(dataD, 2, 3);
+
+  Matrix2 result2 = C * D;
+
+  float expectedData2[] = {27, 30, 33, 61, 68, 75, 95, 106, 117};
+  for (int i = 0; i < 9; ++i)
+    EXPECT_TRUE(result2.data[i] == expectedData2[i]);
+  std::cout << "Test 2 passed: 3x2 * 2x3 matrix multiplication.\n";
+  
+  // Test 3: Multiplying with a zero matrix
+  Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
+  Matrix2 result3 = A * zeroMatrix;
+
+  for (int i = 0; i < 4; ++i)
+    EXPECT_TRUE(result3.data[i] == 0);
+  std::cout << "Test 3 passed: Multiplication with zero matrix.\n";
+  
+  // Test 4: Multiplying with an identity matrix
+  Matrix2 identityMatrix = Matrix2::Identity(2);
+  Matrix2 result4 = A * identityMatrix;
+
+  for (int i = 0; i < 4; ++i)
+    EXPECT_TRUE(result4.data[i] == A.data[i]);
+  std::cout << "Test 4 passed: Multiplication with identity matrix.\n";
+  
+}
+
 TEST(MatrixSingle, Init) {
   // zero
   MatrixOf<float> m0 = MatrixOf<float>(0, 0);

test/Vector2_test.cc

@@ -34,26 +34,32 @@ TEST(Vector2, FromPolar) {
   EXPECT_FLOAT_EQ(r.y, 0.0F) << "FromPolar(0 0)";
 }
 
-TEST(Vector2, Equality) {
+TEST(Vector2, Magnitude) {
-  Vector2 v1 = Vector2(4, 5);
+  Vector2 v = Vector2(1, 2);
-  Vector2 v2 = Vector2(1, 2);
+  float m = 0;
-  bool r = false;
 
-  r = v1 == v2;
+  m = v.magnitude();
-  EXPECT_FALSE(r) << "4 5 == 1 2";
+  EXPECT_FLOAT_EQ(m, 2.236068F) << "v.magnitude 1 2";
 
-  v2 = Vector2(4, 5);
+  m = Vector2::Magnitude(v);
-  r = v1 == v2;
+  EXPECT_FLOAT_EQ(m, 2.236068F) << "Vector2::Magnitude 1 2";
-  EXPECT_TRUE(r) << "4 5 == 1 2";
+
+  v = Vector2(-1, -2);
+  m = v.magnitude();
+  EXPECT_FLOAT_EQ(m, 2.236068F) << "v.magnitude -1 -2";
+
+  v = Vector2(0, 0);
+  m = v.magnitude();
+  EXPECT_FLOAT_EQ(m, 0) << "v.magnitude 0 0 ";
 
   if (std::numeric_limits<float>::is_iec559) {
-    v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
+    v = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
-    r = v1 == v2;
+    m = v.magnitude();
-    EXPECT_FALSE(r) << "4 5 == INFINITY INFINITY";
+    EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.magnitude INFINITY INFINITY ";
 
-    v1 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
+    v = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
-    r = v1 == v2;
+    m = v.magnitude();
-    EXPECT_FALSE(r) << "-INFINITY -INFINITY == INFINITY INFINITY";
+    EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.magnitude -INFINITY -INFINITY ";
   }
 }
 
@@ -86,35 +92,6 @@ TEST(Vector2, SqrMagnitude) {
   }
 }
 
-TEST(Vector2, Magnitude) {
-  Vector2 v = Vector2(1, 2);
-  float m = 0;
-
-  m = v.magnitude();
-  EXPECT_FLOAT_EQ(m, 2.236068F) << "v.magnitude 1 2";
-
-  m = Vector2::Magnitude(v);
-  EXPECT_FLOAT_EQ(m, 2.236068F) << "Vector2::Magnitude 1 2";
-
-  v = Vector2(-1, -2);
-  m = v.magnitude();
-  EXPECT_FLOAT_EQ(m, 2.236068F) << "v.magnitude -1 -2";
-
-  v = Vector2(0, 0);
-  m = v.magnitude();
-  EXPECT_FLOAT_EQ(m, 0) << "v.magnitude 0 0 ";
-
-  if (std::numeric_limits<float>::is_iec559) {
-    v = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
-    m = v.magnitude();
-    EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.magnitude INFINITY INFINITY ";
-
-    v = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
-    m = v.magnitude();
-    EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.magnitude -INFINITY -INFINITY ";
-  }
-}
-
 TEST(Vector2, Normalize) {
   bool r = false;
 
@@ -334,33 +311,6 @@ TEST(Vector2, Divide) {
   }
 }
 
-TEST(Vector2, Distance) {
-  Vector2 v1 = Vector2(4, 5);
-  Vector2 v2 = Vector2(1, 2);
-  float f = 0;
-
-  f = Vector2::Distance(v1, v2);
-  EXPECT_FLOAT_EQ(f, 4.24264F) << "Distance(4 5, 1 2)";
-
-  v2 = Vector2(-1, -2);
-  f = Vector2::Distance(v1, v2);
-  EXPECT_FLOAT_EQ(f, 8.602325F) << "Distance(4 5, -1 -2)";
-
-  v2 = Vector2(0, 0);
-  f = Vector2::Distance(v1, v2);
-  EXPECT_FLOAT_EQ(f, 6.403124F) << "Distance(4 5, 0 0)";
-
-  if (std::numeric_limits<float>::is_iec559) {
-    v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
-    f = Vector2::Distance(v1, v2);
-    EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Distance(4 5, INFINITY INFINITY)";
-
-    v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
-    f = Vector2::Distance(v1, v2);
-    EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Distance(4 5, -INFINITY -INFINITY)";
-  }
-}
-
 TEST(Vector2, Dot) {
   Vector2 v1 = Vector2(4, 5);
   Vector2 v2 = Vector2(1, 2);
@@ -388,6 +338,56 @@ TEST(Vector2, Dot) {
   }
 }
 
+TEST(Vector2, Equality) {
+  Vector2 v1 = Vector2(4, 5);
+  Vector2 v2 = Vector2(1, 2);
+  bool r = false;
+
+  r = v1 == v2;
+  EXPECT_FALSE(r) << "4 5 == 1 2";
+
+  v2 = Vector2(4, 5);
+  r = v1 == v2;
+  EXPECT_TRUE(r) << "4 5 == 1 2";
+
+  if (std::numeric_limits<float>::is_iec559) {
+    v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
+    r = v1 == v2;
+    EXPECT_FALSE(r) << "4 5 == INFINITY INFINITY";
+
+    v1 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
+    r = v1 == v2;
+    EXPECT_FALSE(r) << "-INFINITY -INFINITY == INFINITY INFINITY";
+  }
+}
+
+TEST(Vector2, Distance) {
+  Vector2 v1 = Vector2(4, 5);
+  Vector2 v2 = Vector2(1, 2);
+  float f = 0;
+
+  f = Vector2::Distance(v1, v2);
+  EXPECT_FLOAT_EQ(f, 4.24264F) << "Distance(4 5, 1 2)";
+
+  v2 = Vector2(-1, -2);
+  f = Vector2::Distance(v1, v2);
+  EXPECT_FLOAT_EQ(f, 8.602325F) << "Distance(4 5, -1 -2)";
+
+  v2 = Vector2(0, 0);
+  f = Vector2::Distance(v1, v2);
+  EXPECT_FLOAT_EQ(f, 6.403124F) << "Distance(4 5, 0 0)";
+
+  if (std::numeric_limits<float>::is_iec559) {
+    v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
+    f = Vector2::Distance(v1, v2);
+    EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Distance(4 5, INFINITY INFINITY)";
+
+    v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
+    f = Vector2::Distance(v1, v2);
+    EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Distance(4 5, -INFINITY -INFINITY)";
+  }
+}
+
 TEST(Vector2, Angle) {
   Vector2 v1 = Vector2(4, 5);
   Vector2 v2 = Vector2(1, 2);