RoboidControl-cpp/LinearAlgebra/Matrix.cpp

#include "Matrix.h"
#include <iostream>

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)
    data = nullptr;
  else {
    this->data = new float[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() {
  if (data != nullptr && !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(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 (uint rowIx = 0; rowIx < this->nRows; rowIx++) {
    for (uint 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 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;
}