Processomg Acc data * propagating

EspIdf/EspIdfParticipant.cpp

@@ -107,7 +107,7 @@ void ParticipantUDP::Receive() {
       break;
     }
 
-    std::cout << "receiving " << packetSize << " bytes, msgId " << (int)this->buffer[0] << "\n";
+    // std::cout << "receiving " << packetSize << " bytes, msgId " << (int)this->buffer[0] << "\n";
     inet_ntoa_r(source_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
     unsigned int sender_port = ntohs(source_addr.sin_port);
 

LinearAlgebra/Matrix.cpp

@@ -1,7 +1,9 @@
 #include "Matrix.h"
 #include <iostream>
 
-#pragma region Matrix1
+namespace LinearAlgebra {
+
+  #pragma region Matrix1
 
 Matrix1::Matrix1(int size) : size(size) {
   if (this->size == 0)
@@ -12,8 +14,7 @@ Matrix1::Matrix1(int size) : size(size) {
   }
 }
 
-Matrix1::Matrix1(float* data, int size)
+Matrix1::Matrix1(float* data, int size) : data(data), size(size) {
-    : data(data), size(size) {
   this->externalData = true;
 }
 
@@ -28,12 +29,7 @@ Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
 }
 
 Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
-  return Quaternion(
+  return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
-    this->data[0],
-    this->data[1],
-    this->data[2],
-    this->data[3]
-  );
 }
 
 // Matrix1
@@ -64,6 +60,13 @@ Matrix2::~Matrix2() {
     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),
@@ -121,6 +124,16 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
   return r;
 }
 
+Matrix2 Matrix2::Transpose() const {
+  Matrix2 r = Matrix2(this->nCols, this->nRows);
+
+  for (uint rowIx = 0; rowIx < this->nRows; rowIx++) {
+    for (uint 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++)
@@ -149,22 +162,38 @@ 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";
+      // std::cout << " 0";
       int BIndex = j;
       for (int k = 0; k < ACols; ++k) {
-        std::cout << " + " << this->data[ARowOffset + k] << " * "
+        // std::cout << " + " << this->data[ARowOffset + k] << " * "
-                  << B.data[BIndex];
+        //           << 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";
     }
   }
   return r;
 }
 
-void LinearAlgebra::Matrix2::SetSlice(int rowStart,
+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,
@@ -181,7 +210,7 @@ void LinearAlgebra::Matrix2::SetSlice(int rowStart,
 /// @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));
+  r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));
 
   // set last row to -v
   int ix = 3 * 4;
@@ -200,6 +229,8 @@ Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) {
 // Matrix2
 #pragma endregion
 
+}  // namespace LinearAlgbra
+
 template <>
 MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   if (rows <= 0 || cols <= 0) {

LinearAlgebra/Matrix.h

@@ -36,6 +36,8 @@ class Matrix2 {
 
   ~Matrix2();
 
+  Matrix2 Clone() const;
+
   static Matrix2 Zero(int nRows, int nCols);
 
   static Matrix2 Identity(int size);
@@ -44,6 +46,8 @@ class Matrix2 {
 
   static Matrix2 SkewMatrix(const Vector3& v);
 
+  Matrix2 Transpose() const;
+
   Matrix2 operator-() const;
 
   /// @brief Add a matrix to this matrix
@@ -74,14 +78,23 @@ class Matrix2 {
     }
     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,
+  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,
@@ -208,6 +221,6 @@ class MatrixOf {
 };
 
 }  // namespace LinearAlgebra
-//using namespace LinearAlgebra;
+// using namespace LinearAlgebra;
 
 #endif

Participants/ParticipantUDP.cpp

@@ -374,9 +374,9 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
     thing = this->Get(msg->networkId, msg->thingId);
     if (thing != nullptr)
       thing->ProcessBinary(msg->bytes);
-    // else
+    else
-    //   std::cout << "custom msg for unknown thing [" << (int)msg->networkId
+      std::cout << "custom msg for unknown thing [" << (int)msg->networkId
-    //             << "/" << (int)msg->thingId << "]\n";
+                << "/" << (int)msg->thingId << "]\n";
   }
 }