First steps implementation Matrix operations

CMakeLists.txt

@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 3.13)  # CMake version check
 if(ESP_PLATFORM)
     idf_component_register(
-        SRC_DIRS "."
+        SRC_DIRS "." "LinearAlgebra"
-        INCLUDE_DIRS "."
+        INCLUDE_DIRS "." "LinearAlgebra"
     )
 else() 
     project(RoboidControl)

LinearAlgebra/Matrix.cpp

@@ -1,6 +1,105 @@
 #include "Matrix.h"
 
-template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
+#pragma region Matrix2
+
+Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
+  this->nValues = nRows * nCols;
+  data = new float[nValues]();
+}
+
+Matrix2::Matrix2(float* data, int nRows, int nCols)
+    : nRows(nRows), nCols(nCols), data(data) {
+  this->nValues = nRows * nCols;
+}
+
+Matrix2::~Matrix2() {
+  delete[] data;
+}
+
+Matrix2 Matrix2::Zero(int nRows, int nCols) {
+  Matrix2 m = Matrix2(nRows, nCols);
+  for (int ix = 0; ix < m.nValues; ix++)
+    m.data[ix] = 0;
+  return m;
+}
+
+Matrix2 Matrix2::Identity(int size) {
+  return Diagonal(1, size);
+}
+
+Matrix2 Matrix2::Diagonal(float f, int size) {
+  Matrix2 r = Matrix2(size, size);
+  float* data = r.data;
+  int valueIx = 0;
+  for (int ix = 0; ix < r.nValues; ix++) {
+    data[valueIx] = f;
+    valueIx += size + 1;
+  }
+  return r;
+}
+
+Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
+  Matrix2 r = Matrix2(3, 3);
+  float* data = r.data;
+  data[0 * 3 + 1] = -v.z;  // result(0, 1)
+  data[0 * 3 + 2] = v.y;   // result(0, 2)
+  data[1 * 3 + 0] = v.z;   // result(1, 0)
+  data[1 * 3 + 2] = -v.x;  // result(1, 2)
+  data[2 * 3 + 0] = -v.y;  // result(2, 0)
+  data[2 * 3 + 1] = v.x;   // result(2, 1)
+  return r;
+}
+
+Matrix2 LinearAlgebra::Matrix2::operator-() const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  for (int ix = 0; ix < r.nValues; ix++)
+    r.data[ix] = -this->data[ix];
+  return r;
+}
+
+Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
+  Matrix2 r = Matrix2(this->nRows, B.nCols);
+
+  int ACols = this->nCols;
+  int BCols = B.nCols;
+  int ARows = this->nRows;
+  //int BRows = B.nRows;
+
+  for (int i = 0; i < ARows; ++i) {
+    // Pre-compute row offsets
+    int ARowOffset = i * ACols;  // ARowOffset is constant for each row of A
+    int BColOffset = i * BCols;  // BColOffset is constant for each row of B
+    for (int j = 0; j < BCols; ++j) {
+      float sum = 0;
+      int BIndex = j;
+      for (int k = 0; k < ACols; ++k) {
+        sum += this->data[ARowOffset + k] * B.data[BIndex];
+        BIndex += BCols;
+      }
+      r.data[BColOffset + j] = sum;
+    }
+  }
+  return r;
+}
+
+void LinearAlgebra::Matrix2::SetSlice(int rowStart,
+                                      int rowStop,
+                                      int colStart,
+                                      int colStop,
+                                      const Matrix2& m) const {
+  for (int i = rowStart; i < rowStop; i++) {
+    for (int j = colStart; j < colStop; j++)
+      this->data[i * this->nCols + j] =
+          m.data[(i - rowStart) * m.nCols + (j - colStart)];
+    // this->data[i, j] = m.data[i - rowStart, j - colStart];
+  }
+}
+
+// Matrix2
+#pragma endregion
+
+template <>
+MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   if (rows <= 0 || cols <= 0) {
     this->rows = 0;
     this->cols = 0;
@@ -14,15 +113,17 @@ template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   this->data = new float[matrixSize]{0.0f};
 }
 
-template <> MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
+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,
+void MatrixOf<float>::Multiply(const MatrixOf<float>* m1,
-                               const MatrixOf<float> *m2, MatrixOf<float> *r) {
+                               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;
@@ -37,7 +138,7 @@ void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
 }
 
 template <>
-Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
+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);
 
@@ -47,10 +148,11 @@ Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
   return r;
 }
 
-template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
+template <typename T>
-  float *vData = new float[3]{v.Right(), v.Up(), v.Forward()};
+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]{};
+  float* rData = new float[3]{};
   MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
 
   Multiply(this, &v_m, &r_m);

LinearAlgebra/Matrix.h

