diff --git a/Matrix.cpp b/Matrix.cpp
index d32e95b..0ade0d9 100644
--- a/Matrix.cpp
+++ b/Matrix.cpp
@@ -1,49 +1,54 @@
 #include "Matrix.h"
 
-template <> void MatrixOf<float>::Transpose(MatrixOf<float> *r) {
+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;
+  int matrixSize = this->cols * this->rows;
   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->cols = this->rows;
+  r->rows = this->cols;
+
+  for (int rDataIx = 0; rDataIx < matrixSize; rDataIx++) {
+    int rowIx = rDataIx / this->rows;
+    int colIx = rDataIx % this->rows;
+    int mDataIx = this->cols * colIx + rowIx;
+    r->data[rDataIx] = this->data[mDataIx];
   }
-  r->rows = cols;
-  r->cols = rows;
 }
 
 template <>
-void MatrixOf<float>::Multiply(MatrixOf<float> *m1, MatrixOf<float> *m2,
-                               MatrixOf<float> *r) {
+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];
+      int rDataIx = colIx2 * m2->cols + rowIx1;
+      r->data[rDataIx] = 0.0F;
+      for (int kIx = 0; kIx < m2->rows; kIx++) {
+        int dataIx1 = rowIx1 * m1->cols + kIx;
+        int dataIx2 = kIx * m2->cols + colIx2;
+        r->data[rDataIx] += m1->data[dataIx1] * m2->data[dataIx2];
       }
     }
   }
 }
 
 template <>
-void MatrixOf<float>::Multiply(MatrixOf<float> *m2, MatrixOf<float> *r) {
+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};
+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]{};
+  float* rData = new float[3]{};
   MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
 
   Multiply(m, &v_m, &r_m);
diff --git a/test/Matrix_test.cc b/test/Matrix_test.cc
index 208f1d7..48620df 100644
--- a/test/Matrix_test.cc
+++ b/test/Matrix_test.cc
@@ -1,7 +1,7 @@
 #if GTEST
 #include <gtest/gtest.h>
-#include <limits>
 #include <math.h>
+#include <limits>
 
 #include "Matrix.h"
 
@@ -30,18 +30,93 @@ TEST(MatrixSingle, Transpose) {
   MatrixOf<float> r = MatrixOf<float>(1, 1, data2);
   m.Transpose(&r);
 
+  // 2 x 2
+
   float data3[] = {1.0F, 2.0F, 3.0F, 4.0F};
-  m = MatrixOf<float>(2, 2, data3);
-  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);
+  MatrixOf<float> m22 = MatrixOf<float>(2, 2, data3);
+  EXPECT_EQ(m22.rows, 2);
+  EXPECT_EQ(m22.cols, 2);
+
+  float data4[] = {0.0F, 0.0F, 0.0F, 0.0F};
+  MatrixOf<float> r22 = MatrixOf<float>(2, 2, data4);
+  EXPECT_EQ(r22.rows, 2);
+  EXPECT_EQ(r22.cols, 2);
+
+  m22.Transpose(&r22);
+  EXPECT_EQ(r22.rows, 2);
+  EXPECT_EQ(r22.cols, 2);
+  EXPECT_FLOAT_EQ(r22.data[0], 1.0F);
+  EXPECT_FLOAT_EQ(r22.data[1], 3.0F);
+  EXPECT_FLOAT_EQ(r22.data[2], 2.0F);
+  EXPECT_FLOAT_EQ(r22.data[3], 4.0F);
+
+  // 1 x 2
+  float data12[] = {1.0F, 2.0F};
+  MatrixOf<float> m12 = MatrixOf<float>(1, 2, data12);
+  EXPECT_EQ(m12.rows, 1);
+  EXPECT_EQ(m12.cols, 2);
+
+  float data21[] = {0.0F, 0.0F};
+  MatrixOf<float> r21 = MatrixOf<float>(2, 1, data21);
+  EXPECT_EQ(r21.rows, 2);
+  EXPECT_EQ(r21.cols, 1);
+
+  m12.Transpose(&r21);
+  EXPECT_EQ(r21.rows, 2);
+  EXPECT_EQ(r21.cols, 1);
+  EXPECT_FLOAT_EQ(r21.data[0], 1.0F);
+  EXPECT_FLOAT_EQ(r21.data[1], 2.0F);
+
+  // changing dimensions, same size is okay
+  MatrixOf<float> r12 = MatrixOf<float>(1, 2, data21);
+  EXPECT_EQ(r12.rows, 1);
+  EXPECT_EQ(r12.cols, 2);
+
+  m12.Transpose(&r12);
+  EXPECT_EQ(r12.rows, 2);
+  EXPECT_EQ(r12.cols, 1);
+  EXPECT_FLOAT_EQ(r12.data[0], 1.0F);
+  EXPECT_FLOAT_EQ(r12.data[1], 2.0F);
 }
 
 TEST(MatrixSingle, Multiply) {
-  float data[] = {1.0F};
-  MatrixOf<float> m = MatrixOf<float>(1, 1, data);
+  float m12data[] = {1.0F, 2.0F};
+  MatrixOf<float> m12 = MatrixOf<float>(1, 2, m12data);
+
+  EXPECT_EQ(m12.rows, 1);
+  EXPECT_EQ(m12.cols, 2);
+  EXPECT_FLOAT_EQ(m12.data[0], 1.0F);
+  EXPECT_FLOAT_EQ(m12.data[1], 2.0F);
+
+  float m21data[] = {3.0F, 4.0F};
+  MatrixOf<float> m21 = MatrixOf<float>(2, 1, m21data);
+
+  EXPECT_EQ(m21.rows, 2);
+  EXPECT_EQ(m21.cols, 1);
+  EXPECT_FLOAT_EQ(m21.data[0], 3.0F);
+  EXPECT_FLOAT_EQ(m21.data[1], 4.0F);
+
+  float r11data[] = {0.0F};
+  MatrixOf<float> r11 = MatrixOf<float>(1, 1, r11data);
+
+  EXPECT_EQ(r11.rows, 1);
+  EXPECT_EQ(r11.cols, 1);
+
+  MatrixOf<float>::Multiply(&m12, &m21, &r11);
+  EXPECT_EQ(r11.rows, 1);
+  EXPECT_EQ(r11.cols, 1);
+  EXPECT_FLOAT_EQ(r11.data[0], 11.0F);
+
+  float r22data[] = {0.0F, 0.0F, 0.0F, 0.0F};
+  MatrixOf<float> r22 = MatrixOf<float>(2, 2, r22data);
+
+  MatrixOf<float>::Multiply(&m21, &m12, &r22);
+  EXPECT_EQ(r22.rows, 2);
+  EXPECT_EQ(r22.cols, 2);
+  EXPECT_FLOAT_EQ(r22.data[0], 3.0F);
+  EXPECT_FLOAT_EQ(r22.data[1], 4.0F);
+  EXPECT_FLOAT_EQ(r22.data[2], 6.0F);
+  EXPECT_FLOAT_EQ(r22.data[3], 8.0F);
 }
 
 #endif
\ No newline at end of file