RoboidControl-cpp/LinearAlgebra/Matrix.h

#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