RoboidControl-cpp/LinearAlgebra/Matrix.cpp

#include "Matrix.h"

#pragma region Matrix2

Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
  this->nValues = nRows * nCols;
  data = new float[nValues]();
}

Matrix2::Matrix2(float* data, int nRows, int nCols)
    : nRows(nRows), nCols(nCols), data(data) {
  this->nValues = nRows * nCols;
}

Matrix2::~Matrix2() {
  delete[] data;
}

Matrix2 Matrix2::Zero(int nRows, int nCols) {
  Matrix2 m = Matrix2(nRows, nCols);
  for (int ix = 0; ix < m.nValues; ix++)
    m.data[ix] = 0;
  return m;
}

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 < r.nValues; 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;
      int BIndex = j;
      for (int k = 0; k < ACols; ++k) {
        sum += this->data[ARowOffset + k] * B.data[BIndex];
        BIndex += BCols;
      }
      r.data[BColOffset + j] = sum;
    }
  }
  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;
}