diff --git a/Matrix.cpp b/Matrix.cpp
index c7e53c7..0f99850 100644
--- a/Matrix.cpp
+++ b/Matrix.cpp
@@ -1,25 +1,345 @@
 #include "Matrix.h"
-#if !defined(NO_STD) 
+#if !defined(NO_STD)
+#include <math.h>
 #include <iostream>
+
 #endif
 
 namespace LinearAlgebra {
 
-#pragma region Matrix1
+#pragma region Matrix1Of
 
-Matrix1::Matrix1(int size) : size(size) {
-  if (this->size == 0)
+template <typename T>
+Matrix1Of<T>::Matrix1Of() {}
+
+template <typename T>
+Matrix1Of<T>::Matrix1Of(int size) : size(size), externalData(false) {
+  if (this->size <= 0)
     data = nullptr;
+  else
+    this->data = new T[size]();
+}
+
+template <typename T>
+Matrix1Of<T>::Matrix1Of(int size, T* data) : data(data), size(size) {
+  this->externalData = true;
+}
+
+template <typename T>
+Matrix1Of<T>::Matrix1Of(const Matrix1Of& m) : size(m.size) {
+  if (this->size <= 0)
+    this->data = nullptr;
   else {
-    this->data = new float[size]();
-    this->externalData = false;
+    this->data = new T[this->size];
+
+    for (unsigned int ix = 0; ix < this->size; ++ix)
+      this->data[ix] = m.data[ix];
   }
 }
 
-Matrix1::Matrix1(float* data, int size) : data(data), size(size) {
+template <typename T>
+Matrix1Of<T>::Matrix1Of(Matrix1Of&& m) noexcept : data(m.data), size(m.size) {
+  m.data = nullptr;  // Leave the moved-from object in a safe state
+  m.size = 0;
+}
+
+template <typename T>
+Matrix1Of<T>::~Matrix1Of() {
+  if (!this->externalData) {
+    if (this->size > 0)
+      delete[] data;
+  }
+}
+
+template <typename T>
+Matrix1Of<T>& Matrix1Of<T>::operator=(const Matrix1Of& m) {
+  if (this != &m) {
+    if (this->size > 0)
+      delete[] this->data;  // Free the current memory
+
+    this->size = m.size;
+    this->externalData = m.externalData;
+    if (this->size == 0)
+      this->data = nullptr;
+    else {
+      this->data = new T[this->size];
+      for (unsigned int ix = 0; ix < this->size; ++ix)
+        this->data[ix] = m.data[ix];
+    }
+  }
+  return *this;
+}
+
+template <typename T>
+Matrix1Of<T>& Matrix1Of<T>::operator=(Matrix1Of&& m) noexcept {
+  if (this != &m) {
+    if (this->size > 0) {
+      // printf("Matrix1 Move Assignment Deallocate %d\n", this->size);
+      delete[] this->data;  // Free the current memory
+    }
+
+    // Transfer ownership of resources
+    this->size = m.size;
+    this->data = m.data;
+    this->externalData = m.externalData;
+
+    // Leave the moved-from object in a valid state
+    m.size = 0;
+    m.data = nullptr;
+  }
+  return *this;
+}
+
+template <typename T>
+unsigned int Matrix1Of<T>::Size() const {
+  return this->size;
+}
+
+template <typename T>
+void Matrix1Of<T>::Resize(int size) {
+  if (!this->externalData && this->size > 0)
+    delete[] this->data;
+
+  this->externalData = false;
+
+  if (size <= 0) {
+    this->data = nullptr;
+    this->size = 0;
+  } else {
+    this->data = new T[size]();
+  }
+}
+
+template <typename T>
+Matrix1Of<T> Matrix1Of<T>::Zero(unsigned int size) {
+  Matrix1Of<T> r(size);
+  r.Fill(0);
+  return r;
+}
+
+template <typename T>
+void Matrix1Of<T>::Fill(T value) {
+  for (unsigned int ix = 0; ix < this->size; ix++)
+    this->data[ix] = value;
+}
+
+template <typename T>
+void Matrix1Of<T>::Fill(T value, unsigned int start, unsigned int stop) {
+  if (start > stop || start > this->size)
+    return;
+
+  if (stop > this->size)
+    stop = this->size;
+
+  for (unsigned int ix = start; ix < stop; ix++)
+    this->data[ix] = value;
+}
+
+template <typename T>
+Matrix1Of<T> Matrix1Of<T>::FromVector3(Vector3Of<T> v) {
+  Matrix1Of<T> r(3);
+  r.data[0] = v.horizontal;
+  r.data[1] = v.vertical;
+  r.data[2] = v.depth;
+  return r;
+}
+
+template <typename T>
+Vector3Of<T> Matrix1Of<T>::ToVector3() {
+  return Vector3Of<T>(this->data[0], this->data[1], this->data[2]);
+}
+
+template <typename T>
+Matrix1Of<float> Matrix1Of<T>::FromQuaternion(Quaternion q) {
+  Matrix1Of<float> r(4);
+  r(0) = q.x;
+  r(1) = q.y;
+  r(2) = q.z;
+  r(3) = q.w;
+  return r;
+}
+
+template <typename T>
+Quaternion Matrix1Of<T>::ToQuaternion() {
+    float x = static_cast<float>(data[0]);
+    float y = static_cast<float>(data[1]);
+    float z = static_cast<float>(data[2]);
+    float w = static_cast<float>(data[3]);
+    return Quaternion(x, y, z, w);
+}
+
+template <typename T>
+Matrix1Of<T> Matrix1Of<T>::Slice(unsigned int start, unsigned int stop) {
+  if (start < 0 || stop >= this->size)
+    std::cerr << "Slice index out of range." << std::endl;
+
+  Matrix1Of<T> result(stop - start);
+  int resultIx = 0;
+  for (unsigned int ix = start; ix < stop; ix++)
+    result.data[resultIx++] = this->data[ix];
+
+  return result;
+}
+
+template <typename T>
+void Matrix1Of<T>::UpdateSlice(unsigned int start, unsigned int stop, const Matrix1Of& source) const {
+  if (start > stop || start > this->size || start > source.size)
+    return;
+
+  if (stop > this->size)
+    stop = this->size;
+  if (stop > source.size)
+    stop = source.size;
+
+  unsigned int sourceIx = 0;
+  for (unsigned int ix = start; ix < stop; ix++, sourceIx++) {
+    this->data[ix] = source.data[sourceIx];
+  }
+}
+
+template <typename T>
+const T& Matrix1Of<T>::operator()(int ix) const {
+  return data[ix];
+}
+template <typename T>
+T& Matrix1Of<T>::operator()(int ix) {
+  return data[ix];
+}
+
+template class Matrix1Of<int>;
+template class Matrix1Of<float>;
+template class Matrix1Of<double>;
+
+#pragma endregion Matrix1Of
+
+/*
+#pragma region Matrix1
+
+Matrix1::Matrix1() {}
+
+Matrix1::Matrix1(int size) : size(size), externalData(false) {
+  if (this->size <= 0)
+    data = nullptr;
+  else
+    this->data = new float[size]();
+}
+
+Matrix1::Matrix1(int size, float* data) : data(data), size(size) {
   this->externalData = true;
 }
 
+Matrix1::Matrix1(const Matrix1& m) : size(m.size) {
+  if (this->size <= 0)
+    this->data = nullptr;
+  else {
+    this->data = new float[this->size];
+
+    for (int ix = 0; ix < this->size; ++ix)
+      this->data[ix] = m.data[ix];
+  }
+}
+
+Matrix1::Matrix1(Matrix1&& m) noexcept : data(m.data), size(m.size) {
+  m.data = nullptr;  // Leave the moved-from object in a safe state
+  m.size = 0;
+}
+
+Matrix1::~Matrix1() {
+  if (!this->externalData) {
+    if (this->size > 0)
+      delete[] data;
+  }
+}
+
+Matrix1& Matrix1::operator=(const Matrix1& m) {
+  if (this != &m) {
+    if (this->size > 0)
+      delete[] this->data;  // Free the current memory
+
+    this->size = m.size;
+    this->externalData = m.externalData;
+    if (this->size == 0)
+      this->data = nullptr;
+    else {
+      this->data = new float[this->size];
+      for (int ix = 0; ix < this->size; ++ix)
+        this->data[ix] = m.data[ix];
+    }
+  }
+  return *this;
+}
+
+Matrix1& Matrix1::operator=(Matrix1&& m) noexcept {
+  if (this != &m) {
+    if (this->size > 0) {
+      // printf("Matrix1 Move Assignment Deallocate %d\n", this->size);
+      delete[] this->data;  // Free the current memory
+    }
+
+    // Transfer ownership of resources
+    this->size = m.size;
+    this->data = m.data;
+    this->externalData = m.externalData;
+
+    // Leave the moved-from object in a valid state
+    m.size = 0;
+    m.data = nullptr;
+  }
+  return *this;
+}
+
+unsigned int Matrix1::Size() const {
+  return this->size;
+}
+
+void Matrix1::Resize(int size) {
+  if (!this->externalData && this->size > 0)
+    delete[] this->data;
+
+  this->externalData = false;
+
+  if (size <= 0) {
+    this->data = nullptr;
+    this->size = 0;
+  } else {
+    this->data = new float[size]();
+  }
+}
+
+Matrix1 Matrix1::FromVector3(Vector3Float v) {
+  Matrix1 r(3);
+  r.data[0] = v.horizontal;
+  r.data[1] = v.vertical;
+  r.data[2] = v.depth;
+  return r;
+}
+
+Matrix1 Matrix1::Zero(unsigned int size) {
+  Matrix1 r(size);
+  r.Fill(0);
+  return r;
+}
+
+void Matrix1::Fill(float value) {
+  for (int ix = 0; ix < this->size; ix++)
+    this->data[ix] = value;
+}
+
+void Matrix1::Fill(float value, unsigned int start, unsigned int stop) {
+  if (start > stop || start > this->size)
+    return;
+
+  if (stop > this->size)
+    stop = this->size;
+
+  for (unsigned int ix = start; ix < stop; ix++)
+    this->data[ix] = value;
+}
+
+Vector3Float Matrix1::ToVector3() {
+  return Vector3Float(this->data[0], this->data[1], this->data[2]);
+}
+
 Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
   Matrix1 r = Matrix1(4);
   float* data = r.data;
@@ -34,110 +354,182 @@ Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
   return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
 }
 
+Matrix1 Matrix1::Slice(int start, int stop) {
+  if (start < 0 || stop >= this->size)
+    std::cerr << "Slice index out of range." << std::endl;
+
+  Matrix1 result(stop - start);
+  int resultIx = 0;
+  for (int ix = start; ix < stop; ix++)
+    result.data[resultIx++] = this->data[ix];
+
+  return result;
+}
+
+void Matrix1::UpdateSlice(int start, int stop, const Matrix1& source) const {
+  if (start > stop || start > this->size || start > source.size)
+    return;
+
+  if (stop > this->size)
+    stop = this->size;
+  if (stop > source.size)
+    stop = source.size;
+
+  unsigned int sourceIx = 0;
+  for (int ix = start; ix < stop; ix++, sourceIx++) {
+    this->data[ix] = source.data[sourceIx];
+  }
+}
+
+const float& Matrix1::operator()(int ix) const {
+  return data[ix];
+}
+float& Matrix1::operator()(int ix) {
+  return data[ix];
+}
+
 // 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)
+Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols), externalData(false) {
+  this->size = nRows * nCols;
+
+  if (this->size <= 0) {
     this->data = nullptr;
-  else {
-    this->data = new float[this->nValues];
-    this->externalData = false;
+    return;
   }
+
+  // printf("Matrix2 Allocate %d\n", this->size * sizeof(float));
+  this->data = new float[this->size];
 }
 
-Matrix2::Matrix2(float* data, int nRows, int nCols)
-    : nRows(nRows), nCols(nCols), data(data) {
-  this->nValues = nRows * nCols;
+Matrix2::Matrix2(int nRows, int nCols, float* data) : nRows(nRows), nCols(nCols), data(data) {
+  // printf("Matrix2 No-Allocate %d\n", this->size * sizeof(float));
+  this->size = nRows * nCols;
   this->externalData = true;
 }
 
-Matrix2::Matrix2(const Matrix2& m)
-    : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) {
-  if (this->nValues == 0)
+Matrix2::Matrix2(const Matrix2& m) : nRows(m.nRows), nCols(m.nCols), size(m.size) {
+  // printf("Matrix2 Copy Allocate %d\n", this->size * sizeof(float));
+  if (this->size == 0)
     this->data = nullptr;
   else {
-    this->data = new float[this->nValues];
+    this->data = new float[this->size];
 
-    for (int ix = 0; ix < this->nValues; ++ix)
+    for (int ix = 0; ix < this->size; ++ix)
       this->data[ix] = m.data[ix];
   }
 }
 
+Matrix2::Matrix2(Matrix2&& m) noexcept
+    : nRows(m.nRows), nCols(m.nCols), size(m.size), data(m.data) {
+  m.nRows = 0;
+  m.nCols = 0;
+  m.size = 0;
+  m.data = nullptr;  // Leave the moved-from object in a safe state
+}
+
 Matrix2& Matrix2::operator=(const Matrix2& m) {
   if (this != &m) {
-    delete[] this->data;  // Free the current memory
+    if (this->size > 0) {
+      // printf("Matrix2 Copy Assignment Deallocate %d\n", this->size * sizeof(float));
+      delete[] this->data;  // Free the current memory
+    }
 
     this->nRows = m.nRows;
     this->nCols = m.nCols;
-    this->nValues = m.nValues;
-    if (this->nValues == 0)
+    this->size = m.size;
+    this->externalData = m.externalData;
+    if (this->size == 0)
       this->data = nullptr;
     else {
-      this->data = new float[this->nValues];
-      for (int ix = 0; ix < this->nValues; ++ix)
+      this->data = new float[this->size];
+      for (int ix = 0; ix < this->size; ++ix)
         this->data[ix] = m.data[ix];
     }
   }
   return *this;
 }
 
-Matrix2::~Matrix2() {
-  if (!this->externalData)
-    delete[] data;
-}
+Matrix2& Matrix2::operator=(Matrix2&& m) noexcept {
+  if (this != &m) {
+    if (this->size > 0) {
+      // printf("Matrix2 Move Assignment Deallocate %d\n", this->size * sizeof(float));
+      delete[] this->data;  // Free the current memory
+    }
 
-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;
-}
+    // Transfer ownership of resources
+    this->nRows = m.nRows;
+    this->nCols = m.nCols;
+    this->size = m.size;
+    this->externalData = m.externalData;
+    this->data = m.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
+    // Leave the moved-from object in a valid state
+    m.size = 0;
+    m.data = nullptr;
   }
   return *this;
 }
 
+Matrix2::~Matrix2() {
+  if (!this->externalData) {
+    if (this->size > 0) {
+      // printf("Matrix2 Deallocate %d\n", this->size * sizeof(float));
+      delete[] data;
+    }
+  }
+}
+
+unsigned int Matrix2::NRows() const {
+  return this->nRows;
+}
+
+unsigned int Matrix2::NCols() const {
+  return this->nCols;
+}
+
 Matrix2 Matrix2::Zero(int nRows, int nCols) {
   Matrix2 r = Matrix2(nRows, nCols);
-  for (int ix = 0; ix < r.nValues; ix++)
+  for (int ix = 0; ix < r.size; ix++)
     r.data[ix] = 0;
   return r;
 }
 
-void Matrix2::Clear() {
-  for (int ix = 0; ix < this->nValues; ix++)
-    this->data[ix] = 0;
+const float& Matrix2::operator()(int row, int col) const {
+  return data[row * this->nCols + col];
+}
+float& Matrix2::operator()(int row, int col) {
+  return data[row * this->nCols + col];
+}
+
+void Matrix2::Fill(float value) {
+  for (int ix = 0; ix < this->size; ix++)
+    this->data[ix] = value;
+}
+
+void Matrix2::FillDiagonal(float f) {
+  int n = this->nRows < this->nCols ? nRows : nCols;
+  for (int ix = 0; ix < n; ix++)
+    this->data[ix * this->nCols + ix] = f;
 }
 
 Matrix2 Matrix2::Identity(int size) {
   return Diagonal(1, size);
 }
 
+Matrix2 Matrix2::Identity(int nRows, int nCols) {
+  Matrix2 m = Matrix2::Zero(nRows, nCols);
+  m.FillDiagonal(1);
+  return m;
+}
+
 Matrix2 Matrix2::Diagonal(float f, int size) {
   Matrix2 r = Matrix2::Zero(size, size);
   float* data = r.data;
@@ -149,15 +541,18 @@ Matrix2 Matrix2::Diagonal(float f, int size) {
   return r;
 }
 
-Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
+Matrix2 Matrix2::SkewMatrix(const Vector3Of<float>& 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)
+  data[0 * 3 + 0] = 0;
+  data[0 * 3 + 1] = -v.depth;    // result(0, 1)
+  data[0 * 3 + 2] = v.vertical;  // result(0, 2)
+  data[1 * 3 + 0] = v.depth;     // result(1, 0)
+  data[1 * 3 + 1] = 0;
+  data[1 * 3 + 2] = -v.horizontal;  // result(1, 2)
+  data[2 * 3 + 0] = -v.vertical;    // result(2, 0)
+  data[2 * 3 + 1] = v.horizontal;   // result(2, 1)
+  data[2 * 3 + 2] = 0;
   return r;
 }
 
@@ -166,39 +561,56 @@ Matrix2 Matrix2::Transpose() const {
 
   for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
     for (int colIx = 0; colIx < this->nCols; colIx++)
-      r.data[colIx * this->nCols + rowIx] =
-          this->data[rowIx * this->nCols + colIx];
+      r.data[colIx * this->nRows + 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++)
+  for (int ix = 0; ix < r.size; 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++)
+  for (int ix = 0; ix < r.size; ix++)
     r.data[ix] = this->data[ix] + v.data[ix];
   return r;
 }
 
-Matrix2 Matrix2::operator+=(const Matrix2& v) {
-  for (int ix = 0; ix < this->nValues; ix++)
+Matrix2& Matrix2::operator+=(const Matrix2& v) {
+  for (int ix = 0; ix < this->size; ix++)
     this->data[ix] += v.data[ix];
   return *this;
 }
 
-Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
+Matrix2 Matrix2::operator-(const Matrix2& v) const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  for (int ix = 0; ix < r.size; ix++)
+    r.data[ix] = this->data[ix] - v.data[ix];
+  return r;
+}
+
+Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
+  Matrix1 r = Matrix1(m.nRows);
+  for (int rowIx = 0; rowIx < m.nRows; rowIx++) {
+    int mRowIx = rowIx * m.nCols;
+    for (int colIx = 0; colIx < m.nCols; colIx++)
+      r(rowIx) += m.data[mRowIx + colIx] * v(colIx);
+    // r(rowIx) += m(rowIx, colIx) * v(colIx);
+  }
+  return r;
+}
+
+Matrix2 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;
+  const int ACols = this->nCols;
+  const int BCols = B.nCols;
+  const int ARows = this->nRows;
+  // const int BRows = B.nRows;
 
   for (int i = 0; i < ARows; ++i) {
     // Pre-compute row offsets
@@ -206,32 +618,95 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
     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";
+      // printf(" 0");
       int BIndex = j;
       for (int k = 0; k < ACols; ++k) {
-        std::cout << " + " << this->data[ARowOffset + k] << " * "
-                  << B.data[BIndex];
+        // printf(" + %f * %f", this->data[ARowOffset + k], B.data[BIndex]);
+        //  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";
+      // std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
+      // if (sum != std::trunc(sum)) {
+      //   int BIndex = j;
+      //   printf(" 0");
+      //   for (int k = 0; k < ACols; ++k) {
+      //     printf(" + %f * %f", this->data[ARowOffset + k], B.data[BIndex]);
+      //     BIndex += BCols;
+      //   }
+      //   printf(" = %f ix: %d\n", sum, BColOffset + j);
+      // }
+    }
+  }
+
+  //  if (ACols != BRows)
+  //   printf("### Acols != Nrows\n");
+
+  //       for (size_t i = 0; i < ARows; ++i) {
+  //         for (size_t p = 0; p < ACols; ++p) {
+  //             float a_ip = this->data[i * ACols + p];           // A(i,p)
+  //             for (size_t j = 0; j < BCols; ++j) {
+  //                 r.data[i * BCols + j] += a_ip * B.data[p * BCols + j]; //
+  //                 C(i,j) += A(i,p)*B(p,j)
+  //             }
+  //         }
+  //     }
+
+  return r;
+}
+
+Matrix2 Matrix2::GetRows(int from, int to) {
+  if (from < 0 || to >= this->nRows)
+    std::cerr << "Slice index out of range." << std::endl;
+
+  Matrix2 result = Matrix2(to - from, this->nCols);
+  int resultRowIx = 0;
+  for (int rowIx = from; rowIx < to; rowIx++) {
+    for (int colIx = 0; colIx < this->nCols; colIx++)
+      result.data[resultRowIx * result.nCols + colIx] = this->data[rowIx * this->nCols + colIx];
+
+    resultRowIx++;
+  }
+
+  return result;
+}
+
+Vector3 Matrix2::GetRow3(int rowIx) {
+  int cellIx = rowIx * this->nCols;
+  Vector3 row(this->data[cellIx + 0], this->data[cellIx + 1], this->data[cellIx + 2]);
+  return row;
+}
+
+void Matrix2::SetRow(int rowIx, Matrix1 source) {
+  int cellIx = rowIx * this->nCols;
+  for (int ix = 0; ix < source.Size(); ix++)
+    this->data[cellIx + ix] = source(ix);
+}
+
+void Matrix2::SetRow3(int rowIx, Vector3 v) {
+  int cellIx = rowIx * this->nCols;
+  this->data[cellIx + 0] = v.x;
+  this->data[cellIx + 1] = v.y;
+  this->data[cellIx + 2] = v.z;
+}
+
+Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) const {
+  Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
+
+  for (int i = rowStart; i < rowStop; i++) {
+    for (int j = colStart; j < colStop; j++) {
+      int resultRowIx = i - rowStart;
+      int resultColIx = j - colStart;
+      r.data[resultRowIx * r.nCols + resultColIx] = this->data[i * this->nCols + j];
     }
   }
   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, const Matrix2& m) const {
+  UpdateSlice(rowStart, rowStop, 0, this->nCols, m);
 }
 
 void Matrix2::UpdateSlice(int rowStart,
@@ -239,111 +714,669 @@ void Matrix2::UpdateSlice(int rowStart,
                           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;
+  // printf("UpdateSlice ====== (%d, %d) -> (%d, %d)\n", rowStart, rowStop,
+  //        colStart, colStop);
   for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
-    rRowDataIx = rowIx * this->nCols;
-    // rRowDataIx += this->nCols;
-    mRowDataIx += m.nCols;
+    int rRowDataIx = rowIx * this->nCols;
     for (int colIx = colStart; colIx < colStop; colIx++) {
       this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
+      // printf("UpdateSlice (%d, %d) -> (%d, %d) = %F\n", mRowDataIx / m.nCols,
+      //        (colIx - colStart), rowIx, colIx, m.data[mRowDataIx + (colIx - colStart)]);
+    }
+    mRowDataIx += m.nCols;
+  }
+}
+
+
+// Matrix2 Matrix2::Inverse() {
+//   int n = this->nRows;
+//   // // Create an identity matrix of the same size as the original matrix
+//   Matrix2 augmentedMatrix(n, 2 * n);
+//   for (int i = 0; i < n; i++) {
+//     for (int j = 0; j < n; j++) {
+//       int ix = i * this->nCols + j;
+//       augmentedMatrix.data[ix] = this->data[ix];
+//       augmentedMatrix.data[ix + n] = (i == j) ? 1 : 0;  // identity matrix;
+//     }
+//     if (n == 3) {
+//       int ix2 = i * this->nCols;
+//       std::cerr << augmentedMatrix.data[ix2] << " " << augmentedMatrix.data[ix2+1] << " "
+//                 << augmentedMatrix.data[ix2+2] << " " << augmentedMatrix.data[ix2+3] << " "
+//                 << augmentedMatrix.data[ix2+4] << " " << augmentedMatrix.data[ix2+5] << "\n";
+//     }
+//   }
+
+//   // Perform Gaussian elimination
+//   for (int i = 0; i < n; i++) {
+//     // Find the pivot row
+//     float pivot = augmentedMatrix.data[i * augmentedMatrix.nCols + i];
+//     std::cerr << "pivot " << (int) i << " | " << (i * augmentedMatrix.nCols + i) << " = " << pivot
+// << std::endl; if (abs(pivot) < 1e-10)  // Check for singular matrix std::cerr << "Matrix is singular
+// and cannot be inverted. " << (int)i << std::endl;
+
+//     // Normalize the pivot row
+//     for (int j = 0; j < 2 * n; j++)
+//       augmentedMatrix.data[i * augmentedMatrix.nCols + j] /= pivot;
+
+//     // Eliminate the column below the pivot
+//     for (int j = i + 1; j < n; j++) {
+//       float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
+//       for (int k = 0; k < 2 * n; k++)
+//         augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
+//             factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
+//     }
+//   }
+
+//   // Back substitution
+//   for (int i = n - 1; i >= 0; i--) {
+//     // Eliminate the column above the pivot
+//     for (int j = i - 1; j >= 0; j--) {
+//       float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
+//       for (int k = 0; k < 2 * n; k++)
+//         augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
+//             factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
+//     }
+//   }
+
+//   // Extract the inverse matrix from the augmented matrix
+//   Matrix2 inverse(n, n);
+//   for (int i = 0; i < n; i++) {
+//     for (int j = 0; j < n; j++)
+//       inverse.data[i * inverse.nCols + j] = augmentedMatrix.data[i * augmentedMatrix.nCols + j + n];
+//   }
+//   return inverse;
+// }
+
+Matrix2 Matrix2::Inverse() {
+  int n = this->nRows;              // need to check for nRows = nCols...
+  Matrix2 matrix = Matrix2(*this);  // clone this matrix
+  Matrix2 inverse = Matrix2::Identity(n);
+
+  // Forward elimination
+  for (int i = 0; i < n; ++i) {
+    float pivot = matrix(i, i);
+    if (abs(pivot) < 1e-10)  // Check for singular matrix
+      std::cerr << "Matrix is singular and cannot be inverted. " << (int)i << std::endl;
+
+    for (int j = 0; j < n; ++j) {
+      matrix(i, j) /= pivot;   //  Normalize pivot row
+      inverse(i, j) /= pivot;  // Apply the same to inverse
+    }
+
+    for (int j = 0; j < n; ++j) {
+      if (j != i) {  // outside diagonal
+        float factor = matrix(j, i);
+        for (int k = 0; k < n; ++k) {
+          matrix(j, k) -= factor * matrix(i, k);    // Eliminate column
+          inverse(j, k) -= factor * inverse(i, k);  // Update inverse
+        }
+      }
     }
   }
+  return inverse;
+}
+
+Matrix2 Matrix2::DeleteRows(int rowStart, int rowStop) const {
+  Matrix2 r = Matrix2(this->nRows - (rowStop - rowStart), this->nCols);
+
+  int resultRowIx = 0;
+  for (int i = 0; i < this->nRows; i++) {
+    if (i >= rowStart && i < rowStop)
+      continue;
+
+    for (int j = 0; j < this->nCols; j++)
+      r.data[resultRowIx * r.nCols + j] = this->data[i * this->nCols + j];
+
+    resultRowIx++;
+  }
+  return r;
+}
+
+Matrix2 Matrix2::DeleteColumns(int colStart, int colStop) const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols - (colStop - colStart));
+
+  for (int i = 0; i < this->nRows; i++) {
+    int resultColIx = 0;
+    for (int j = 0; j < this->nCols; j++) {
+      if (j >= colStart && j < colStop)
+        continue;
+
+      r.data[i * r.nCols + resultColIx++] = this->data[i * this->nCols + j];
+    }
+  }
+  return r;
 }
 
 /// @brief Compute the Omega matrix of a 3D vector
 /// @param v The vector
 /// @return 4x4 Omega matrix
-Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) {
+template <typename T>
+Matrix2 Matrix2::Omega(const Vector3Of<T>& 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;
+  r.data[ix++] = -v.horizontal;
+  r.data[ix++] = -v.vertical;
+  r.data[ix] = -v.depth;
 
   // Set last column to v
   ix = 3;
-  r.data[ix += 4] = v.x;
-  r.data[ix += 4] = v.y;
-  r.data[ix] = v.z;
+  r.data[ix += 4] = v.horizontal;
+  r.data[ix += 4] = v.vertical;
+  r.data[ix] = v.depth;
   return r;
 }
 
 // Matrix2
 #pragma endregion
 
-}  // namespace LinearAlgebra
+#pragma region Block // Submatrix2
 
-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;
+SubMatrix2::SubMatrix2(const Matrix2& m, int startRow, int startCol, int rowCount, int colCount)
+    : parent(m), startRow(startRow), startCol(startCol), rowCount(rowCount), colCount(colCount) {
+  assert(this->startRow >= 0 && this->startCol >= 0);
+  assert(this->startRow + this->rowCount <= this->parent.NRows() &&
+         this->startCol + this->colCount <= this->parent.NCols());
+}
+
+void SubMatrix2::operator=(const Matrix2& src) {
+  assert(src.NRows() == this->rowCount && src.NCols() == this->colCount);
+
+  // If the block is contiguous in memory and src is contiguous, we can memcpy
+  // row by row. In row-major layout, each row of the block is contiguous.
+  for (int row = 0; row < this->rowCount; row++) {
+    float* dstRowPtr =
+        this->parent.GetData() + (this->startRow + row) * this->parent.NCols() + this->startCol;
+    const float* srcRowPtr = src.GetData() + row * src.NCols();
+    // printf("(%d, %d) %d   (%d -> %d)\n", this->startRow + row, startCol,
+    // this->colCount, (int)srcRowPtr, (int)dstRowPtr); memcpy each row
+    std::memcpy(dstRowPtr, srcRowPtr, sizeof(float) * this->colCount);
   }
-  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());
-}
+void SubMatrix2::operator=(const SubMatrix2& src) {
+  assert(src.rowCount == this->rowCount && src.colCount == this->colCount);
 
-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];
-      }
+  //  If src.parent == parent and regions overlap, we must copy safely (use
+  //  temp row buffer).
+  bool alias =
+      (&src.parent == &this->parent) && !((src.startRow + src.rowCount <= this->startRow) ||
+                                          (this->startRow + this->rowCount <= src.startRow) ||
+                                          (src.startCol + src.colCount <= this->startCol) ||
+                                          (this->startCol + this->colCount <= src.startCol));
+
+  if (!alias) {
+    // safe to copy row-by-row directly
+    for (int row = 0; row < this->rowCount; row++) {
+      float* dstRowPtr =
+          this->parent.GetData() + (this->startRow + row) * this->parent.NCols() + this->startCol;
+      const float* srcRowPtr =
+          src.parent.GetData() + (src.startRow + row) * src.parent.NCols() + src.startCol;
+      std::memcpy(dstRowPtr, srcRowPtr, sizeof(float) * this->colCount);
+    }
+  } else {
+    // overlapping regions in same parent: copy to temporary then assign
+    Matrix2 tmp(this->rowCount, this->colCount);
+    for (int row = 0; row < this->rowCount; row++) {
+      const float* srcRowPtr =
+          src.parent.GetData() + (src.startRow + row) * src.parent.NCols() + src.startCol;
+      std::memcpy(tmp.GetData() + row * this->colCount, srcRowPtr, sizeof(float) * this->colCount);
+    }
+    // now copy tmp into destination
+    for (int row = 0; row < this->rowCount; row++) {
+      float* dstRowPtr =
+          parent.GetData() + (this->startRow + row) * this->parent.NCols() + this->startCol;
+      std::memcpy(dstRowPtr, tmp.GetData() + row * this->colCount, sizeof(float) * this->colCount);
     }
   }
 }
 
-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);
+float SubMatrix2::operator()(int row, int col) const {
+  assert(row >= 0 && col >= 0 && row < rowCount && col < colCount);
+  return parent(startRow + row, startCol + col);
+}
 
-  Multiply(m, &v_m, &r_m);
+const float& SubMatrix2::operator()(int row, int col) {
+  assert(row >= 0 && col >= 0 && row < rowCount && col < colCount);
+  return parent(startRow + row, startCol + col);
+}
 
-  Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
+#pragma endregion Block
+*/
+
+#pragma region Matrix2Of
+
+template <typename T>
+Matrix2Of<T>::Matrix2Of() {}
+
+template <typename T>
+Matrix2Of<T>::Matrix2Of(unsigned int rowCount, unsigned int colCount)
+    : nRows(rowCount), nCols(colCount), externalData(false) {
+  this->size = this->nRows * this->nCols;
+
+  if (this->size <= 0) {
+    this->data = nullptr;
+    return;
+  }
+
+  this->data = new T[this->size];
+}
+
+template <typename T>
+Matrix2Of<T>::Matrix2Of(unsigned int rowCount, unsigned int colCount, T* data)
+    : nRows(rowCount), nCols(colCount), data(data), externalData(true) {
+  this->size = this->nRows * this->nCols;
+}
+
+template <typename T>
+Matrix2Of<T>::Matrix2Of(const Matrix2Of& m) : nRows(m.nRows), nCols(m.nCols), size(m.size) {
+  if (this->size == 0)
+    this->data = nullptr;
+  else {
+    this->data = new T[this->size];
+
+    for (unsigned int ix = 0; ix < this->size; ++ix)
+      this->data[ix] = m.data[ix];
+  }
+}
+
+template <typename T>
+Matrix2Of<T>::Matrix2Of(Matrix2Of&& m) noexcept
+    : nRows(m.nRows), nCols(m.nCols), size(m.size), data(m.data) {
+  m.nRows = 0;
+  m.nCols = 0;
+  m.size = 0;
+  m.data = nullptr;  // Leave the moved-from object in a safe state
+}
+
+template <typename T>
+Matrix2Of<T>::~Matrix2Of() {
+  if (!this->externalData) {
+    if (this->size > 0)
+      delete[] data;
+  }
+}
+
+template <typename T>
+Matrix2Of<T>& Matrix2Of<T>::operator=(const Matrix2Of& m) {
+  if (this != &m) {
+    if (this->size > 0) {
+      delete[] this->data;  // Free the current memory
+    }
+
+    this->nRows = m.nRows;
+    this->nCols = m.nCols;
+    this->size = m.size;
+    this->externalData = m.externalData;
+    if (this->size == 0)
+      this->data = nullptr;
+    else {
+      this->data = new T[this->size];
+      for (unsigned int ix = 0; ix < this->size; ++ix)
+        this->data[ix] = m.data[ix];
+    }
+  }
+  return *this;
+}
+
+template <typename T>
+Matrix2Of<T>& Matrix2Of<T>::operator=(Matrix2Of&& m) noexcept {
+  if (this != &m) {
+    if (this->size > 0) {
+      delete[] this->data;  // Free the current memory
+    }
+
+    // Transfer ownership of resources
+    this->nRows = m.nRows;
+    this->nCols = m.nCols;
+    this->size = m.size;
+    this->externalData = m.externalData;
+    this->data = m.data;
+
+    // Leave the moved-from object in a valid state
+    m.size = 0;
+    m.data = nullptr;
+  }
+  return *this;
+}
+
+template <typename T>
+unsigned int Matrix2Of<T>::RowCount() const {
+  return this->nRows;
+}
+template <typename T>
+unsigned int Matrix2Of<T>::ColCount() const {
+  return this->nCols;
+}
+
+template <typename T>
+const T& Matrix2Of<T>::operator()(unsigned int rowIx, unsigned int colIx) const {
+  return data[rowIx * this->nCols + colIx];
+}
+template <typename T>
+T& Matrix2Of<T>::operator()(unsigned int rowIx, unsigned int colIx) {
+  return data[rowIx * this->nCols + colIx];
+}
+
+template <typename T>
+T* Matrix2Of<T>::GetData() {
+  return this->data;
+}
+template <typename T>
+T* Matrix2Of<T>::GetData() const {
+  return this->data;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::Zero(unsigned int rowCount, unsigned int colCount) {
+  Matrix2Of r(rowCount, colCount);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = 0;
   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);
+void Matrix2Of<T>::Fill(T value) {
+  for (unsigned int ix = 0; ix < this->size; ix++)
+    this->data[ix] = value;
+}
 
-  Multiply(this, &v_m, &r_m);
+template <typename T>
+void Matrix2Of<T>::FillDiagonal(T f) {
+  int n = this->nRows < this->nCols ? nRows : nCols;
+  for (int ix = 0; ix < n; ix++)
+    this->data[ix * this->nCols + ix] = f;
+}
 
-  Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
-  delete[] vData;
-  delete[] rData;
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::Identity(unsigned int size) {
+  return Diagonal(1, size);
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::Identity(unsigned int rowCount, unsigned int colCount) {
+  Matrix2Of<T> m = Matrix2Of::Zero(rowCount, colCount);
+  m.FillDiagonal(1);
+  return m;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::Diagonal(T f, unsigned int size) {
+  Matrix2Of r = Matrix2Of::Zero(size, size);
+  T* data = r.data;
+  int valueIx = 0;
+  for (unsigned int ix = 0; ix < size; ix++) {
+    data[valueIx] = f;
+    valueIx += size + 1;
+  }
   return r;
 }
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::SkewMatrix(const Vector3Of<T>& v) {
+  Matrix2Of r(3, 3);
+  T* data = r.data;
+  data[0 * 3 + 0] = 0;
+  data[0 * 3 + 1] = -v.depth;    // result(0, 1)
+  data[0 * 3 + 2] = v.vertical;  // result(0, 2)
+  data[1 * 3 + 0] = v.depth;     // result(1, 0)
+  data[1 * 3 + 1] = 0;
+  data[1 * 3 + 2] = -v.horizontal;  // result(1, 2)
+  data[2 * 3 + 0] = -v.vertical;    // result(2, 0)
+  data[2 * 3 + 1] = v.horizontal;   // result(2, 1)
+  data[2 * 3 + 2] = 0;
+  return r;
+}
+
+template <typename T>
+Matrix1Of<T> Matrix2Of<T>::GetRow(unsigned int rowIx) {
+  Matrix1Of<T> r = Matrix1Of<T>(this->nCols);
+  unsigned int rowStartIx = rowIx * this->nCols;
+  for (unsigned int colIx = 0; colIx < this->nCols; colIx++) 
+    r.data[colIx] = this->data[rowStartIx + colIx];
+  return r;
+}
+
+template <typename T>
+Vector3Of<T> Matrix2Of<T>::GetRow3(unsigned int rowIx) {
+  int cellIx = rowIx * this->nCols;
+  Vector3Of<T> row(this->data[cellIx + 0], this->data[cellIx + 1], this->data[cellIx + 2]);
+  return row;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::GetRows(unsigned int fromRowIx, unsigned int toRowIx) {
+  if (toRowIx >= this->nRows)
+    std::cerr << "Slice index out of range." << std::endl;
+
+  Matrix2Of result = Matrix2Of(toRowIx - fromRowIx, this->nCols);
+  int resultRowIx = 0;
+  for (unsigned int rowIx = fromRowIx; rowIx < toRowIx; rowIx++) {
+    for (unsigned int colIx = 0; colIx < this->nCols; colIx++)
+      result.data[resultRowIx * result.nCols + colIx] = this->data[rowIx * this->nCols + colIx];
+
+    resultRowIx++;
+  }
+
+  return result;
+}
+
+template <typename T>
+void Matrix2Of<T>::SetRow(unsigned int rowIx, Matrix1Of<T> source) {
+  int cellIx = rowIx * this->nCols;
+  for (unsigned int ix = 0; ix < source.Size(); ix++)
+    this->data[cellIx + ix] = source(ix);
+}
+
+template <typename T>
+void Matrix2Of<T>::SetRow3(unsigned int rowIx, Vector3Of<T> v) {
+  int cellIx = rowIx * this->nCols;
+  this->data[cellIx + 0] = v.horizontal;
+  this->data[cellIx + 1] = v.vertical;
+  this->data[cellIx + 2] = v.depth;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::DeleteRows(unsigned int fromRowIx, unsigned int toRowIx) const {
+  Matrix2Of r(this->nRows - (toRowIx - fromRowIx), this->nCols);
+
+  int resultRowIx = 0;
+  for (unsigned int i = 0; i < this->nRows; i++) {
+    if (i >= fromRowIx && i < toRowIx)
+      continue;
+
+    for (unsigned int j = 0; j < this->nCols; j++)
+      r.data[resultRowIx * r.nCols + j] = this->data[i * this->nCols + j];
+
+    resultRowIx++;
+  }
+  return r;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::DeleteColumns(unsigned int fromColIx, unsigned int toColIx) const {
+  Matrix2Of r(this->nRows, this->nCols - (toColIx - fromColIx));
+
+  for (unsigned int i = 0; i < this->nRows; i++) {
+    int resultColIx = 0;
+    for (unsigned int j = 0; j < this->nCols; j++) {
+      if (j >= fromColIx && j < toColIx)
+        continue;
+
+      r.data[i * r.nCols + resultColIx++] = this->data[i * this->nCols + j];
+    }
+  }
+  return r;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::operator-() const {
+  Matrix2Of r(this->nRows, this->nCols);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = -this->data[ix];
+  return r;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::operator+(const Matrix2Of& v) const {
+  Matrix2Of r(this->nRows, this->nCols);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = this->data[ix] + v.data[ix];
+  return r;
+}
+
+template <typename T>
+Matrix2Of<T>& Matrix2Of<T>::operator+=(const Matrix2Of& v) {
+  for (unsigned int ix = 0; ix < this->size; ix++)
+    this->data[ix] += v.data[ix];
+  return *this;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::operator-(const Matrix2Of& v) const {
+  Matrix2Of r(this->nRows, this->nCols);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = this->data[ix] - v.data[ix];
+  return r;
+}
+
+template <typename T>
+Matrix2Of<T>& Matrix2Of<T>::operator-=(const Matrix2Of& v) {
+  for (unsigned int ix = 0; ix < this->size; ix++)
+    this->data[ix] -= v.data[ix];
+  return *this;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::operator*(const Matrix2Of& B) const {
+  Matrix2Of r(this->nRows, B.nCols);
+
+  const int ACols = this->nCols;
+  const int BCols = B.nCols;
+  const int ARows = this->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) {
+      T 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;
+}
+
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::Transposed() const {
+  Matrix2Of r(this->nCols, this->nRows);
+
+  for (unsigned int rowIx = 0; rowIx < this->nRows; rowIx++) {
+    for (unsigned int colIx = 0; colIx < this->nCols; colIx++)
+      r.data[colIx * this->nRows + rowIx] = this->data[rowIx * this->nCols + colIx];
+  }
+  return r;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::Inverse() {
+  int n = this->nRows;              // need to check for nRows = nCols...
+  Matrix2Of matrix = Matrix2Of(*this);  // clone this matrix
+  Matrix2Of inverse = Matrix2Of::Identity(n);
+
+  // Forward elimination
+  for (int i = 0; i < n; ++i) {
+    T pivot = matrix(i, i);
+    if (abs(pivot) < 1e-10)  // Check for singular matrix
+      std::cerr << "Matrix is singular and cannot be inverted. " << (int)i << std::endl;
+
+    for (int j = 0; j < n; ++j) {
+      matrix(i, j) /= pivot;   //  Normalize pivot row
+      inverse(i, j) /= pivot;  // Apply the same to inverse
+    }
+
+    for (int j = 0; j < n; ++j) {
+      if (j != i) {  // outside diagonal
+        T factor = matrix(j, i);
+        for (int k = 0; k < n; ++k) {
+          matrix(j, k) -= factor * matrix(i, k);    // Eliminate column
+          inverse(j, k) -= factor * inverse(i, k);  // Update inverse
+        }
+      }
+    }
+  }
+  return inverse;
+}
+
+template <typename T>
+Matrix2Of<T> Matrix2Of<T>::Slice(int rowStart, int rowStop, int colStart, int colStop) const {
+  Matrix2Of r(rowStop - rowStart, colStop - colStart);
+
+  for (int i = rowStart; i < rowStop; i++) {
+    for (int j = colStart; j < colStop; j++) {
+      int resultRowIx = i - rowStart;
+      int resultColIx = j - colStart;
+      r.data[resultRowIx * r.nCols + resultColIx] = this->data[i * this->nCols + j];
+    }
+  }
+  return r;
+}
+
+template <typename T>
+void Matrix2Of<T>::UpdateSlice(int rowStart, int rowStop, const Matrix2Of& m) const {
+  UpdateSlice(rowStart, rowStop, 0, this->nCols, m);
+}
+
+template <typename T>
+void Matrix2Of<T>::UpdateSlice(int rowStart,
+                          int rowStop,
+                          int colStart,
+                          int colStop,
+                          const Matrix2Of& m) const {
+  int mRowDataIx = 0;
+  // printf("UpdateSlice ====== (%d, %d) -> (%d, %d)\n", rowStart, rowStop,
+  //        colStart, colStop);
+  for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
+    int rRowDataIx = rowIx * this->nCols;
+    for (int colIx = colStart; colIx < colStop; colIx++) {
+      this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
+      // printf("UpdateSlice (%d, %d) -> (%d, %d) = %F\n", mRowDataIx / m.nCols,
+      //        (colIx - colStart), rowIx, colIx, m.data[mRowDataIx + (colIx - colStart)]);
+    }
+    mRowDataIx += m.nCols;
+  }
+}
+
+// template <typename T>
+// Matrix2Of<T> Matrix2Of<T>::Omega(const Vector3Of<T>& v) {
+//   Matrix2Of r = Matrix2Of::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.horizontal;
+//   r.data[ix++] = -v.vertical;
+//   r.data[ix] = -v.depth;
+
+//   // Set last column to v
+//   ix = 3;
+//   r.data[ix += 4] = v.horizontal;
+//   r.data[ix += 4] = v.vertical;
+//   r.data[ix] = v.depth;
+//   return r;
+// }
+
+#pragma endregion Matrix2Of
+
+}  // namespace LinearAlgebra
diff --git a/Matrix.h b/Matrix.h
index ef72922..2aa7e0f 100644
--- a/Matrix.h
+++ b/Matrix.h
@@ -4,52 +4,213 @@
 #include "Quaternion.h"
 #include "Vector3.h"
 
+#include <cstring>  // for memcpy
+
 namespace LinearAlgebra {
 
+template <typename T>
+class Matrix2Of;  // forward declaration
+
 /// @brief A 1-dimensional matrix or vector of arbitrary size
-class Matrix1 {
+template <typename T>
+class Matrix1Of {
  public:
-  float* data = nullptr;
-  int size = 0;
+  /// @brief Create a zero-sized matrix
+  Matrix1Of();
+  /// @brief Create a 1 dimensional matrix
+  /// @param size the length of the matrix
+  Matrix1Of(int size);
+  /// @brief Create a 1 dimensional matrix with the given data
+  /// @param size the length of the matrix
+  /// @param data the data used to fill the matrix
+  Matrix1Of(int size, T* data);
+  /// @brief Make a copy of a matrix (copy constructor)
+  /// @param m the matrix to copy
+  Matrix1Of(const Matrix1Of& m);
+  /// @brief Move the data of matrix m to a new matrix (move constructor)
+  /// @param m the matrix to move
+  /// The data of the matrix m will be re-used for the new matrix.
+  /// @note This operator will empty matrix m
+  Matrix1Of(Matrix1Of&& m) noexcept;
 
-  Matrix1(int size);
-  Matrix1(float* data, int size);
+  /// @brief matrix destructor
+  ~Matrix1Of();
 
-  static Matrix1 FromQuaternion(Quaternion q);
+  /// @brief Copy the contents of a matrix
+  /// @param m the matrix to copy
+  /// @return The matrix with a copy of the contents of m
+  Matrix1Of& operator=(const Matrix1Of& m);
+  /// @brief Move the contents of a matrix
+  /// @param m the matrix for which the contents should be moved
+  /// @return A matrix with the contents of matrix m
+  /// @note This operator will empty matrix m
+  Matrix1Of& operator=(Matrix1Of&& m) noexcept;
+
+  /// @brief Get the size of the matrix
+  /// @return the size of the matrix
+  unsigned int Size() const;
+  /// @brief Reize the matrix
+  /// @param size The resized matrix
+  /// @note This will destruct the data contents of the matrix
+  void Resize(int size);
+
+  /// @brief Create a matrix initialized to all zeros
+  /// @param size The length of the matrix
+  /// @return the initialized matrix
+  static Matrix1Of Zero(unsigned int size);
+  /// @brief Overwrite all elements with a new value
+  /// @param value the value to set for all elements
+  void Fill(T value);
+  /// @brief Overwrite a range of elements with a new value
+  /// @param value the value to set
+  /// @param start the first element to overwrite
+  /// @param stop the first element to not overwrite
+  /// @remark This will overwrite the elements [start..stop)
+  /// So the element at index 'stop' will not be overwritten
+  void Fill(T value, unsigned int start, unsigned int stop);
+
+  /// @brief Convert from a 3d vector
+  /// @param v the vector to convert
+  /// @return a matrix with the 3 elements of the vector
+  static Matrix1Of FromVector3(Vector3Of<T> v);
+  /// @brief Convert to a 3d vector
+  /// @return a vector with the first 3 elements of the matrix
+  Vector3Of<T> ToVector3();
+
+  /// @brief Convert from a quaternion
+  /// @param q the quaternion to convert
+  /// @return a matrix with the 4 elements of the quaternion
+  static Matrix1Of<float> FromQuaternion(Quaternion q);
+  /// @brief Convert to a quaternion
+  /// @return a quaternion with the first 4 elements of the quaternion
   Quaternion ToQuaternion();
 
+  /// @brief Get an element from the matrix
+  /// @param ix the index of the element to retrieve
+  /// @return the element at index ix
+  const T& operator()(int ix) const;
+  /// @brief Get the reference to an element of a matrix
+  /// @param ix the index of the element
+  /// @return a reference to the element of index ix
+  T& operator()(int ix);
+
+  /// @brief Get a subsection of a matrix
+  /// @param start the first element to retrieve
+  /// @param stop the first element to not retrieve
+  /// @return a matrix with the requested elements
+  /// @remark This will retrieve the elements [start..stop)
+  /// So the element at index 'stop' will not be included
+  Matrix1Of Slice(unsigned int start, unsigned int stop);
+  /// @brief Overwrite a subsection of a matrix with the values from another matrix
+  /// @param start the first element to overwrite
+  /// @param stop the first element to not overwrite
+  /// @param m a matrix with the value to write to the given range
+  /// @remark This will overwrite the elements [start..stop)
+  /// So the element at index 'stop' will not be overwritten
+  void UpdateSlice(unsigned int start, unsigned int stop, const Matrix1Of& m) const;
+
+ protected:
+  /// @brief The data values
+  T* data = nullptr;
+  /// @brief The length of the matrix
+  unsigned int size = 0;
+
  private:
+  /// @brief if this is true, functions will not do memory management
+  /// The allocation and deallocation of this->data should be handled outside this library
   bool externalData = true;
+
+  friend class Matrix2Of<T>;
+};
+
+using Matrix1Int = Matrix1Of<int>;
+using Matrix1Float = Matrix1Of<float>;
+using Matrix1Double = Matrix1Of<double>;
+
+using Matrix1 = Matrix1Float;
+
+/*
+class Matrix2;
+
+class SubMatrix2 {
+ public:
+  const Matrix2& parent;
+  int startRow;
+  int startCol;
+  int rowCount;
+  int colCount;
+
+  SubMatrix2(const Matrix2& m, int startRow, int startCol, int rowCount, int colCount);
+
+  // Assign from another full Matrix (sizes must match)
+  void operator=(const Matrix2& src);
+
+  // Assign from another SubMatrix2 (handles aliasing)
+  void operator=(const SubMatrix2& src);
+
+  // Read element
+  float operator()(int row, int col) const;
+
+  // Write element
+  const float& operator()(int row, int col);
 };
 
 /// @brief A 2-dimensional matrix of arbitrary size
 class Matrix2 {
  public:
-  int nRows = 0;
-  int nCols = 0;
-  int nValues = 0;
-  float* data = nullptr;
-
   Matrix2();
   Matrix2(int nRows, int nCols);
-  Matrix2(float* data, int nRows, int nCols);
-  Matrix2(const Matrix2& m);
-  Matrix2& operator=(const Matrix2& other);
+  Matrix2(int nRows, int nCols, float* data);
+  Matrix2(const Matrix2& m);      // Copy constructor
+  Matrix2(Matrix2&& m) noexcept;  // Move constructor
 
   ~Matrix2();
 
-  Matrix2 Clone() const;
+  Matrix2& operator=(const Matrix2& m);          // Copy assignment operator
+  Matrix2& operator=(Matrix2&& other) noexcept;  // Move assignment operator
+
+  unsigned int NRows() const;
+  unsigned int NCols() const;
+
+  float* GetData() { return this->data; }
+  float* GetData() const { return this->data; }
 
   static Matrix2 Zero(int nRows, int nCols);
-  void Clear();
+  void Fill(float value);
+  void FillDiagonal(float f);
 
   static Matrix2 Identity(int size);
-
+  static Matrix2 Identity(int nRows, int nCols);
   static Matrix2 Diagonal(float f, int size);
 
-  static Matrix2 SkewMatrix(const Vector3& v);
+  static Matrix2 SkewMatrix(const Vector3Of<float>& v);
 
   Matrix2 Transpose() const;
+  Matrix2 Inverse();
+
+  const float& operator()(int row, int col) const;
+  float& operator()(int row, int col);
+
+  Matrix2 GetRows(int from, int to);
+  Vector3 GetRow3(int rowIx);
+
+  void SetRow(int rowIx, Matrix1 source);
+  void SetRow3(int rowIx, Vector3 v);
+
+  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop) const;
+
+  void UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const;
+  void UpdateSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const;
+
+  inline SubMatrix2 GetBlock(int startRow, int startCol, int rowCount, int colCount) const {
+    return SubMatrix2(*this, startRow, startCol, rowCount, colCount);
+  }
+  inline Matrix2 CopyBlock(int startRow, int startCol, int rowCount, int colCount) const {
+    return Slice(startRow, startRow + rowCount, startCol, startCol + colCount);
+  }
+
+  Matrix2 DeleteRows(int rowStart, int rowStop) const;
+  Matrix2 DeleteColumns(int colStart, int colStop) const;
 
   Matrix2 operator-() const;
 
@@ -57,172 +218,203 @@ class Matrix2 {
   /// @param m The matrix to add to this matrix
   /// @return The result of the addition
   Matrix2 operator+(const Matrix2& v) const;
-  Matrix2 operator+=(const Matrix2& v);
+  Matrix2& operator+=(const Matrix2& v);
+
+  Matrix2 operator-(const Matrix2& v) const;
 
   Matrix2 operator*(const Matrix2& m) const;
   friend Matrix2 operator*(const Matrix2& m, float f) {
     Matrix2 r = Matrix2(m.nRows, m.nCols);
-    for (int ix = 0; ix < r.nValues; ix++)
+    for (int ix = 0; ix < r.size; ix++)
       r.data[ix] = m.data[ix] * f;
     return r;
   }
   friend Matrix2 operator*(float f, const Matrix2& m) {
     Matrix2 r = Matrix2(m.nRows, m.nCols);
-    for (int ix = 0; ix < r.nValues; ix++)
+    for (int ix = 0; ix < r.size; ix++)
       r.data[ix] = f * m.data[ix];
     return r;
   }
 
-  friend Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
-    Matrix1 r = Matrix1(m.nRows);
-    for (int rowIx = 0; rowIx < m.nRows; rowIx++) {
-      int mRowIx = rowIx * m.nCols;
-      for (int colIx = 0; colIx < m.nCols; colIx++)
-        r.data[rowIx] += m.data[mRowIx + colIx] * v.data[rowIx];
-    }
-    return r;
-  }
+  friend Matrix1 operator*(const Matrix2& m, const Matrix1& v);
 
   friend Matrix2 operator/(const Matrix2& m, float f) {
     Matrix2 r = Matrix2(m.nRows, m.nCols);
-    for (int ix = 0; ix < r.nValues; ix++)
+    for (int ix = 0; ix < r.size; ix++)
       r.data[ix] = m.data[ix] / f;
     return r;
   }
   friend Matrix2 operator/(float f, const Matrix2& m) {
     Matrix2 r = Matrix2(m.nRows, m.nCols);
-    for (int ix = 0; ix < r.nValues; ix++)
+    for (int ix = 0; ix < r.size; ix++)
       r.data[ix] = f / m.data[ix];
     return r;
   }
 
-  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
+  template <typename T>
+  static Matrix2 Omega(const Vector3Of<T>& v);
 
-  void UpdateSlice(int rowStart,
-                int rowStop,
-                int colStart,
-                int colStop,
-                const Matrix2& m) const;
-  // private:
-  //  move constructor and move assignment operator
-  Matrix2(Matrix2&& other) noexcept;
-  Matrix2& operator=(Matrix2&& other) noexcept;
+ protected:
+  int nRows = 0;
+  int nCols = 0;
 
-  static Matrix2 Omega(const Vector3& v);
+ protected:
+  int size = 0;
+  float* data = nullptr;
+
+ private:
+  bool externalData = true;
+};
+*/
+
+/// @brief A 2-dimensional matrix of arbitrary size
+template <typename T>
+class Matrix2Of {
+ public:
+  Matrix2Of();
+  Matrix2Of(unsigned int rowCount, unsigned int colCount);
+  Matrix2Of(unsigned int rowCount, unsigned int colCount, T* data);
+  Matrix2Of(const Matrix2Of& m);      // Copy Constructor
+  Matrix2Of(Matrix2Of&& m) noexcept;  // Move Contructor
+
+  ~Matrix2Of();
+
+  Matrix2Of& operator=(const Matrix2Of& m);      // Copy assignment operator
+  Matrix2Of& operator=(Matrix2Of&& m) noexcept;  // Move assignment operator
+
+  unsigned int RowCount() const;
+  unsigned int ColCount() const;
+
+  const T& operator()(unsigned int rowIx, unsigned int colIx) const;
+  T& operator()(unsigned int rowIx, unsigned int colIx);
+
+  T* GetData();
+  T* GetData() const;
+
+  static Matrix2Of Zero(unsigned int nRows, unsigned int nCols);
+  void Fill(T value);
+  void FillDiagonal(T f);
+
+  static Matrix2Of Identity(unsigned int size);
+  static Matrix2Of Identity(unsigned int nRows, unsigned int nCols);
+  static Matrix2Of Diagonal(T f, unsigned int size);
+
+  static Matrix2Of SkewMatrix(const Vector3Of<T>& v);
+
+  Matrix1Of<T> GetRow(unsigned int rowIx);
+  Vector3Of<T> GetRow3(unsigned int rowIx);
+  Matrix2Of<T> GetRows(unsigned int fromRowIx, unsigned int toRowIx);
+
+  void SetRow(unsigned int rowIx, Matrix1Of<T> source);
+  void SetRow3(unsigned int rowIx, Vector3Of<T> source);
+
+  Matrix2Of DeleteRows(unsigned int fromRowIx, unsigned int toRowIx) const;
+  Matrix2Of DeleteColumns(unsigned int fromColIx, unsigned int colStop) const;
+
+  Matrix2Of operator-() const;
+
+  /// @brief Add a matrix to this matrix
+  /// @param m The matrix to add to this matrix
+  /// @return The result of the addition
+  Matrix2Of operator+(const Matrix2Of& v) const;
+  Matrix2Of& operator+=(const Matrix2Of& v);
+
+  Matrix2Of operator-(const Matrix2Of& v) const;
+  Matrix2Of& operator-=(const Matrix2Of& v);
+
+  Matrix2Of operator*(const Matrix2Of& m) const;
+  template <typename U>
+  friend Matrix2Of<U> operator*(const Matrix2Of<U>& m, U f);
+  template <typename U>
+  friend Matrix2Of<U> operator*(U f, const Matrix2Of<U>& m);
+  template <typename U>
+  friend Matrix1Of<U> operator*(const Matrix2Of<U>& m, const Matrix1Of<U>& v);
+
+  template <typename U>
+  friend Matrix2Of<U> operator/(const Matrix2Of<U>& m, U f);
+  template <typename U>
+  friend Matrix2Of<U> operator/(U f, const Matrix2Of<U>& m);
+
+  Matrix2Of Transposed() const;
+  Matrix2Of Inverse();
+
+  Matrix2Of Slice(int rawStart, int rowStop, int colStart, int colStop) const;
+
+  void UpdateSlice(int rowStart, int rowStop, const Matrix2Of& m) const;
+  void UpdateSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2Of& m) const;
+
+  /// @brief Compute the Omega matrix of a 3D vector
+  /// @param v The vector
+  /// @return 4x4 Omega matrix
+  // static Matrix2Of Omega(const Vector3Of<T>& v);
+
+ protected:
+  unsigned int nRows = 0;
+  unsigned int nCols = 0;
+
+ protected:
+  unsigned int size = 0;
+  T* data = nullptr;
 
  private:
   bool externalData = true;
 };
 
-/// @brief Single precision float matrix
-template <typename T>
-class MatrixOf {
- public:
-  MatrixOf(unsigned int rows, unsigned int cols);
-  MatrixOf(unsigned int rows, unsigned int cols, const T* source)
-      : MatrixOf(rows, cols) {
-    Set(source);
+#pragma region friend functions
+
+// For a class template a friend function must also be a template and defined in the header (unless
+// you restrict to a concrete instantiation)...
+template <typename U>
+Matrix2Of<U> operator*(const Matrix2Of<U>& m, U f) {
+  Matrix2Of<U> r(m.nRows, m.nCols);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = m.data[ix] * f;
+  return r;
+}
+template <typename U>
+Matrix2Of<U> operator*(U f, const Matrix2Of<U>& m) {
+  Matrix2Of<U> r(m.nRows, m.nCols);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = f * m.data[ix];
+  return r;
+}
+template <typename U>
+Matrix1Of<U> operator*(const Matrix2Of<U>& m, const Matrix1Of<U>& v) {
+  Matrix1Of<U> r(m.nRows);
+  for (unsigned int rowIx = 0; rowIx < m.nRows; rowIx++) {
+    int mRowIx = rowIx * m.nCols;
+    for (unsigned int colIx = 0; colIx < m.nCols; colIx++)
+      r(rowIx) += m.data[mRowIx + colIx] * v(colIx);
   }
-  MatrixOf(Vector3 v);  // creates a 3,1 matrix
+  return r;
+}
 
-  ~MatrixOf() {
-    if (this->data == nullptr)
-      return;
+template <typename U>
+Matrix2Of<U> operator/(const Matrix2Of<U>& m, U f) {
+  Matrix2Of<U> r(m.nRows, m.nCols);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = m.data[ix] / f;
+  return r;
+}
+template <typename U>
+Matrix2Of<U> operator/(U f, const Matrix2Of<U>& m) {
+  Matrix2Of r(m.nRows, m.nCols);
+  for (unsigned int ix = 0; ix < r.size; ix++)
+    r.data[ix] = f / m.data[ix];
+  return r;
+}
 
-    delete[] this->data;
-  }
+#pragma endregion friend functions
 
-  /// @brief Transpose with result in matrix m
-  /// @param r The matrix in which the transposed matrix is stored
-  void Transpose(MatrixOf<T>* r) const {
-    // 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
-    unsigned int matrixSize = this->cols * this->rows;
-    unsigned int resultSize = r->rows * r->cols;
-    if (matrixSize != resultSize) {
-      // Return a null matrix;
-      // We dont set data to nullptr because it is allocated memory
-      // Instead we write all zeros
-      for (unsigned int dataIx = 0; dataIx < resultSize; dataIx++)
-        r->data[dataIx] = 0.0f;
-      r->rows = 0;
-      r->cols = 0;
-      return;
-    }
+template class Matrix2Of<int>;
+template class Matrix2Of<float>;
+template class Matrix2Of<double>;
 
-    r->cols = this->rows;
-    r->rows = this->cols;
+using Matrix2Int = Matrix2Of<int>;
+using Matrix2Float = Matrix2Of<float>;
+using Matrix2Double = Matrix2Of<double>;
 
-    for (unsigned int rDataIx = 0; rDataIx < matrixSize; rDataIx++) {
-      unsigned int rowIx = rDataIx / this->rows;
-      unsigned int colIx = rDataIx % this->rows;
-      unsigned int mDataIx = this->cols * colIx + rowIx;
-      r->data[rDataIx] = this->data[mDataIx];
-    }
-  }
-
-  static void Multiply(const MatrixOf<T>* m1,
-                       const MatrixOf<T>* m2,
-                       MatrixOf<T>* r);
-  void Multiply(const MatrixOf<T>* m, MatrixOf<T>* r) const {
-    Multiply(this, m, r);
-  }
-
-  static Vector3 Multiply(const MatrixOf<T>* m, Vector3 v);
-  Vector3 operator*(const Vector3 v) const;
-
-  T Get(unsigned int rowIx, unsigned int colIx) const {
-    unsigned int dataIx = rowIx * this->cols + colIx;
-    return this->data[dataIx];
-  }
-
-  void Set(unsigned int rowIx, unsigned int colIx, T value) {
-    unsigned int dataIx = rowIx * this->cols + colIx;
-    this->data[dataIx] = value;
-  }
-
-  // This function does not check on source size!
-  void Set(const T* source) {
-    unsigned int matrixSize = this->cols * this->rows;
-    for (unsigned int dataIx = 0; dataIx < matrixSize; dataIx++)
-      this->data[dataIx] = source[dataIx];
-  }
-
-  // This function does not check on source size!
-  void SetRow(unsigned int rowIx, const T* source) {
-    unsigned int dataIx = rowIx * this->cols;
-    for (unsigned int sourceIx = 0; sourceIx < this->cols; dataIx++, sourceIx++)
-      this->data[dataIx] = source[sourceIx];
-  }
-
-  // This function does not check on source size!
-  void SetCol(unsigned int colIx, const T* source) {
-    unsigned int dataIx = colIx;
-    for (unsigned int sourceIx = 0; sourceIx < this->cols;
-         dataIx += this->cols, sourceIx++)
-      this->data[dataIx] = source[sourceIx];
-  }
-
-  void CopyFrom(const MatrixOf<T>* m) {
-    unsigned int thisMatrixSize = this->cols * this->rows;
-    unsigned int mMatrixSize = m->cols * m->rows;
-    if (mMatrixSize != thisMatrixSize)
-      return;
-
-    for (unsigned int dataIx = 0; dataIx < thisMatrixSize; dataIx++)
-      this->data[dataIx] = m->data[dataIx];
-  }
-
-  unsigned int RowCount() const { return rows; }
-  unsigned int ColCount() const { return cols; }
-
- private:
-  unsigned int rows;
-  unsigned int cols;
-  T* data;
-};
+//using Matrix2 = Matrix2Float;
 
 }  // namespace LinearAlgebra
 // using namespace LinearAlgebra;
diff --git a/Matrix_old.cpp b/Matrix_old.cpp
new file mode 100644
index 0000000..4a763f3
--- /dev/null
+++ b/Matrix_old.cpp
@@ -0,0 +1,351 @@
+/*
+#include "Matrix.h"
+#if !defined(NO_STD) 
+#include <iostream>
+#endif
+
+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)
+    this->data = nullptr;
+  else {
+    this->data = new float[this->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(const Matrix2& m)
+    : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) {
+  if (this->nValues == 0)
+    this->data = nullptr;
+  else {
+    this->data = new float[this->nValues];
+
+    for (int ix = 0; ix < this->nValues; ++ix)
+      this->data[ix] = m.data[ix];
+  }
+}
+
+Matrix2& Matrix2::operator=(const Matrix2& m) {
+  if (this != &m) {
+    delete[] this->data;  // Free the current memory
+
+    this->nRows = m.nRows;
+    this->nCols = m.nCols;
+    this->nValues = m.nValues;
+    if (this->nValues == 0)
+      this->data = nullptr;
+    else {
+      this->data = new float[this->nValues];
+      for (int ix = 0; ix < this->nValues; ++ix)
+        this->data[ix] = m.data[ix];
+    }
+  }
+  return *this;
+}
+
+Matrix2::~Matrix2() {
+  if (!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::Zero(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 (int rowIx = 0; rowIx < this->nRows; rowIx++) {
+    for (int 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 Matrix2::operator+=(const Matrix2& v) {
+  for (int ix = 0; ix < this->nValues; ix++)
+    this->data[ix] += v.data[ix];
+  return *this;
+}
+
+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;
+}
+*/
\ No newline at end of file
diff --git a/Matrix_old.h b/Matrix_old.h
new file mode 100644
index 0000000..aa0a1e0
--- /dev/null
+++ b/Matrix_old.h
@@ -0,0 +1,233 @@
+/*
+
+#ifndef MATRIX_H
+#define MATRIX_H
+
+#include "Quaternion.h"
+#include "Vector3.h"
+
+namespace LinearAlgebra {
+
+/// @brief A 1-dimensional matrix or vector of arbitrary size
+class Matrix1 {
+ public:
+  float* data = nullptr;
+  int size = 0;
+
+  Matrix1(int size);
+  Matrix1(float* data, int size);
+
+  static Matrix1 FromQuaternion(Quaternion q);
+  Quaternion ToQuaternion();
+
+ private:
+  bool externalData = true;
+};
+
+/// @brief A 2-dimensional matrix of arbitrary size
+class Matrix2 {
+ public:
+  int nRows = 0;
+  int nCols = 0;
+  int nValues = 0;
+  float* data = nullptr;
+
+  Matrix2();
+  Matrix2(int nRows, int nCols);
+  Matrix2(float* data, int nRows, int nCols);
+  Matrix2(const Matrix2& m);
+  Matrix2& operator=(const Matrix2& other);
+
+  ~Matrix2();
+
+  Matrix2 Clone() const;
+
+  static Matrix2 Zero(int nRows, int nCols);
+  void Clear();
+
+  static Matrix2 Identity(int size);
+
+  static Matrix2 Diagonal(float f, int size);
+
+  static Matrix2 SkewMatrix(const Vector3& v);
+
+  Matrix2 Transpose() const;
+
+  Matrix2 operator-() const;
+
+  /// @brief Add a matrix to this matrix
+  /// @param m The matrix to add to this matrix
+  /// @return The result of the addition
+  Matrix2 operator+(const Matrix2& v) const;
+  Matrix2 operator+=(const Matrix2& v);
+
+  Matrix2 operator*(const Matrix2& m) const;
+  friend Matrix2 operator*(const Matrix2& m, float f) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = m.data[ix] * f;
+    return r;
+  }
+  friend Matrix2 operator*(float f, const Matrix2& m) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = f * m.data[ix];
+    return r;
+  }
+
+  friend Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
+    Matrix1 r = Matrix1(m.nRows);
+    for (int rowIx = 0; rowIx < m.nRows; rowIx++) {
+      int mRowIx = rowIx * m.nCols;
+      for (int colIx = 0; colIx < m.nCols; colIx++)
+        r.data[rowIx] += m.data[mRowIx + colIx] * v.data[rowIx];
+    }
+    return r;
+  }
+
+  friend Matrix2 operator/(const Matrix2& m, float f) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = m.data[ix] / f;
+    return r;
+  }
+  friend Matrix2 operator/(float f, const Matrix2& m) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = f / m.data[ix];
+    return r;
+  }
+
+  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
+
+  void UpdateSlice(int rowStart,
+                int rowStop,
+                int colStart,
+                int colStop,
+                const Matrix2& m) const;
+  // private:
+  //  move constructor and move assignment operator
+  Matrix2(Matrix2&& other) noexcept;
+  Matrix2& operator=(Matrix2&& other) noexcept;
+
+  static Matrix2 Omega(const Vector3& v);
+
+ private:
+  bool externalData = true;
+};
+
+/// @brief Single precision float matrix
+template <typename T>
+class MatrixOf {
+ public:
+  MatrixOf(unsigned int rows, unsigned int cols);
+  MatrixOf(unsigned int rows, unsigned int cols, const T* source)
+      : MatrixOf(rows, cols) {
+    Set(source);
+  }
+  MatrixOf(Vector3 v);  // creates a 3,1 matrix
+
+  ~MatrixOf() {
+    if (this->data == nullptr)
+      return;
+
+    delete[] this->data;
+  }
+
+  /// @brief Transpose with result in matrix m
+  /// @param r The matrix in which the transposed matrix is stored
+  void Transpose(MatrixOf<T>* r) const {
+    // 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
+    unsigned int matrixSize = this->cols * this->rows;
+    unsigned int resultSize = r->rows * r->cols;
+    if (matrixSize != resultSize) {
+      // Return a null matrix;
+      // We dont set data to nullptr because it is allocated memory
+      // Instead we write all zeros
+      for (unsigned int dataIx = 0; dataIx < resultSize; dataIx++)
+        r->data[dataIx] = 0.0f;
+      r->rows = 0;
+      r->cols = 0;
+      return;
+    }
+
+    r->cols = this->rows;
+    r->rows = this->cols;
+
+    for (unsigned int rDataIx = 0; rDataIx < matrixSize; rDataIx++) {
+      unsigned int rowIx = rDataIx / this->rows;
+      unsigned int colIx = rDataIx % this->rows;
+      unsigned int mDataIx = this->cols * colIx + rowIx;
+      r->data[rDataIx] = this->data[mDataIx];
+    }
+  }
+
+  static void Multiply(const MatrixOf<T>* m1,
+                       const MatrixOf<T>* m2,
+                       MatrixOf<T>* r);
+  void Multiply(const MatrixOf<T>* m, MatrixOf<T>* r) const {
+    Multiply(this, m, r);
+  }
+
+  static Vector3 Multiply(const MatrixOf<T>* m, Vector3 v);
+  Vector3 operator*(const Vector3 v) const;
+
+  T Get(unsigned int rowIx, unsigned int colIx) const {
+    unsigned int dataIx = rowIx * this->cols + colIx;
+    return this->data[dataIx];
+  }
+
+  void Set(unsigned int rowIx, unsigned int colIx, T value) {
+    unsigned int dataIx = rowIx * this->cols + colIx;
+    this->data[dataIx] = value;
+  }
+
+  // This function does not check on source size!
+  void Set(const T* source) {
+    unsigned int matrixSize = this->cols * this->rows;
+    for (unsigned int dataIx = 0; dataIx < matrixSize; dataIx++)
+      this->data[dataIx] = source[dataIx];
+  }
+
+  // This function does not check on source size!
+  void SetRow(unsigned int rowIx, const T* source) {
+    unsigned int dataIx = rowIx * this->cols;
+    for (unsigned int sourceIx = 0; sourceIx < this->cols; dataIx++, sourceIx++)
+      this->data[dataIx] = source[sourceIx];
+  }
+
+  // This function does not check on source size!
+  void SetCol(unsigned int colIx, const T* source) {
+    unsigned int dataIx = colIx;
+    for (unsigned int sourceIx = 0; sourceIx < this->cols;
+         dataIx += this->cols, sourceIx++)
+      this->data[dataIx] = source[sourceIx];
+  }
+
+  void CopyFrom(const MatrixOf<T>* m) {
+    unsigned int thisMatrixSize = this->cols * this->rows;
+    unsigned int mMatrixSize = m->cols * m->rows;
+    if (mMatrixSize != thisMatrixSize)
+      return;
+
+    for (unsigned int dataIx = 0; dataIx < thisMatrixSize; dataIx++)
+      this->data[dataIx] = m->data[dataIx];
+  }
+
+  unsigned int RowCount() const { return rows; }
+  unsigned int ColCount() const { return cols; }
+
+ private:
+  unsigned int rows;
+  unsigned int cols;
+  T* data;
+};
+
+}  // namespace LinearAlgebra
+// using namespace LinearAlgebra;
+
+#endif
+*/
\ No newline at end of file
diff --git a/Quaternion.cpp b/Quaternion.cpp
index dda8c18..5bae6c2 100644
--- a/Quaternion.cpp
+++ b/Quaternion.cpp
@@ -98,27 +98,27 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) {
   }
 }
 
-Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
-  Matrix2 r = Matrix2(3, 3);
+// Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
+//   Matrix2 r = Matrix2(3, 3);
 
-  float x = this->x;
-  float y = this->y;
-  float z = this->z;
-  float w = this->w;
+//   float x = this->x;
+//   float y = this->y;
+//   float z = this->z;
+//   float w = this->w;
 
-  float* data = r.data;
-  data[0 * 3 + 0] = 1 - 2 * (y * y + z * z);
-  data[0 * 3 + 1] = 2 * (x * y - w * z);
-  data[0 * 3 + 2] = 2 * (x * z + w * y);
-  data[1 * 3 + 0] = 2 * (x * y + w * z);
-  data[1 * 3 + 1] = 1 - 2 * (x * x + z * z);
-  data[1 * 3 + 2] = 2 * (y * z - w * x);
-  data[2 * 3 + 0] = 2 * (x * z - w * y);
-  data[2 * 3 + 1] = 2 * (y * z + w * x);
-  data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
+//   float* data = r.data;
+//   data[0 * 3 + 0] = 1 - 2 * (y * y + z * z);
+//   data[0 * 3 + 1] = 2 * (x * y - w * z);
+//   data[0 * 3 + 2] = 2 * (x * z + w * y);
+//   data[1 * 3 + 0] = 2 * (x * y + w * z);
+//   data[1 * 3 + 1] = 1 - 2 * (x * x + z * z);
+//   data[1 * 3 + 2] = 2 * (y * z - w * x);
+//   data[2 * 3 + 0] = 2 * (x * z - w * y);
+//   data[2 * 3 + 1] = 2 * (y * z + w * x);
+//   data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
 
-  return r;
-}
+//   return r;
+// }
 
 Quaternion Quaternion::operator*(const Quaternion& r2) const {
   return Quaternion(