RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Processomg Acc data * propagating

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-08 17:25:29 +02:00
parent bb33724873
commit aede0e5cd3
4 changed files with 71 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EspIdf/EspIdfParticipant.cpp
View File

@ -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);

61
LinearAlgebra/Matrix.cpp
View File

@ -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)
: 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<float>::MatrixOf(unsigned int rows, unsigned int cols) {
if (rows <= 0 || cols <= 0) {

29
LinearAlgebra/Matrix.h
View File

@ -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

6
Participants/ParticipantUDP.cpp
View File

@ -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";
}
}