From aede0e5cd3c76c4d34b0a1230c74b0a26ae4c27e Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 8 Apr 2025 17:25:29 +0200 Subject: [PATCH] Processomg Acc data * propagating --- EspIdf/EspIdfParticipant.cpp | 2 +- LinearAlgebra/Matrix.cpp | 61 +++++++++++++++++++++++++-------- LinearAlgebra/Matrix.h | 29 +++++++++++----- Participants/ParticipantUDP.cpp | 6 ++-- 4 files changed, 71 insertions(+), 27 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index c8188a5..099ae5c 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/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); diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index fca4360..c18ce3a 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,7 +1,9 @@ #include "Matrix.h" #include -#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) - : data(data), size(size) { +Matrix1::Matrix1(float* data, int size) : data(data), size(size) { this->externalData = true; } @@ -28,12 +29,7 @@ Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) { } Quaternion LinearAlgebra::Matrix1::ToQuaternion() { - return Quaternion( - this->data[0], - this->data[1], - this->data[2], - this->data[3] - ); + return Quaternion(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] << " * " - << 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"; } } 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::MatrixOf(unsigned int rows, unsigned int cols) { if (rows <= 0 || cols <= 0) { diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index a269bee..65d55d0 100644 --- a/LinearAlgebra/Matrix.h +++ b/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 \ No newline at end of file diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 08270cf..622574e 100644 --- a/Participants/ParticipantUDP.cpp +++ b/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 - // std::cout << "custom msg for unknown thing [" << (int)msg->networkId - // << "/" << (int)msg->thingId << "]\n"; + else + std::cout << "custom msg for unknown thing [" << (int)msg->networkId + << "/" << (int)msg->thingId << "]\n"; } }