Added initial MatrixOf

Matrix.h

@@ -4,52 +4,213 @@
 #include "Quaternion.h"
 #include "Vector3.h"
 
+#include <cstring>  // for memcpy
+
 namespace LinearAlgebra {
 
+template <typename T>
+class Matrix2Of;  // forward declaration
+
 /// @brief A 1-dimensional matrix or vector of arbitrary size
-class Matrix1 {
+template <typename T>
+class Matrix1Of {
  public:
-  float* data = nullptr;
+  /// @brief Create a zero-sized matrix
-  int size = 0;
+  Matrix1Of();
+  /// @brief Create a 1 dimensional matrix
+  /// @param size the length of the matrix
+  Matrix1Of(int size);
+  /// @brief Create a 1 dimensional matrix with the given data
+  /// @param size the length of the matrix
+  /// @param data the data used to fill the matrix
+  Matrix1Of(int size, T* data);
+  /// @brief Make a copy of a matrix (copy constructor)
+  /// @param m the matrix to copy
+  Matrix1Of(const Matrix1Of& m);
+  /// @brief Move the data of matrix m to a new matrix (move constructor)
+  /// @param m the matrix to move
+  /// The data of the matrix m will be re-used for the new matrix.
+  /// @note This operator will empty matrix m
+  Matrix1Of(Matrix1Of&& m) noexcept;
 
-  Matrix1(int size);
+  /// @brief matrix destructor
-  Matrix1(float* data, int size);
+  ~Matrix1Of();
 
-  static Matrix1 FromQuaternion(Quaternion q);
+  /// @brief Copy the contents of a matrix
+  /// @param m the matrix to copy
+  /// @return The matrix with a copy of the contents of m
+  Matrix1Of& operator=(const Matrix1Of& m);
+  /// @brief Move the contents of a matrix
+  /// @param m the matrix for which the contents should be moved
+  /// @return A matrix with the contents of matrix m
+  /// @note This operator will empty matrix m
+  Matrix1Of& operator=(Matrix1Of&& m) noexcept;
+
+  /// @brief Get the size of the matrix
+  /// @return the size of the matrix
+  unsigned int Size() const;
+  /// @brief Reize the matrix
+  /// @param size The resized matrix
+  /// @note This will destruct the data contents of the matrix
+  void Resize(int size);
+
+  /// @brief Create a matrix initialized to all zeros
+  /// @param size The length of the matrix
+  /// @return the initialized matrix
+  static Matrix1Of Zero(unsigned int size);
+  /// @brief Overwrite all elements with a new value
+  /// @param value the value to set for all elements
+  void Fill(T value);
+  /// @brief Overwrite a range of elements with a new value
+  /// @param value the value to set
+  /// @param start the first element to overwrite
+  /// @param stop the first element to not overwrite
+  /// @remark This will overwrite the elements [start..stop)
+  /// So the element at index 'stop' will not be overwritten
+  void Fill(T value, unsigned int start, unsigned int stop);
+
+  /// @brief Convert from a 3d vector
+  /// @param v the vector to convert
+  /// @return a matrix with the 3 elements of the vector
+  static Matrix1Of FromVector3(Vector3Of<T> v);
+  /// @brief Convert to a 3d vector
+  /// @return a vector with the first 3 elements of the matrix
+  Vector3Of<T> ToVector3();
+
+  /// @brief Convert from a quaternion
+  /// @param q the quaternion to convert
+  /// @return a matrix with the 4 elements of the quaternion
+  static Matrix1Of<float> FromQuaternion(Quaternion q);
+  /// @brief Convert to a quaternion
+  /// @return a quaternion with the first 4 elements of the quaternion
   Quaternion ToQuaternion();
 
+  /// @brief Get an element from the matrix
+  /// @param ix the index of the element to retrieve
+  /// @return the element at index ix
+  const T& operator()(int ix) const;
+  /// @brief Get the reference to an element of a matrix
+  /// @param ix the index of the element
+  /// @return a reference to the element of index ix
+  T& operator()(int ix);
+
+  /// @brief Get a subsection of a matrix
+  /// @param start the first element to retrieve
+  /// @param stop the first element to not retrieve
+  /// @return a matrix with the requested elements
+  /// @remark This will retrieve the elements [start..stop)
+  /// So the element at index 'stop' will not be included
+  Matrix1Of Slice(unsigned int start, unsigned int stop);
+  /// @brief Overwrite a subsection of a matrix with the values from another matrix
+  /// @param start the first element to overwrite
+  /// @param stop the first element to not overwrite
+  /// @param m a matrix with the value to write to the given range
+  /// @remark This will overwrite the elements [start..stop)
+  /// So the element at index 'stop' will not be overwritten
+  void UpdateSlice(unsigned int start, unsigned int stop, const Matrix1Of& m) const;
+
+ protected:
+  /// @brief The data values
+  T* data = nullptr;
+  /// @brief The length of the matrix
+  unsigned int size = 0;
+
  private:
+  /// @brief if this is true, functions will not do memory management
+  /// The allocation and deallocation of this->data should be handled outside this library
   bool externalData = true;
+
+  friend class Matrix2Of<T>;
+};
+
+using Matrix1Int = Matrix1Of<int>;
+using Matrix1Float = Matrix1Of<float>;
+using Matrix1Double = Matrix1Of<double>;
+
+using Matrix1 = Matrix1Float;
+
+/*
+class Matrix2;
+
+class SubMatrix2 {
+ public:
+  const Matrix2& parent;
+  int startRow;
+  int startCol;
+  int rowCount;
+  int colCount;
+
+  SubMatrix2(const Matrix2& m, int startRow, int startCol, int rowCount, int colCount);
+
+  // Assign from another full Matrix (sizes must match)
+  void operator=(const Matrix2& src);
+
+  // Assign from another SubMatrix2 (handles aliasing)
+  void operator=(const SubMatrix2& src);
+
+  // Read element
+  float operator()(int row, int col) const;
+
+  // Write element
+  const float& operator()(int row, int col);
 };
 
 /// @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(int nRows, int nCols, float* data);
-  Matrix2(const Matrix2& m);
+  Matrix2(const Matrix2& m);      // Copy constructor
-  Matrix2& operator=(const Matrix2& other);
+  Matrix2(Matrix2&& m) noexcept;  // Move constructor
 
   ~Matrix2();
 
-  Matrix2 Clone() const;
+  Matrix2& operator=(const Matrix2& m);          // Copy assignment operator
+  Matrix2& operator=(Matrix2&& other) noexcept;  // Move assignment operator
+
+  unsigned int NRows() const;
+  unsigned int NCols() const;
+
+  float* GetData() { return this->data; }
+  float* GetData() const { return this->data; }
 
   static Matrix2 Zero(int nRows, int nCols);
-  void Clear();
+  void Fill(float value);
+  void FillDiagonal(float f);
 
   static Matrix2 Identity(int size);
-
+  static Matrix2 Identity(int nRows, int nCols);
   static Matrix2 Diagonal(float f, int size);
 
-  static Matrix2 SkewMatrix(const Vector3& v);
+  static Matrix2 SkewMatrix(const Vector3Of<float>& v);
 
   Matrix2 Transpose() const;
+  Matrix2 Inverse();
+
+  const float& operator()(int row, int col) const;
+  float& operator()(int row, int col);
+
+  Matrix2 GetRows(int from, int to);
+  Vector3 GetRow3(int rowIx);
+
+  void SetRow(int rowIx, Matrix1 source);
+  void SetRow3(int rowIx, Vector3 v);
+
+  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop) const;
+
+  void UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const;
+  void UpdateSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const;
+
+  inline SubMatrix2 GetBlock(int startRow, int startCol, int rowCount, int colCount) const {
+    return SubMatrix2(*this, startRow, startCol, rowCount, colCount);
+  }
+  inline Matrix2 CopyBlock(int startRow, int startCol, int rowCount, int colCount) const {
+    return Slice(startRow, startRow + rowCount, startCol, startCol + colCount);
+  }
+
+  Matrix2 DeleteRows(int rowStart, int rowStop) const;
+  Matrix2 DeleteColumns(int colStart, int colStop) const;
 
   Matrix2 operator-() const;
 
@@ -57,172 +218,203 @@ class Matrix2 {
   /// @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& v);
+
+  Matrix2 operator-(const Matrix2& v) 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++)
+    for (int ix = 0; ix < r.size; 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++)
+    for (int ix = 0; ix < r.size; ix++)
       r.data[ix] = f * m.data[ix];
     return r;
   }
 
-  friend Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
+  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++)
+    for (int ix = 0; ix < r.size; 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++)
+    for (int ix = 0; ix < r.size; ix++)
       r.data[ix] = f / m.data[ix];
     return r;
   }
 
-  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
+  template <typename T>
+  static Matrix2 Omega(const Vector3Of<T>& v);
 
-  void UpdateSlice(int rowStart,
+ protected:
-                int rowStop,
+  int nRows = 0;
-                int colStart,
+  int nCols = 0;
-                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);
+ protected:
+  int size = 0;
+  float* data = nullptr;
+
+ private:
+  bool externalData = true;
+};
+*/
+
+/// @brief A 2-dimensional matrix of arbitrary size
+template <typename T>
+class Matrix2Of {
+ public:
+  Matrix2Of();
+  Matrix2Of(unsigned int rowCount, unsigned int colCount);
+  Matrix2Of(unsigned int rowCount, unsigned int colCount, T* data);
+  Matrix2Of(const Matrix2Of& m);      // Copy Constructor
+  Matrix2Of(Matrix2Of&& m) noexcept;  // Move Contructor
+
+  ~Matrix2Of();
+
+  Matrix2Of& operator=(const Matrix2Of& m);      // Copy assignment operator
+  Matrix2Of& operator=(Matrix2Of&& m) noexcept;  // Move assignment operator
+
+  unsigned int RowCount() const;
+  unsigned int ColCount() const;
+
+  const T& operator()(unsigned int rowIx, unsigned int colIx) const;
+  T& operator()(unsigned int rowIx, unsigned int colIx);
+
+  T* GetData();
+  T* GetData() const;
+
+  static Matrix2Of Zero(unsigned int nRows, unsigned int nCols);
+  void Fill(T value);
+  void FillDiagonal(T f);
+
+  static Matrix2Of Identity(unsigned int size);
+  static Matrix2Of Identity(unsigned int nRows, unsigned int nCols);
+  static Matrix2Of Diagonal(T f, unsigned int size);
+
+  static Matrix2Of SkewMatrix(const Vector3Of<T>& v);
+
+  Matrix1Of<T> GetRow(unsigned int rowIx);
+  Vector3Of<T> GetRow3(unsigned int rowIx);
+  Matrix2Of<T> GetRows(unsigned int fromRowIx, unsigned int toRowIx);
+
+  void SetRow(unsigned int rowIx, Matrix1Of<T> source);
+  void SetRow3(unsigned int rowIx, Vector3Of<T> source);
+
+  Matrix2Of DeleteRows(unsigned int fromRowIx, unsigned int toRowIx) const;
+  Matrix2Of DeleteColumns(unsigned int fromColIx, unsigned int colStop) const;
+
+  Matrix2Of operator-() const;
+
+  /// @brief Add a matrix to this matrix
+  /// @param m The matrix to add to this matrix
+  /// @return The result of the addition
+  Matrix2Of operator+(const Matrix2Of& v) const;
+  Matrix2Of& operator+=(const Matrix2Of& v);
+
+  Matrix2Of operator-(const Matrix2Of& v) const;
+  Matrix2Of& operator-=(const Matrix2Of& v);
+
+  Matrix2Of operator*(const Matrix2Of& m) const;
+  template <typename U>
+  friend Matrix2Of<U> operator*(const Matrix2Of<U>& m, U f);
+  template <typename U>
+  friend Matrix2Of<U> operator*(U f, const Matrix2Of<U>& m);
+  template <typename U>
+  friend Matrix1Of<U> operator*(const Matrix2Of<U>& m, const Matrix1Of<U>& v);
+
+  template <typename U>
+  friend Matrix2Of<U> operator/(const Matrix2Of<U>& m, U f);
+  template <typename U>
+  friend Matrix2Of<U> operator/(U f, const Matrix2Of<U>& m);
+
+  Matrix2Of Transposed() const;
+  Matrix2Of Inverse();
+
+  Matrix2Of Slice(int rawStart, int rowStop, int colStart, int colStop) const;
+
+  void UpdateSlice(int rowStart, int rowStop, const Matrix2Of& m) const;
+  void UpdateSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2Of& m) const;
+
+  /// @brief Compute the Omega matrix of a 3D vector
+  /// @param v The vector
+  /// @return 4x4 Omega matrix
+  // static Matrix2Of Omega(const Vector3Of<T>& v);
+
+ protected:
+  unsigned int nRows = 0;
+  unsigned int nCols = 0;
+
+ protected:
+  unsigned int size = 0;
+  T* data = nullptr;
 
  private:
   bool externalData = true;
 };
 
-/// @brief Single precision float matrix
+#pragma region friend functions
-template <typename T>
+
-class MatrixOf {
+// For a class template a friend function must also be a template and defined in the header (unless
- public:
+// you restrict to a concrete instantiation)...
-  MatrixOf(unsigned int rows, unsigned int cols);
+template <typename U>
-  MatrixOf(unsigned int rows, unsigned int cols, const T* source)
+Matrix2Of<U> operator*(const Matrix2Of<U>& m, U f) {
-      : MatrixOf(rows, cols) {
+  Matrix2Of<U> r(m.nRows, m.nCols);
-    Set(source);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = m.data[ix] * f;
+  return r;
+}
+template <typename U>
+Matrix2Of<U> operator*(U f, const Matrix2Of<U>& m) {
+  Matrix2Of<U> r(m.nRows, m.nCols);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = f * m.data[ix];
+  return r;
+}
+template <typename U>
+Matrix1Of<U> operator*(const Matrix2Of<U>& m, const Matrix1Of<U>& v) {
+  Matrix1Of<U> r(m.nRows);
+  for (unsigned int rowIx = 0; rowIx < m.nRows; rowIx++) {
+    int mRowIx = rowIx * m.nCols;
+    for (unsigned int colIx = 0; colIx < m.nCols; colIx++)
+      r(rowIx) += m.data[mRowIx + colIx] * v(colIx);
   }
-  MatrixOf(Vector3 v);  // creates a 3,1 matrix
+  return r;
+}
 
-  ~MatrixOf() {
+template <typename U>
-    if (this->data == nullptr)
+Matrix2Of<U> operator/(const Matrix2Of<U>& m, U f) {
-      return;
+  Matrix2Of<U> r(m.nRows, m.nCols);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = m.data[ix] / f;
+  return r;
+}
+template <typename U>
+Matrix2Of<U> operator/(U f, const Matrix2Of<U>& m) {
+  Matrix2Of r(m.nRows, m.nCols);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = f / m.data[ix];
+  return r;
+}
 
-    delete[] this->data;
+#pragma endregion friend functions
-  }
 
-  /// @brief Transpose with result in matrix m
+template class Matrix2Of<int>;
-  /// @param r The matrix in which the transposed matrix is stored
+template class Matrix2Of<float>;
-  void Transpose(MatrixOf<T>* r) const {
+template class Matrix2Of<double>;
-    // Check dimensions first
-    // We dont care about the rows and cols (we overwrite them)
-    // but the data size should be equal to avoid problems
-    // We cannot check the data size directly, but the row*col should be equal
-    unsigned int matrixSize = this->cols * this->rows;
-    unsigned int resultSize = r->rows * r->cols;
-    if (matrixSize != resultSize) {
-      // Return a null matrix;
-      // We dont set data to nullptr because it is allocated memory
-      // Instead we write all zeros
-      for (unsigned int dataIx = 0; dataIx < resultSize; dataIx++)
-        r->data[dataIx] = 0.0f;
-      r->rows = 0;
-      r->cols = 0;
-      return;
-    }
 
-    r->cols = this->rows;
+using Matrix2Int = Matrix2Of<int>;
-    r->rows = this->cols;
+using Matrix2Float = Matrix2Of<float>;
+using Matrix2Double = Matrix2Of<double>;
 
-    for (unsigned int rDataIx = 0; rDataIx < matrixSize; rDataIx++) {
+//using Matrix2 = Matrix2Float;
-      unsigned int rowIx = rDataIx / this->rows;
-      unsigned int colIx = rDataIx % this->rows;
-      unsigned int mDataIx = this->cols * colIx + rowIx;
-      r->data[rDataIx] = this->data[mDataIx];
-    }
-  }
-
-  static void Multiply(const MatrixOf<T>* m1,
-                       const MatrixOf<T>* m2,
-                       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);
-  Vector3 operator*(const Vector3 v) const;
-
-  T Get(unsigned int rowIx, unsigned int colIx) const {
-    unsigned int dataIx = rowIx * this->cols + colIx;
-    return this->data[dataIx];
-  }
-
-  void Set(unsigned int rowIx, unsigned int colIx, T value) {
-    unsigned int dataIx = rowIx * this->cols + colIx;
-    this->data[dataIx] = value;
-  }
-
-  // This function does not check on source size!
-  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) {
-    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) {
-    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) {
-    unsigned int thisMatrixSize = this->cols * this->rows;
-    unsigned int mMatrixSize = m->cols * m->rows;
-    if (mMatrixSize != thisMatrixSize)
-      return;
-
-    for (unsigned int dataIx = 0; dataIx < thisMatrixSize; dataIx++)
-      this->data[dataIx] = m->data[dataIx];
-  }
-
-  unsigned int RowCount() const { return rows; }
-  unsigned int ColCount() const { return cols; }
-
- private:
-  unsigned int rows;
-  unsigned int cols;
-  T* data;
-};
 
 }  // namespace LinearAlgebra
 // using namespace LinearAlgebra;

