Implemented integrate function

LinearAlgebra/Matrix.cpp

@@ -1,8 +1,48 @@
 #include "Matrix.h"
 #include <iostream>
 
+#pragma region Matrix1
+
+LinearAlgebra::Matrix1::Matrix1(int size) : size(size) {
+  if (this->size == 0)
+    data = nullptr;
+  else {
+    this->data = new float[size]();
+    this->externalData = false;
+  }
+}
+
+LinearAlgebra::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)
@@ -14,7 +54,7 @@ Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
 }
 
 Matrix2::Matrix2(float* data, int nRows, int nCols)
-    : nRows(nRows), nCols(nCols), data(data){
+    : nRows(nRows), nCols(nCols), data(data) {
   this->nValues = nRows * nCols;
   this->externalData = true;
 }
@@ -25,22 +65,26 @@ Matrix2::~Matrix2() {
 }
 
 // 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
+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;
+  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) {
@@ -84,13 +128,20 @@ Matrix2 LinearAlgebra::Matrix2::operator-() const {
   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 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;
+  // int BRows = B.nRows;
 
   for (int i = 0; i < ARows; ++i) {
     // Pre-compute row offsets
@@ -101,7 +152,8 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
       std::cout << " 0";
       int BIndex = j;
       for (int k = 0; k < ACols; ++k) {
-        std::cout << " + " << 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;
       }
@@ -121,10 +173,30 @@ void LinearAlgebra::Matrix2::SetSlice(int rowStart,
     for (int j = colStart; j < colStop; j++)
       this->data[i * this->nCols + j] =
           m.data[(i - rowStart) * m.nCols + (j - colStart)];
-    // this->data[i, j] = m.data[i - rowStart, j - colStart];
   }
 }
 
+/// @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.SetSlice(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
 

LinearAlgebra/Matrix.h

@@ -1,22 +1,40 @@
 #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:
+  int size = 0;
+  float* data = nullptr;
+
+  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;
-  bool externalData = true;
 
+  Matrix2();
   Matrix2(int nRows, int nCols);
   Matrix2(float* data, int nRows, int nCols);
 
- ~Matrix2();
+  ~Matrix2();
 
   static Matrix2 Zero(int nRows, int nCols);
 
@@ -28,13 +46,55 @@ class Matrix2 {
 
   Matrix2 operator-() const;
 
-  Matrix2 operator*(const Matrix2& m) 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;
 
-  void SetSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const;
-//private:
-  // move constructor and move assignment operator
+  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*(float f, const Matrix2& v) {
+    Matrix2 r = Matrix2(v.nRows, v.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = f * v.data[ix];
+    return r;
+  }
+
+  void SetSlice(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