Added matrix functions

CMakeLists.txt

@@ -30,6 +30,7 @@ else()
         "Quaternion.cpp"
         "Polar.cpp" 
         "Spherical.cpp"
+        "Matrix.cpp"
     )
 
     enable_testing()
@@ -41,6 +42,7 @@ else()
         "test/Vector2_test.cc"
         "test/Vector3_test.cc"
         "test/Quaternion_test.cc"
+        "test/Matrix_test.cc"
     )
     target_link_libraries(
         VectorAlgebraTest

Matrix.cpp

@@ -0,0 +1,55 @@
+#include "Matrix.h"
+
+template <> void MatrixOf<float>::Transpose(MatrixOf<float> *r) {
+  // 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
+  int matrixSize = this->rows * this->cols;
+  if (matrixSize != r->rows * r->cols)
+    // Exception??? For now we don't do anything
+    return;
+
+  for (int dataIx = 0; dataIx < matrixSize; dataIx++) {
+    int rowIx = dataIx / rows;
+    int colIx = dataIx & rows;
+    r->data[dataIx] = this->data[cols * colIx + rowIx];
+  }
+  r->rows = cols;
+  r->cols = rows;
+}
+
+template <>
+void MatrixOf<float>::Multiply(MatrixOf<float> *m1, MatrixOf<float> *m2,
+                               MatrixOf<float> *r) {
+  for (int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) {
+    for (int colIx2 = 0; colIx2 < m2->cols; colIx2++) {
+      int dataIx = colIx2 * m2->cols + rowIx1;
+      r->data[dataIx] = 0.0F;
+      for (int rowIx2 = 0; rowIx2 < m2->rows; rowIx2++) {
+        int dataIx1 = rowIx2 * m1->cols + rowIx1;
+        int dataIx2 = colIx2 * m2->cols + rowIx2;
+        r->data[dataIx] += m1->data[dataIx1] * m2->data[dataIx2];
+      }
+    }
+  }
+}
+
+template <>
+void MatrixOf<float>::Multiply(MatrixOf<float> *m2, MatrixOf<float> *r) {
+  Multiply(this, m2, r);
+}
+
+template <> Vector3 MatrixOf<float>::Multiply(MatrixOf<float> *m, Vector3 v) {
+  float *vData = new float[3]{v.x, v.y, v.z};
+  MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
+  float *rData = new float[3]{};
+  MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
+
+  Multiply(m, &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;
+}

Matrix.h

@@ -0,0 +1,27 @@
+#ifndef MATRIX_H
+#define MATRIX_H
+
+#include "Vector3.h"
+
+/// @brief Single precision float matrix
+template <typename T> class MatrixOf {
+public:
+  MatrixOf(int rows, int cols, T *data) : rows(rows), cols(cols), data(data) {}
+  MatrixOf(Vector3 v); // creates a 3,1 matrix
+
+  /// @brief Transpose with result in matrix m
+  /// @param r The matrix in which the transposed matrix is stored
+  void Transpose(MatrixOf<T> *r);
+
+  static void Multiply(MatrixOf<T> *m1, MatrixOf<T> *m2, MatrixOf<T> *r);
+  void Multiply(MatrixOf<T> *m, MatrixOf<T> *r);
+
+  static Vector3 Multiply(MatrixOf<T> *m, Vector3 v);
+
+private:
+  int rows;
+  int cols;
+  T *data;
+};
+
+#endif

test/Matrix_test.cc

@@ -0,0 +1,47 @@
+#if GTEST
+#include <gtest/gtest.h>
+#include <limits>
+#include <math.h>
+
+#include "Matrix.h"
+
+TEST(MatrixSingle, Init) {
+  // zero
+  float data0[] = {};
+  MatrixOf<float> m0 = MatrixOf<float>(0, 0, data0);
+
+  // one
+  float data1[] = {1.0F};
+  MatrixOf<float> m1 = MatrixOf<float>(1, 1, data1);
+
+  // two
+  float data2[] = {1.0F, 2.0F, 3.0F, 4.0F};
+  MatrixOf<float> m2 = MatrixOf<float>(2, 2, data2);
+
+  // negative
+  MatrixOf<float> m_1 = MatrixOf<float>(-1, -1, data0);
+}
+
+TEST(MatrixSingle, Transpose) {
+  float data1[] = {1.0F};
+  MatrixOf<float> m = MatrixOf<float>(1, 1, data1);
+
+  float data2[] = {1.0F};
+  MatrixOf<float> r = MatrixOf<float>(1, 1, data2);
+  m.Transpose(&r);
+
+  float data2[] = {1.0F, 2.0F, 3.0F, 4.0F};
+  m = MatrixOf<float>(2, 2, data1);
+  m.Transpose(&r);
+  EXPECT_FLOAT_EQ(r.data[0], 1.0F);
+  EXPECT_FLOAT_EQ(r.data[1], 3.0F);
+  EXPECT_FLOAT_EQ(r.data[2], 2.0F);
+  EXPECT_FLOAT_EQ(r.data[3], 4.0F);
+}
+
+TEST(MatrixSingle, Multiply) {
+  float data[] = {1.0F};
+  MatrixOf<float> m = MatrixOf<float>(1, 1, data);
+}
+
+#endif