Matrix_old.cpp

@@ -0,0 +1,351 @@
+/*
+#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;
+  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;
+    this->data = nullptr;
+    return;
+  }
+  this->rows = rows;
+  this->cols = cols;
+
+  unsigned int matrixSize = this->cols * this->rows;
+  this->data = new float[matrixSize]{0.0f};
+}
+
+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,
+                               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;
+      r->data[rDataIx] = 0.0F;
+      for (unsigned int kIx = 0; kIx < m2->rows; kIx++) {
+        unsigned int dataIx1 = rowIx1 * m1->cols + kIx;
+        unsigned int dataIx2 = kIx * m2->cols + colIx2;
+        r->data[rDataIx] += m1->data[dataIx1] * m2->data[dataIx2];
+      }
+    }
+  }
+}
+
+template <>
+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);
+
+  Multiply(m, &v_m, &r_m);
+
+  Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
+  return r;
+}
+
+template <typename T>
+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]{};
+  MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
+
+  Multiply(this, &v_m, &r_m);
+
+  Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
+  delete[] vData;
+  delete[] rData;
+  return r;
+}
+*/

Matrix_old.h

@@ -0,0 +1,233 @@
+/*
+
+#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;
+  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 {
+ public:
+  MatrixOf(unsigned int rows, unsigned int cols);
+  MatrixOf(unsigned int rows, unsigned int cols, const T* source)
+      : MatrixOf(rows, cols) {
+    Set(source);
+  }
+  MatrixOf(Vector3 v);  // creates a 3,1 matrix
+
+  ~MatrixOf() {
+    if (this->data == nullptr)
+      return;
+
+    delete[] this->data;
+  }
+
+  /// @brief Transpose with result in matrix m
+  /// @param r The matrix in which the transposed matrix is stored
+  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
+    // We cannot check the data size directly, but the row*col should be equal
+    unsigned int matrixSize = this->cols * this->rows;
+    unsigned int resultSize = r->rows * r->cols;
+    if (matrixSize != resultSize) {
+      // Return a null matrix;
+      // We dont set data to nullptr because it is allocated memory
+      // Instead we write all zeros
+      for (unsigned int dataIx = 0; dataIx < resultSize; dataIx++)
+        r->data[dataIx] = 0.0f;
+      r->rows = 0;
+      r->cols = 0;
+      return;
+    }
+
+    r->cols = this->rows;
+    r->rows = this->cols;
+
+    for (unsigned int rDataIx = 0; rDataIx < matrixSize; rDataIx++) {
+      unsigned int rowIx = rDataIx / this->rows;
+      unsigned int colIx = rDataIx % this->rows;
+      unsigned int mDataIx = this->cols * colIx + rowIx;
+      r->data[rDataIx] = this->data[mDataIx];
+    }
+  }
+
+  static void Multiply(const MatrixOf<T>* m1,
+                       const MatrixOf<T>* m2,
+                       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);
+  Vector3 operator*(const Vector3 v) const;
+
+  T Get(unsigned int rowIx, unsigned int colIx) const {
+    unsigned int dataIx = rowIx * this->cols + colIx;
+    return this->data[dataIx];
+  }
+
+  void Set(unsigned int rowIx, unsigned int colIx, T value) {
+    unsigned int dataIx = rowIx * this->cols + colIx;
+    this->data[dataIx] = value;
+  }
+
+  // This function does not check on source size!
+  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) {
+    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) {
+    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) {
+    unsigned int thisMatrixSize = this->cols * this->rows;
+    unsigned int mMatrixSize = m->cols * m->rows;
+    if (mMatrixSize != thisMatrixSize)
+      return;
+
+    for (unsigned int dataIx = 0; dataIx < thisMatrixSize; dataIx++)
+      this->data[dataIx] = m->data[dataIx];
+  }
+
+  unsigned int RowCount() const { return rows; }
+  unsigned int ColCount() const { return cols; }
+
+ private:
+  unsigned int rows;
+  unsigned int cols;
+  T* data;
+};
+
+}  // namespace LinearAlgebra
+// using namespace LinearAlgebra;
+
+#endif
+*/

