RoboidControl-cpp/LinearAlgebra/Matrix.h

#ifndef MATRIX_H
#define MATRIX_H

#include "Vector3.h"

namespace LinearAlgebra {

class Matrix2 {
 public:
  int nRows = 0;
  int nCols = 0;
  int nValues = 0;
  float* data = nullptr;
  bool externalData = true;

  Matrix2(int nRows, int nCols);
  Matrix2(float* data, int nRows, int nCols);

 ~Matrix2();

  static Matrix2 Zero(int nRows, int nCols);

  static Matrix2 Identity(int size);

  static Matrix2 Diagonal(float f, int size);

  static Matrix2 SkewMatrix(const Vector3& v);

  Matrix2 operator-() const;

  Matrix2 operator*(const Matrix2& m) const;

  void SetSlice(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;
};

/// @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