@@ -5,6 +5,33 @@
 
 namespace LinearAlgebra {
 
+class Matrix2 {
+ public:
+  int nRows = 0;
+  int nCols = 0;
+  int nValues = 0;
+  float* data = nullptr;
+
+  Matrix2(int nRows, int nCols);
+  Matrix2(float* data, int nRows, int nCols);
+
+  ~Matrix2();
+
+  static Matrix2 Zero(int nRows, int nCols);
+
+  static Matrix2 Identity(int size);
+
+  static Matrix2 Diagonal(float f, int size);
+
+  static Matrix2 SkewMatrix(const Vector3& v);
+
+  Matrix2 operator-() const;
+
+  Matrix2 operator*(const Matrix2& m) const;
+
+  void SetSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const;
+};
+
 /// @brief Single precision float matrix
 template <typename T>
 class MatrixOf {

LinearAlgebra/Quaternion.cpp

@@ -6,6 +6,7 @@
 #include <float.h>
 #include <math.h>
 #include "Angle.h"
+#include "Matrix.h"
 #include "Vector3.h"
 
 void CopyQuat(const Quat& q1, Quat& q2) {
@@ -97,6 +98,28 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) {
   }
 }
 
+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* 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;
+}
+
 Quaternion Quaternion::operator*(const Quaternion& r2) const {
   return Quaternion(
       this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x,

LinearAlgebra/Quaternion.h

@@ -34,6 +34,8 @@ typedef struct Quat {
 
 namespace LinearAlgebra {
 
+class Matrix2;
+
 /// <summary>
 /// A quaternion
 /// </summary>
@@ -89,6 +91,8 @@ struct Quaternion : Quat {
   /// The euler angles performed in the order: Z, X, Y
   static Vector3 ToAngles(const Quaternion& q);
 
+  Matrix2 ToRotationMatrix();
+
   /// <summary>
   /// Rotate a vector using this quaterion
   /// </summary>

LinearAlgebra/Vector3.h

@@ -14,7 +14,7 @@ extern "C" {
 /// This is a C-style implementation
 /// This uses the right-handed coordinate system.
 typedef struct Vec3 {
- protected:
+ public:
   /// <summary>
   /// The right axis of the vector
   /// </summary>

LinearAlgebra/test/Matrix_test.cc

@@ -5,6 +5,58 @@
 
 #include "Matrix.h"
 
+TEST(Matrix2, Multiplication) {
+      // Test 1: Multiplying two 2x2 matrices
+      float dataA[] = {1, 2, 3, 4};
+      float dataB[] = {5, 6, 7, 8};
+      Matrix2 A(dataA, 2, 2);
+      Matrix2 B(dataB, 2, 2);
+  
+      Matrix2 result = A * B;
+  
+      float expectedData[] = {19, 22, 43, 50};
+      for (int i = 0; i < 4; ++i) {
+          //assert(result.data[i] == expectedData[i]);
+          EXPECT_TRUE(result.data[i] == expectedData[i]);
+      }
+      std::cout << "Test 1 passed: 2x2 matrix multiplication.\n";
+  
+      // Test 2: Multiplying a 3x2 matrix with a 2x3 matrix
+      float dataC[] = {1, 2, 3, 4, 5, 6};
+      float dataD[] = {7, 8, 9, 10, 11, 12};
+      Matrix2 C(dataC, 3, 2);
+      Matrix2 D(dataD, 2, 3);
+  
+      Matrix2 result2 = C * D;
+  
+      float expectedData2[] = {29, 32, 35, 65, 72, 79, 101, 112, 123};
+      for (int i = 0; i < 9; ++i) {
+          assert(result2.data[i] == expectedData2[i]);
+          EXPECT_TRUE(result2.data[i] == expectedData2[i]);
+      }
+      std::cout << "Test 2 passed: 3x2 * 2x3 matrix multiplication.\n";
+  
+      // Test 3: Multiplying with a zero matrix
+      Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
+      Matrix2 result3 = A * zeroMatrix;
+  
+      for (int i = 0; i < 4; ++i) {
+          assert(result3.data[i] == 0);
+          EXPECT_TRUE(result3.data[i] == 0);
+      }
+      std::cout << "Test 3 passed: Multiplication with zero matrix.\n";
+  
+      // Test 4: Multiplying with an identity matrix
+      Matrix2 identityMatrix = Matrix2::Identity(2);
+      Matrix2 result4 = A * identityMatrix;
+  
+      for (int i = 0; i < 4; ++i) {
+          assert(result4.data[i] == A.data[i]);
+          EXPECT_TRUE(result4.data[i] == A.data[i]);
+      }
+      std::cout << "Test 4 passed: Multiplication with identity matrix.\n";
+}
+
 TEST(MatrixSingle, Init) {
   // zero
   MatrixOf<float> m0 = MatrixOf<float>(0, 0);

LocalParticipant.cpp

@@ -196,6 +196,8 @@ bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) {
   Arduino::LocalParticipant* thisArduino =
       static_cast<Arduino::LocalParticipant*>(this);
   return thisArduino->Send(remoteParticipant, bufferSize);
+#else
+  return false;  
 #endif
 }
 
@@ -233,6 +235,8 @@ bool LocalParticipant::Publish(IMessage* msg) {
   Arduino::LocalParticipant* thisArduino =
       static_cast<Arduino::LocalParticipant*>(this);
   return thisArduino->Publish(msg);
+#else
+  return false;
 #endif
 }
 

LocalParticipant.h

@@ -87,9 +87,9 @@ class LocalParticipant : public Participant {
 #if defined(__unix__) || defined(__APPLE__)
   int sock;
 #endif
-  sockaddr_in remote_addr;
+  // sockaddr_in remote_addr;
-  sockaddr_in server_addr;
+  // sockaddr_in server_addr;
-  sockaddr_in broadcast_addr;
+  // sockaddr_in broadcast_addr;
 
 #endif