Quaternion.cpp

@@ -98,27 +98,27 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) {
   }
 }
 
-Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
+// Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
-  Matrix2 r = Matrix2(3, 3);
+//   Matrix2 r = Matrix2(3, 3);
 
-  float x = this->x;
+//   float x = this->x;
-  float y = this->y;
+//   float y = this->y;
-  float z = this->z;
+//   float z = this->z;
-  float w = this->w;
+//   float w = this->w;
 
-  float* data = r.data;
+//   float* data = r.data;
-  data[0 * 3 + 0] = 1 - 2 * (y * y + z * z);
+//   data[0 * 3 + 0] = 1 - 2 * (y * y + z * z);
-  data[0 * 3 + 1] = 2 * (x * y - w * z);
+//   data[0 * 3 + 1] = 2 * (x * y - w * z);
-  data[0 * 3 + 2] = 2 * (x * z + w * y);
+//   data[0 * 3 + 2] = 2 * (x * z + w * y);
-  data[1 * 3 + 0] = 2 * (x * y + w * z);
+//   data[1 * 3 + 0] = 2 * (x * y + w * z);
-  data[1 * 3 + 1] = 1 - 2 * (x * x + z * z);
+//   data[1 * 3 + 1] = 1 - 2 * (x * x + z * z);
-  data[1 * 3 + 2] = 2 * (y * z - w * x);
+//   data[1 * 3 + 2] = 2 * (y * z - w * x);
-  data[2 * 3 + 0] = 2 * (x * z - w * y);
+//   data[2 * 3 + 0] = 2 * (x * z - w * y);
-  data[2 * 3 + 1] = 2 * (y * z + w * x);
+//   data[2 * 3 + 1] = 2 * (y * z + w * x);
-  data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
+//   data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
 
-  return r;
+//   return r;
-}
+// }
 
 Quaternion Quaternion::operator*(const Quaternion& r2) const {
   return Quaternion(