RoboidControl-cpp/LinearAlgebra/Matrix.cpp

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

#pragma region 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;
}

// 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;
}

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 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& 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;
}

void LinearAlgebra::Matrix2::SetSlice(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)];
    // this->data[i, j] = m.data[i - rowStart, j - colStart];
  }
}

// Matrix2
#pragma endregion

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;
}