Fix tests

Angle.h

@@ -21,7 +21,7 @@ template <typename T>
 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;
@@ -209,7 +209,7 @@ class AngleOf {
  private:
   T value;
 
-  AngleOf<T>(T rawValue);
+  AngleOf(T rawValue);
 };
 
 using AngleSingle = AngleOf<float>;

Direction.cpp

@@ -9,6 +9,7 @@
 
 #include <math.h>
 
+namespace LinearAlgebra {
 template <typename T>
 DirectionOf<T>::DirectionOf() {
   this->horizontal = AngleOf<T>();
@@ -98,5 +99,6 @@ void DirectionOf<T>::Normalize() {
   }
 }
 
-template class DirectionOf<float>;
+template class LinearAlgebra::DirectionOf<float>;
-template class DirectionOf<signed short>;
+template class LinearAlgebra::DirectionOf<signed short>;
+}

Direction.h

@@ -30,11 +30,11 @@ class DirectionOf {
   AngleOf<T> vertical;
 
   /// @brief Create a new direction with zero angles
-  DirectionOf<T>();
+  DirectionOf();
   /// @brief Create a new direction
   /// @param horizontal The horizontal angle
   /// @param vertical The vertical angle.
-  DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
+  DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical);
 
   /// @brief Convert the direction into a carthesian vector
   /// @return The carthesian vector corresponding to this direction.
@@ -99,6 +99,4 @@ using Direction = DirectionSingle;
 
 }  // namespace LinearAlgebra
 
-using namespace LinearAlgebra;
-
 #endif

Matrix.cpp

@@ -1,26 +1,137 @@
 #include "Matrix.h"
+#if !defined(NO_STD) 
+#include <iostream>
+#endif
+
+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;
-  data = new float[nValues]();
+  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 m = Matrix2(nRows, nCols);
+  Matrix2 r = Matrix2(nRows, nCols);
-  for (int ix = 0; ix < m.nValues; ix++)
+  for (int ix = 0; ix < r.nValues; ix++)
-    m.data[ix] = 0;
+    r.data[ix] = 0;
-  return m;
+  return r;
+}
+
+void Matrix2::Clear() {
+  for (int ix = 0; ix < this->nValues; ix++)
+    this->data[ix] = 0;
 }
 
 Matrix2 Matrix2::Identity(int size) {
@@ -31,7 +142,7 @@ Matrix2 Matrix2::Diagonal(float f, int size) {
   Matrix2 r = Matrix2(size, size);
   float* data = r.data;
   int valueIx = 0;
-  for (int ix = 0; ix < r.nValues; ix++) {
+  for (int ix = 0; ix < size; ix++) {
     data[valueIx] = f;
     valueIx += size + 1;
   }
@@ -50,6 +161,17 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
   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++)
@@ -57,13 +179,26 @@ Matrix2 LinearAlgebra::Matrix2::operator-() const {
   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;
+  // int BRows = B.nRows;
 
   for (int i = 0; i < ARows; ++i) {
     // Pre-compute row offsets
@@ -71,33 +206,83 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
     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;
 }
 
-void LinearAlgebra::Matrix2::SetSlice(int rowStart,
+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 i = rowStart; i < rowStop; i++) {
-    for (int j = colStart; j < colStop; j++)
+  //   for (int j = colStart; j < colStop; j++)
-      this->data[i * this->nCols + j] =
+  //     this->data[i * this->nCols + j] =
-          m.data[(i - rowStart) * m.nCols + (j - colStart)];
+  //         m.data[(i - rowStart) * m.nCols + (j - colStart)];
-    // this->data[i, j] = m.data[i - rowStart, 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) {

Matrix.h

@@ -1,10 +1,28 @@
 #ifndef MATRIX_H
 #define MATRIX_H
 
+#include "Quaternion.h"
 #include "Vector3.h"
 
 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;
@@ -12,12 +30,18 @@ class Matrix2 {
   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);
 
@@ -25,11 +49,69 @@ class Matrix2 {
 
   static Matrix2 SkewMatrix(const Vector3& v);
 
+  Matrix2 Transpose() const;
+
   Matrix2 operator-() const;
 
-  Matrix2 operator*(const Matrix2& m) 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);
 
-  void SetSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const;
+  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
@@ -143,6 +225,6 @@ class MatrixOf {
 };
 
 }  // namespace LinearAlgebra
-using namespace LinearAlgebra;
+// using namespace LinearAlgebra;
 
 #endif

Polar.cpp

@@ -175,5 +175,5 @@ PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
   return r;
 }
 
-template class PolarOf<float>;
+template class LinearAlgebra::PolarOf<float>;
-template class PolarOf<signed short>;
+template class LinearAlgebra::PolarOf<signed short>;

Spherical.cpp

@@ -5,6 +5,8 @@
 
 #include <math.h>
 
+namespace LinearAlgebra {
+
 template <typename T>
 SphericalOf<T>::SphericalOf() {
   this->distance = 0.0f;
@@ -301,3 +303,5 @@ SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
 
 template class SphericalOf<float>;
 template class SphericalOf<signed short>;
+
+}  // namespace LinearAlgebra

Spherical.h

@@ -24,9 +24,9 @@ class SphericalOf {
   /// @brief The direction of the vector
   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
@@ -186,7 +186,6 @@ using Spherical = SphericalSingle;
 #endif
 
 }  // namespace LinearAlgebra
-using 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>());
@@ -166,3 +168,5 @@ void SwingTwistOf<T>::Normalize() {
 
 template class SwingTwistOf<float>;
 template class SwingTwistOf<signed short>;
+
+}

SwingTwist.h

@@ -21,9 +21,9 @@ class SwingTwistOf {
   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,

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,44 @@
 #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};
@@ -15,12 +49,11 @@ TEST(Matrix2, Multiplication) {
   Matrix2 result = A * B;
 
   float expectedData[] = {19, 22, 43, 50};
-      for (int i = 0; i < 4; ++i) {
+  for (int i = 0; i < 4; ++i) 
-          //assert(result.data[i] == expectedData[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};
@@ -29,32 +62,27 @@ TEST(Matrix2, Multiplication) {
 
   Matrix2 result2 = C * D;
 
-      float expectedData2[] = {29, 32, 35, 65, 72, 79, 101, 112, 123};
+  float expectedData2[] = {27, 30, 33, 61, 68, 75, 95, 106, 117};
-      for (int i = 0; i < 9; ++i) {
+  for (int i = 0; i < 9; ++i)
-          assert(result2.data[i] == expectedData2[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) {
+  for (int i = 0; i < 4; ++i)
-          assert(result3.data[i] == 0);
     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) {
+  for (int i = 0; i < 4; ++i)
-          assert(result4.data[i] == A.data[i]);
     EXPECT_TRUE(result4.data[i] == A.data[i]);
-      }
   std::cout << "Test 4 passed: Multiplication with identity matrix.\n";
+  
 }
 
 TEST(MatrixSingle, Init) {

test/Polar_test.cc

@@ -2,6 +2,7 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
+#include <chrono>
 
 #include "Polar.h"
 #include "Spherical.h"

test/Spherical16_test.cc

@@ -2,6 +2,7 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
+#include <chrono>
 
 #include "Spherical.h"
 #include "Vector3.h"

test/SphericalSingle_test.cc

@@ -2,6 +2,7 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
+#include <chrono>
 
 #include "Spherical.h"