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

Merge branch '0.4_dev' into MQTT

Browse Source
This commit is contained in:
Pascal Serrarens 2025-07-07 12:29:46 +02:00
commit c3f78079d3
27 changed files with 1128 additions and 733 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Arduino/ArduinoParticipant.cpp
View File

@ -88,7 +88,7 @@ void ParticipantUDP::Receive() {
#endif #endif
} }
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) { bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) {
#if defined(ARDUINO) && defined(HAS_WIFI) #if defined(ARDUINO) && defined(HAS_WIFI)
// std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":" // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
// << remoteParticipant->port << "\n"; // << remoteParticipant->port << "\n";

2
Arduino/ArduinoParticipant.h
View File

@ -9,7 +9,7 @@ class ParticipantUDP : public ParticipantUDPGeneric {
public: public:
void Setup(); void Setup();
void Receive(); void Receive();
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg); bool Publish(IMessage* msg);
protected: protected:

2
EspIdf/EspIdfParticipant.cpp
View File

@ -181,7 +181,7 @@ bool ParticipantUDP::Send(IMessage* msg) {
return this->SendTo(this->remoteSite, bufferSize); return this->SendTo(this->remoteSite, bufferSize);
} }
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant,
int bufferSize) { int bufferSize) {
#if defined(IDF_VER) #if defined(IDF_VER)
uint16_t port = ntohs(dest_addr.sin_port); uint16_t port = ntohs(dest_addr.sin_port);

2
EspIdf/EspIdfParticipant.h
View File

@ -23,7 +23,7 @@ class ParticipantUDP : public ParticipantUDPGeneric {
void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive(); void Receive();
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg); bool Publish(IMessage* msg);
// bool SendTest(); // bool SendTest();

332
LinearAlgebra/Matrix.cpp
View File

@ -3,12 +3,19 @@
#include <iostream> #include <iostream>
#endif #endif
// #include "esp_log.h"
// #include "esp_system.h"
// #include "freertos/FreeRTOS.h"
// #include "freertos/task.h"
namespace LinearAlgebra { namespace LinearAlgebra {
#pragma region Matrix1 #pragma region Matrix1
Matrix1::Matrix1() {}
Matrix1::Matrix1(int size) : size(size) { Matrix1::Matrix1(int size) : size(size) {
if (this->size == 0) if (this->size <= 0)
data = nullptr; data = nullptr;
else { else {
this->data = new float[size](); this->data = new float[size]();
@ -16,10 +23,63 @@ Matrix1::Matrix1(int size) : size(size) {
} }
} }
Matrix1::Matrix1(float* data, int size) : data(data), size(size) { Matrix1::Matrix1(int size, float* data) : data(data), size(size) {
this->externalData = true; this->externalData = true;
} }
Matrix1::Matrix1(const Matrix1& m) : size(m.size) {
if (this->size <= 0)
this->data = nullptr;
else {
this->data = new float[this->size];
for (int ix = 0; ix < this->size; ++ix)
this->data[ix] = m.data[ix];
}
}
Matrix1::~Matrix1() {
delete[] data;
}
unsigned int Matrix1::Size() const {
return this->size;
}
Matrix1 Matrix1::FromVector3(Vector3 v) {
Matrix1 r(3);
r.data[0] = v.x;
r.data[1] = v.y;
r.data[2] = v.z;
return r;
}
Matrix1 Matrix1::Zero(unsigned int size) {
Matrix1 r(size);
r.Fill(0);
return r;
}
void Matrix1::Fill(float value) {
for (int ix = 0; ix < this->size; ix++)
this->data[ix] = value;
}
void Matrix1::Fill(float value, unsigned int start, unsigned int stop) {
if (start > stop || start > this->size)
return;
if (stop > this->size)
stop = this->size;
for (unsigned int ix = start; ix < stop; ix++)
this->data[ix] = value;
}
Vector3 Matrix1::ToVector3() {
return Vector3(this->data[0], this->data[1], this->data[2]);
}
Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) { Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
Matrix1 r = Matrix1(4); Matrix1 r = Matrix1(4);
float* data = r.data; float* data = r.data;
@ -34,6 +94,40 @@ 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 Matrix1::Slice(int start, int stop) {
if (start < 0 || stop >= this->size)
std::cerr << "Slice index out of range." << std::endl;
Matrix1 result(stop - start);
int resultIx = 0;
for (int ix = start; ix < stop; ix++)
result.data[resultIx++] = this->data[ix];
return result;
}
void Matrix1::UpdateSlice(int start, int stop, const Matrix1& source) const {
if (start > stop || start > this->size || start > source.size)
return;
if (stop > this->size)
stop = this->size;
if (stop > source.size)
stop = source.size;
unsigned int sourceIx = 0;
for (int ix = start; ix < stop; ix++, sourceIx++) {
this->data[ix] = source.data[sourceIx];
}
}
const float& Matrix1::operator()(int ix) const {
return data[ix];
}
float& Matrix1::operator()(int ix) {
return data[ix];
}
// Matrix1 // Matrix1
#pragma endregion #pragma endregion
@ -43,15 +137,19 @@ Matrix2::Matrix2() {}
Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
this->nValues = nRows * nCols; this->nValues = nRows * nCols;
if (this->nValues == 0) // printf("Allocate %d\n", this->nValues);
if (this->nValues <= 0) {
this->data = nullptr; this->data = nullptr;
else { this->externalData = false;
return;
}
this->data = new float[this->nValues]; this->data = new float[this->nValues];
this->externalData = false; this->externalData = false;
} }
}
Matrix2::Matrix2(float* data, int nRows, int nCols) Matrix2::Matrix2(int nRows, int nCols, float* data, float autoDestruct)
: nRows(nRows), nCols(nCols), data(data) { : nRows(nRows), nCols(nCols), data(data) {
this->nValues = nRows * nCols; this->nValues = nRows * nCols;
this->externalData = true; this->externalData = true;
@ -88,39 +186,43 @@ Matrix2& Matrix2::operator=(const Matrix2& m) {
} }
Matrix2::~Matrix2() { Matrix2::~Matrix2() {
if (!this->externalData) if (!this->externalData) {
// printf("Deallocate %d\n", this->nValues);
delete[] data; delete[] data;
} }
}
Matrix2 Matrix2::Clone() const { unsigned int Matrix2::NRows() {
Matrix2 r = Matrix2(this->nRows, this->nCols); return this->nRows;
for (int ix = 0; ix < this->nValues; ++ix) }
r.data[ix] = this->data[ix];
return r; unsigned int Matrix2::NCols() {
return this->nCols;
} }
// Move constructor // Move constructor
Matrix2::Matrix2(Matrix2&& other) noexcept // Matrix2::Matrix2(Matrix2&& other) noexcept
: nRows(other.nRows), // : nRows(other.nRows),
nCols(other.nCols), // nCols(other.nCols),
nValues(other.nValues), // nValues(other.nValues),
data(other.data) { // data(other.data) {
other.data = nullptr; // Set the other object's pointer to nullptr to avoid // other.data = nullptr; // Set the other object's pointer to nullptr to
// double deletion // avoid
} // // double deletion
// }
// Move assignment operator // Move assignment operator
Matrix2& Matrix2::operator=(Matrix2&& other) noexcept { // Matrix2& Matrix2::operator=(Matrix2&& other) noexcept {
if (this != &other) { // if (this != &other) {
delete[] data; // Clean up current data // delete[] data; // Clean up current data
nRows = other.nRows; // nRows = other.nRows;
nCols = other.nCols; // nCols = other.nCols;
nValues = other.nValues; // nValues = other.nValues;
data = other.data; // data = other.data;
other.data = nullptr; // Avoid double deletion // other.data = nullptr; // Avoid double deletion
} // }
return *this; // return *this;
} // }
Matrix2 Matrix2::Zero(int nRows, int nCols) { Matrix2 Matrix2::Zero(int nRows, int nCols) {
Matrix2 r = Matrix2(nRows, nCols); Matrix2 r = Matrix2(nRows, nCols);
@ -129,9 +231,16 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) {
return r; return r;
} }
void Matrix2::Clear() { const float& Matrix2::operator()(int row, int col) const {
return data[row * this->nCols + col];
}
float& Matrix2::operator()(int row, int col) {
return data[row * this->nCols + col];
}
void Matrix2::Fill(float value) {
for (int ix = 0; ix < this->nValues; ix++) for (int ix = 0; ix < this->nValues; ix++)
this->data[ix] = 0; this->data[ix] = value;
} }
Matrix2 Matrix2::Identity(int size) { Matrix2 Matrix2::Identity(int size) {
@ -186,13 +295,30 @@ Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const {
return r; return r;
} }
Matrix2 Matrix2::operator+=(const Matrix2& v) { Matrix2& Matrix2::operator+=(const Matrix2& v) {
for (int ix = 0; ix < this->nValues; ix++) for (int ix = 0; ix < this->nValues; ix++)
this->data[ix] += v.data[ix]; this->data[ix] += v.data[ix];
return *this; return *this;
} }
Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { Matrix2 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;
}
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(rowIx) += m.data[mRowIx + colIx] * v(rowIx);
}
return r;
}
Matrix2 Matrix2::operator*(const Matrix2& B) const {
Matrix2 r = Matrix2(this->nRows, B.nCols); Matrix2 r = Matrix2(this->nRows, B.nCols);
int ACols = this->nCols; int ACols = this->nCols;
@ -221,6 +347,43 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
return r; return r;
} }
Matrix2 Matrix2::GetRows(int from, int to) {
if (from < 0 || to >= this->nRows)
std::cerr << "Slice index out of range." << std::endl;
Matrix2 result = Matrix2(to - from, this->nCols);
int resultRowIx = 0;
for (int rowIx = from; rowIx < to; rowIx++) {
for (int colIx = 0; colIx < this->nCols; colIx++)
result.data[resultRowIx * result.nCols + colIx] =
this->data[rowIx * this->nCols + colIx];
resultRowIx++;
}
return result;
}
Vector3 Matrix2::GetRow3(int rowIx) {
int cellIx = rowIx * this->nCols;
Vector3 row(this->data[cellIx + 0], this->data[cellIx + 1],
this->data[cellIx + 2]);
return row;
}
void Matrix2::SetRow(int rowIx, Matrix1 source) {
int cellIx = rowIx * this->nCols;
for (int ix = 0; ix < source.Size(); ix++)
this->data[cellIx + ix] = source(ix);
}
void Matrix2::SetRow3(int rowIx, Vector3 v) {
int cellIx = rowIx * this->nCols;
this->data[cellIx + 0] = v.x;
this->data[cellIx + 1] = v.y;
this->data[cellIx + 2] = v.z;
}
Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) { Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart); Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
@ -234,22 +397,18 @@ Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
return r; return r;
} }
void Matrix2::UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const {
UpdateSlice(rowStart, rowStop, 0, this->nCols, m);
}
void Matrix2::UpdateSlice(int rowStart, void Matrix2::UpdateSlice(int rowStart,
int rowStop, int rowStop,
int colStart, int colStart,
int colStop, int colStop,
const Matrix2& m) const { 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)];
// }
int rRowDataIx = rowStart * this->nCols;
int mRowDataIx = 0; int mRowDataIx = 0;
for (int rowIx = rowStart; rowIx < rowStop; rowIx++) { for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
rRowDataIx = rowIx * this->nCols; int rRowDataIx = rowIx * this->nCols;
// rRowDataIx += this->nCols;
mRowDataIx += m.nCols; mRowDataIx += m.nCols;
for (int colIx = colStart; colIx < colStop; colIx++) { for (int colIx = colStart; colIx < colStop; colIx++) {
this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)]; this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
@ -257,6 +416,91 @@ void Matrix2::UpdateSlice(int rowStart,
} }
} }
Matrix2 Matrix2::Inverse() {
int n = this->nRows;
// // Create an identity matrix of the same size as the original matrix
Matrix2 augmentedMatrix(n, 2 * n);
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
augmentedMatrix.data[i * this->nCols + j] =
this->data[i * this->nCols + j];
augmentedMatrix.data[i * this->nCols + (j + n)] =
(i == j) ? 1 : 0; // identity matrix;
}
}
// Perform Gaussian elimination
for (int i = 0; i < n; i++) {
// Find the pivot row
float pivot = augmentedMatrix.data[i * augmentedMatrix.nCols + i];
if (abs(pivot) < 1e-10) // Check for singular matrix
std::cerr << "Matrix is singular and cannot be inverted." << std::endl;
// Normalize the pivot row
for (int j = 0; j < 2 * n; j++)
augmentedMatrix.data[i * augmentedMatrix.nCols + j] /= pivot;
// Eliminate the column below the pivot
for (int j = i + 1; j < n; j++) {
float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
for (int k = 0; k < 2 * n; k++)
augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
}
}
// Back substitution
for (int i = n - 1; i >= 0; i--) {
// Eliminate the column above the pivot
for (int j = i - 1; j >= 0; j--) {
float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
for (int k = 0; k < 2 * n; k++)
augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
}
}
// Extract the inverse matrix from the augmented matrix
Matrix2 inverse(n, n);
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++)
inverse.data[i * inverse.nCols + j] =
augmentedMatrix.data[i * augmentedMatrix.nCols + j + n];
}
return inverse;
}
Matrix2 Matrix2::DeleteRows(int rowStart, int rowStop) {
Matrix2 r = Matrix2(this->nRows - (rowStop - rowStart), this->nCols);
int resultRowIx = 0;
for (int i = 0; i < this->nRows; i++) {
if (i >= rowStart && i < rowStop)
continue;
for (int j = 0; j < this->nCols; j++)
r.data[resultRowIx * r.nCols + j] = this->data[i * this->nCols + j];
resultRowIx++;
}
return r;
}
Matrix2 Matrix2::DeleteColumns(int colStart, int colStop) {
Matrix2 r = Matrix2(this->nRows, this->nCols - (colStop - colStart));
for (int i = 0; i < this->nRows; i++) {
int resultColIx = 0;
for (int j = 0; j < this->nCols; j++) {
if (j >= colStart && j < colStop)
continue;
r.data[i * r.nCols + resultColIx++] = this->data[i * this->nCols + j];
}
}
return r;
}
/// @brief Compute the Omega matrix of a 3D vector /// @brief Compute the Omega matrix of a 3D vector
/// @param v The vector /// @param v The vector
/// @return 4x4 Omega matrix /// @return 4x4 Omega matrix

98
LinearAlgebra/Matrix.h
View File

@ -9,15 +9,36 @@ namespace LinearAlgebra {
/// @brief A 1-dimensional matrix or vector of arbitrary size /// @brief A 1-dimensional matrix or vector of arbitrary size
class Matrix1 { class Matrix1 {
public: public:
float* data = nullptr; Matrix1();
int size = 0;
Matrix1(int size); Matrix1(int size);
Matrix1(float* data, int size); Matrix1(int size, float* data);
Matrix1(const Matrix1& m); // Make a clone
~Matrix1();
unsigned int Size() const;
static Matrix1 Zero(unsigned int size);
void Fill(float value);
void Fill(float value, unsigned int start, unsigned int stop);
static Matrix1 FromVector3(Vector3 v);
Vector3 ToVector3();
static Matrix1 FromQuaternion(Quaternion q); static Matrix1 FromQuaternion(Quaternion q);
Quaternion ToQuaternion(); Quaternion ToQuaternion();
const float& operator()(int ix) const;
float& operator()(int ix);
Matrix1 Slice(int start, int stop);
void UpdateSlice(int start, int stop, const Matrix1& m) const;
protected:
float* data = nullptr;
int size = 0;
private: private:
bool externalData = true; bool externalData = true;
}; };
@ -25,31 +46,47 @@ class Matrix1 {
/// @brief A 2-dimensional matrix of arbitrary size /// @brief A 2-dimensional matrix of arbitrary size
class Matrix2 { class Matrix2 {
public: public:
int nRows = 0;
int nCols = 0;
int nValues = 0;
float* data = nullptr;
Matrix2(); Matrix2();
Matrix2(int nRows, int nCols); Matrix2(int nRows, int nCols);
Matrix2(float* data, int nRows, int nCols); Matrix2(int nRows, int nCols, float* data, float autoDestruct = false);
Matrix2(const Matrix2& m); Matrix2(const Matrix2& m);
Matrix2& operator=(const Matrix2& other); Matrix2& operator=(const Matrix2& other);
~Matrix2(); ~Matrix2();
Matrix2 Clone() const; unsigned int NRows();
unsigned int NCols();
static Matrix2 Zero(int nRows, int nCols); static Matrix2 Zero(int nRows, int nCols);
void Clear(); void Fill(float value);
static Matrix2 Identity(int size); static Matrix2 Identity(int size);
static Matrix2 Diagonal(float f, int size); static Matrix2 Diagonal(float f, int size);
static Matrix2 SkewMatrix(const Vector3& v); static Matrix2 SkewMatrix(const Vector3& v);
Matrix2 Transpose() const; Matrix2 Transpose() const;
Matrix2 Inverse();
const float& operator()(int row, int col) const;
float& operator()(int row, int col);
Matrix2 GetRows(int from, int to);
Vector3 GetRow3(int rowIx);
void SetRow(int rowIx, Matrix1 source);
void SetRow3(int rowIx, Vector3 v);
Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
void UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const;
void UpdateSlice(int rowStart,
int rowStop,
int colStart,
int colStop,
const Matrix2& m) const;
Matrix2 DeleteRows(int rowStart, int rowStop);
Matrix2 DeleteColumns(int colStart, int colStop);
Matrix2 operator-() const; Matrix2 operator-() const;
@ -57,7 +94,9 @@ class Matrix2 {
/// @param m The matrix to add to this matrix /// @param m The matrix to add to this matrix
/// @return The result of the addition /// @return The result of the addition
Matrix2 operator+(const Matrix2& v) const; Matrix2 operator+(const Matrix2& v) const;
Matrix2 operator+=(const Matrix2& v); Matrix2& operator+=(const Matrix2& v);
Matrix2 operator-(const Matrix2& v) const;
Matrix2 operator*(const Matrix2& m) const; Matrix2 operator*(const Matrix2& m) const;
friend Matrix2 operator*(const Matrix2& m, float f) { friend Matrix2 operator*(const Matrix2& m, float f) {
@ -73,15 +112,7 @@ class Matrix2 {
return r; return r;
} }
friend Matrix1 operator*(const Matrix2& m, const Matrix1& v) { 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/(const Matrix2& m, float f) { friend Matrix2 operator/(const Matrix2& m, float f) {
Matrix2 r = Matrix2(m.nRows, m.nCols); Matrix2 r = Matrix2(m.nRows, m.nCols);
@ -96,20 +127,19 @@ class Matrix2 {
return r; return r;
} }
Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
void UpdateSlice(int rowStart,
int rowStop,
int colStart,
int colStop,
const Matrix2& m) const;
// private:
// move constructor and move assignment operator // move constructor and move assignment operator
Matrix2(Matrix2&& other) noexcept; // Matrix2(Matrix2&& other) noexcept;
Matrix2& operator=(Matrix2&& other) noexcept; // Matrix2& operator=(Matrix2&& other) noexcept;
static Matrix2 Omega(const Vector3& v); static Matrix2 Omega(const Vector3& v);
public:
int nRows = 0;
int nCols = 0;
protected:
int nValues = 0;
float* data = nullptr;
private: private:
bool externalData = true; bool externalData = true;
}; };

10
LinearAlgebra/Quaternion.cpp
View File

@ -99,14 +99,12 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) {
} }
Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() { Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
Matrix2 r = Matrix2(3, 3);
float x = this->x; float x = this->x;
float y = this->y; float y = this->y;
float z = this->z; float z = this->z;
float w = this->w; float w = this->w;
float* data = r.data; float* data = new float[9]; // r.data;
data[0 * 3 + 0] = 1 - 2 * (y * y + z * z); data[0 * 3 + 0] = 1 - 2 * (y * y + z * z);
data[0 * 3 + 1] = 2 * (x * y - w * z); data[0 * 3 + 1] = 2 * (x * y - w * z);
data[0 * 3 + 2] = 2 * (x * z + w * y); data[0 * 3 + 2] = 2 * (x * z + w * y);
@ -117,6 +115,8 @@ Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
data[2 * 3 + 1] = 2 * (y * z + w * x); data[2 * 3 + 1] = 2 * (y * z + w * x);
data[2 * 3 + 2] = 1 - 2 * (x * x + y * y); data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
Matrix2 r = Matrix2(3, 3, data, true);
return r; return r;
} }
@ -168,6 +168,10 @@ Quaternion Quaternion::Inverse(Quaternion r) {
return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n); return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n);
} }
Quaternion Quaternion::Reflect(Quaternion q) {
return Quaternion(-q.x, -q.y, -q.z, q.w);
}
Quaternion Quaternion::LookRotation(const Vector3& forward) { Quaternion Quaternion::LookRotation(const Vector3& forward) {
Vector3 up = Vector3(0, 1, 0); Vector3 up = Vector3(0, 1, 0);
return LookRotation(forward, up); return LookRotation(forward, up);

2
LinearAlgebra/Quaternion.h
View File

@ -126,6 +126,8 @@ struct Quaternion : Quat {
/// needed</param> <returns>The inverted quaternion</returns> /// needed</param> <returns>The inverted quaternion</returns>
static Quaternion Inverse(Quaternion quaternion); static Quaternion Inverse(Quaternion quaternion);
static Quaternion Reflect(Quaternion q);
/// <summary> /// <summary>
/// A rotation which looks in the given direction /// A rotation which looks in the given direction
/// </summary> /// </summary>

10
LinearAlgebra/Vector2.cpp
View File

@ -180,3 +180,13 @@ Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) {
Vector2 v = v1 + (v2 - v1) * f; Vector2 v = v1 + (v2 - v1) * f;
return v; return v;
} }
#pragma region Vector2Of
template <typename T>
Vector2Of<T>::Vector2Of() {
this->horizontal = 0;
this->vertical = 0;
}
#pragma endregion Vector2Of

11
LinearAlgebra/Vector2.h
View File

@ -201,6 +201,17 @@ struct Vector2 : Vec2 {
static Vector2 Lerp(const Vector2& v1, const Vector2& v2, float f); static Vector2 Lerp(const Vector2& v1, const Vector2& v2, float f);
}; };
template <typename T>
struct Vector2Of {
public:
Vector2Of();
T horizontal = 0;
T vertical = 0;
};
using Vector2Int = Vector2Of<int>;
} // namespace LinearAlgebra } // namespace LinearAlgebra
using namespace LinearAlgebra; using namespace LinearAlgebra;

340
LinearAlgebra/test/Matrix_test.cc
View File

@ -1,215 +1,215 @@
#if GTEST // #if GTEST
#include <gtest/gtest.h> // #include <gtest/gtest.h>
#include <math.h> // #include <math.h>
#include <limits> // #include <limits>
#include "Matrix.h" // #include "Matrix.h"
TEST(Matrix2, Zero) { // TEST(Matrix2, Zero) {
// Test case 1: 2x2 zero matrix // // Test case 1: 2x2 zero matrix
Matrix2 zeroMatrix = Matrix2::Zero(2, 2); // Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
EXPECT_TRUE(zeroMatrix.nRows == 2); // EXPECT_TRUE(zeroMatrix.nRows == 2);
EXPECT_TRUE(zeroMatrix.nCols == 2); // EXPECT_TRUE(zeroMatrix.nCols == 2);
for (int i = 0; i < zeroMatrix.nValues; ++i) { // for (int i = 0; i < zeroMatrix.nValues; ++i) {
EXPECT_TRUE(zeroMatrix.data[i] == 0.0f); // EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
} // }
std::cout << "Test case 1 passed: 2x2 zero matrix\n"; // std::cout << "Test case 1 passed: 2x2 zero matrix\n";
// Test case 2: 3x3 zero matrix // // Test case 2: 3x3 zero matrix
zeroMatrix = Matrix2::Zero(3, 3); // zeroMatrix = Matrix2::Zero(3, 3);
EXPECT_TRUE(zeroMatrix.nRows == 3); // EXPECT_TRUE(zeroMatrix.nRows == 3);
EXPECT_TRUE(zeroMatrix.nCols == 3); // EXPECT_TRUE(zeroMatrix.nCols == 3);
for (int i = 0; i < zeroMatrix.nValues; ++i) { // for (int i = 0; i < zeroMatrix.nValues; ++i) {
EXPECT_TRUE(zeroMatrix.data[i] == 0.0f); // EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
} // }
std::cout << "Test case 2 passed: 3x3 zero matrix\n"; // std::cout << "Test case 2 passed: 3x3 zero matrix\n";
// Test case 3: 1x1 zero matrix // // Test case 3: 1x1 zero matrix
zeroMatrix = Matrix2::Zero(1, 1); // zeroMatrix = Matrix2::Zero(1, 1);
EXPECT_TRUE(zeroMatrix.nRows == 1); // EXPECT_TRUE(zeroMatrix.nRows == 1);
EXPECT_TRUE(zeroMatrix.nCols == 1); // EXPECT_TRUE(zeroMatrix.nCols == 1);
EXPECT_TRUE(zeroMatrix.data[0] == 0.0f); // EXPECT_TRUE(zeroMatrix.data[0] == 0.0f);
std::cout << "Test case 3 passed: 1x1 zero matrix\n"; // std::cout << "Test case 3 passed: 1x1 zero matrix\n";
// Test case 4: 0x0 matrix (edge case) // // Test case 4: 0x0 matrix (edge case)
zeroMatrix = Matrix2::Zero(0, 0); // zeroMatrix = Matrix2::Zero(0, 0);
EXPECT_TRUE(zeroMatrix.nRows == 0); // EXPECT_TRUE(zeroMatrix.nRows == 0);
EXPECT_TRUE(zeroMatrix.nCols == 0); // EXPECT_TRUE(zeroMatrix.nCols == 0);
EXPECT_TRUE(zeroMatrix.data == nullptr); // EXPECT_TRUE(zeroMatrix.data == nullptr);
std::cout << "Test case 4 passed: 0x0 matrix\n"; // std::cout << "Test case 4 passed: 0x0 matrix\n";
} // }
TEST(Matrix2, Multiplication) { // TEST(Matrix2, Multiplication) {
// Test 1: Multiplying two 2x2 matrices // // Test 1: Multiplying two 2x2 matrices
float dataA[] = {1, 2, 3, 4}; // float dataA[] = {1, 2, 3, 4};
float dataB[] = {5, 6, 7, 8}; // float dataB[] = {5, 6, 7, 8};
Matrix2 A(dataA, 2, 2); // Matrix2 A(dataA, 2, 2);
Matrix2 B(dataB, 2, 2); // Matrix2 B(dataB, 2, 2);
Matrix2 result = A * B; // Matrix2 result = A * B;
float expectedData[] = {19, 22, 43, 50}; // float expectedData[] = {19, 22, 43, 50};
for (int i = 0; i < 4; ++i) // for (int i = 0; i < 4; ++i)
EXPECT_TRUE(result.data[i] == expectedData[i]); // EXPECT_TRUE(result.data[i] == expectedData[i]);
std::cout << "Test 1 passed: 2x2 matrix multiplication.\n"; // std::cout << "Test 1 passed: 2x2 matrix multiplication.\n";
// Test 2: Multiplying a 3x2 matrix with a 2x3 matrix // // Test 2: Multiplying a 3x2 matrix with a 2x3 matrix
float dataC[] = {1, 2, 3, 4, 5, 6}; // float dataC[] = {1, 2, 3, 4, 5, 6};
float dataD[] = {7, 8, 9, 10, 11, 12}; // float dataD[] = {7, 8, 9, 10, 11, 12};
Matrix2 C(dataC, 3, 2); // Matrix2 C(dataC, 3, 2);
Matrix2 D(dataD, 2, 3); // Matrix2 D(dataD, 2, 3);
Matrix2 result2 = C * D; // Matrix2 result2 = C * D;
float expectedData2[] = {27, 30, 33, 61, 68, 75, 95, 106, 117}; // float expectedData2[] = {27, 30, 33, 61, 68, 75, 95, 106, 117};
for (int i = 0; i < 9; ++i) // for (int i = 0; i < 9; ++i)
EXPECT_TRUE(result2.data[i] == expectedData2[i]); // EXPECT_TRUE(result2.data[i] == expectedData2[i]);
std::cout << "Test 2 passed: 3x2 * 2x3 matrix multiplication.\n"; // std::cout << "Test 2 passed: 3x2 * 2x3 matrix multiplication.\n";
// Test 3: Multiplying with a zero matrix // // Test 3: Multiplying with a zero matrix
Matrix2 zeroMatrix = Matrix2::Zero(2, 2); // Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
Matrix2 result3 = A * zeroMatrix; // Matrix2 result3 = A * zeroMatrix;
for (int i = 0; i < 4; ++i) // for (int i = 0; i < 4; ++i)
EXPECT_TRUE(result3.data[i] == 0); // EXPECT_TRUE(result3.data[i] == 0);
std::cout << "Test 3 passed: Multiplication with zero matrix.\n"; // std::cout << "Test 3 passed: Multiplication with zero matrix.\n";
// Test 4: Multiplying with an identity matrix // // Test 4: Multiplying with an identity matrix
Matrix2 identityMatrix = Matrix2::Identity(2); // Matrix2 identityMatrix = Matrix2::Identity(2);
Matrix2 result4 = A * identityMatrix; // Matrix2 result4 = A * identityMatrix;
for (int i = 0; i < 4; ++i) // for (int i = 0; i < 4; ++i)
EXPECT_TRUE(result4.data[i] == A.data[i]); // EXPECT_TRUE(result4.data[i] == A.data[i]);
std::cout << "Test 4 passed: Multiplication with identity matrix.\n"; // std::cout << "Test 4 passed: Multiplication with identity matrix.\n";
} // }
TEST(MatrixSingle, Init) { // TEST(MatrixSingle, Init) {
// zero // // zero
MatrixOf<float> m0 = MatrixOf<float>(0, 0); // MatrixOf<float> m0 = MatrixOf<float>(0, 0);
// one // // one
float data1[] = {1.0F}; // float data1[] = {1.0F};
MatrixOf<float> m1 = MatrixOf<float>(1, 1, data1); // MatrixOf<float> m1 = MatrixOf<float>(1, 1, data1);
// two // // two
float data2[] = {1.0F, 2.0F, 3.0F, 4.0F}; // float data2[] = {1.0F, 2.0F, 3.0F, 4.0F};
MatrixOf<float> m2 = MatrixOf<float>(2, 2, data2); // MatrixOf<float> m2 = MatrixOf<float>(2, 2, data2);
// negative // // negative
// MatrixOf<float> m_1 = MatrixOf<float>(-1, -1); // // MatrixOf<float> m_1 = MatrixOf<float>(-1, -1);
// parameters are unsigned // // parameters are unsigned
} // }
TEST(MatrixSingle, Transpose) { // TEST(MatrixSingle, Transpose) {
float data1[] = {1.0F}; // float data1[] = {1.0F};
MatrixOf<float> m = MatrixOf<float>(1, 1, data1); // MatrixOf<float> m = MatrixOf<float>(1, 1, data1);
MatrixOf<float> r = MatrixOf<float>(1, 1); // MatrixOf<float> r = MatrixOf<float>(1, 1);
m.Transpose(&r); // m.Transpose(&r);
// 2 x 2 // // 2 x 2
float data3[] = {1.0F, 2.0F, 3.0F, 4.0F}; // float data3[] = {1.0F, 2.0F, 3.0F, 4.0F};
MatrixOf<float> m22 = MatrixOf<float>(2, 2, data3); // MatrixOf<float> m22 = MatrixOf<float>(2, 2, data3);
EXPECT_EQ(m22.RowCount(), 2); // EXPECT_EQ(m22.RowCount(), 2);
EXPECT_EQ(m22.ColCount(), 2); // EXPECT_EQ(m22.ColCount(), 2);
float data4[] = {0.0F, 0.0F, 0.0F, 0.0F}; // float data4[] = {0.0F, 0.0F, 0.0F, 0.0F};
MatrixOf<float> r22 = MatrixOf<float>(2, 2, data4); // MatrixOf<float> r22 = MatrixOf<float>(2, 2, data4);
EXPECT_EQ(r22.RowCount(), 2); // EXPECT_EQ(r22.RowCount(), 2);
EXPECT_EQ(r22.ColCount(), 2); // EXPECT_EQ(r22.ColCount(), 2);
m22.Transpose(&r22); // m22.Transpose(&r22);
EXPECT_EQ(r22.RowCount(), 2); // EXPECT_EQ(r22.RowCount(), 2);
EXPECT_EQ(r22.ColCount(), 2); // EXPECT_EQ(r22.ColCount(), 2);
EXPECT_FLOAT_EQ(r22.Get(0, 0), 1.0F); // EXPECT_FLOAT_EQ(r22.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(r22.Get(0, 1), 3.0F); // EXPECT_FLOAT_EQ(r22.Get(0, 1), 3.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 0), 2.0F); // EXPECT_FLOAT_EQ(r22.Get(1, 0), 2.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 1), 4.0F); // EXPECT_FLOAT_EQ(r22.Get(1, 1), 4.0F);
// 1 x 2 // // 1 x 2
float data12[] = {1.0F, 2.0F}; // float data12[] = {1.0F, 2.0F};
MatrixOf<float> m12 = MatrixOf<float>(1, 2, data12); // MatrixOf<float> m12 = MatrixOf<float>(1, 2, data12);
EXPECT_EQ(m12.RowCount(), 1); // EXPECT_EQ(m12.RowCount(), 1);
EXPECT_EQ(m12.ColCount(), 2); // EXPECT_EQ(m12.ColCount(), 2);
float data21[] = {0.0F, 0.0F}; // float data21[] = {0.0F, 0.0F};
MatrixOf<float> r21 = MatrixOf<float>(2, 1, data21); // MatrixOf<float> r21 = MatrixOf<float>(2, 1, data21);
EXPECT_EQ(r21.RowCount(), 2); // EXPECT_EQ(r21.RowCount(), 2);
EXPECT_EQ(r21.ColCount(), 1); // EXPECT_EQ(r21.ColCount(), 1);
m12.Transpose(&r21); // m12.Transpose(&r21);
EXPECT_EQ(r21.RowCount(), 2); // EXPECT_EQ(r21.RowCount(), 2);
EXPECT_EQ(r21.ColCount(), 1); // EXPECT_EQ(r21.ColCount(), 1);
EXPECT_FLOAT_EQ(r21.Get(0, 0), 1.0F); // EXPECT_FLOAT_EQ(r21.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(r21.Get(1, 0), 2.0F); // EXPECT_FLOAT_EQ(r21.Get(1, 0), 2.0F);
// changing dimensions, same size is okay // // changing dimensions, same size is okay
MatrixOf<float> r12 = MatrixOf<float>(1, 2, data21); // MatrixOf<float> r12 = MatrixOf<float>(1, 2, data21);
EXPECT_EQ(r12.RowCount(), 1); // EXPECT_EQ(r12.RowCount(), 1);
EXPECT_EQ(r12.ColCount(), 2); // EXPECT_EQ(r12.ColCount(), 2);
m12.Transpose(&r12); // m12.Transpose(&r12);
EXPECT_EQ(r12.RowCount(), 2); // EXPECT_EQ(r12.RowCount(), 2);
EXPECT_EQ(r12.ColCount(), 1); // EXPECT_EQ(r12.ColCount(), 1);
EXPECT_FLOAT_EQ(r12.Get(0, 0), 1.0F); // EXPECT_FLOAT_EQ(r12.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(r12.Get(0, 1), 2.0F); // EXPECT_FLOAT_EQ(r12.Get(0, 1), 2.0F);
} // }
TEST(MatrixSingle, Multiply) { // TEST(MatrixSingle, Multiply) {
float m12data[] = {1.0F, 2.0F}; // float m12data[] = {1.0F, 2.0F};
MatrixOf<float> m12 = MatrixOf<float>(1, 2, m12data); // MatrixOf<float> m12 = MatrixOf<float>(1, 2, m12data);
EXPECT_EQ(m12.RowCount(), 1); // EXPECT_EQ(m12.RowCount(), 1);
EXPECT_EQ(m12.ColCount(), 2); // EXPECT_EQ(m12.ColCount(), 2);
EXPECT_FLOAT_EQ(m12.Get(0, 0), 1.0F); // EXPECT_FLOAT_EQ(m12.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(m12.Get(0, 1), 2.0F); // EXPECT_FLOAT_EQ(m12.Get(0, 1), 2.0F);
float m21data[] = {3.0F, 4.0F}; // float m21data[] = {3.0F, 4.0F};
MatrixOf<float> m21 = MatrixOf<float>(2, 1, m21data); // MatrixOf<float> m21 = MatrixOf<float>(2, 1, m21data);
EXPECT_EQ(m21.RowCount(), 2); // EXPECT_EQ(m21.RowCount(), 2);
EXPECT_EQ(m21.ColCount(), 1); // EXPECT_EQ(m21.ColCount(), 1);
EXPECT_FLOAT_EQ(m21.Get(0, 0), 3.0F); // EXPECT_FLOAT_EQ(m21.Get(0, 0), 3.0F);
EXPECT_FLOAT_EQ(m21.Get(1, 0), 4.0F); // EXPECT_FLOAT_EQ(m21.Get(1, 0), 4.0F);
float r11data[] = {0.0F}; // float r11data[] = {0.0F};
MatrixOf<float> r11 = MatrixOf<float>(1, 1, r11data); // MatrixOf<float> r11 = MatrixOf<float>(1, 1, r11data);
EXPECT_EQ(r11.RowCount(), 1); // EXPECT_EQ(r11.RowCount(), 1);
EXPECT_EQ(r11.ColCount(), 1); // EXPECT_EQ(r11.ColCount(), 1);
MatrixOf<float>::Multiply(&m12, &m21, &r11); // MatrixOf<float>::Multiply(&m12, &m21, &r11);
EXPECT_EQ(r11.RowCount(), 1); // EXPECT_EQ(r11.RowCount(), 1);
EXPECT_EQ(r11.ColCount(), 1); // EXPECT_EQ(r11.ColCount(), 1);
EXPECT_FLOAT_EQ(r11.Get(0, 0), 11.0F); // EXPECT_FLOAT_EQ(r11.Get(0, 0), 11.0F);
float r22data[] = {0.0F, 0.0F, 0.0F, 0.0F}; // float r22data[] = {0.0F, 0.0F, 0.0F, 0.0F};
MatrixOf<float> r22 = MatrixOf<float>(2, 2, r22data); // MatrixOf<float> r22 = MatrixOf<float>(2, 2, r22data);
MatrixOf<float>::Multiply(&m21, &m12, &r22); // MatrixOf<float>::Multiply(&m21, &m12, &r22);
EXPECT_EQ(r22.RowCount(), 2); // EXPECT_EQ(r22.RowCount(), 2);
EXPECT_EQ(r22.ColCount(), 2); // EXPECT_EQ(r22.ColCount(), 2);
EXPECT_FLOAT_EQ(r22.Get(0, 0), 3.0F); // EXPECT_FLOAT_EQ(r22.Get(0, 0), 3.0F);
EXPECT_FLOAT_EQ(r22.Get(0, 1), 4.0F); // EXPECT_FLOAT_EQ(r22.Get(0, 1), 4.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 0), 6.0F); // EXPECT_FLOAT_EQ(r22.Get(1, 0), 6.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 1), 8.0F); // EXPECT_FLOAT_EQ(r22.Get(1, 1), 8.0F);
} // }
TEST(MatrixSingle, Multiply_Vector3) { // TEST(MatrixSingle, Multiply_Vector3) {
Vector3 v = Vector3(1.0, 2.0, 3.0); // Vector3 v = Vector3(1.0, 2.0, 3.0);
Vector3 r = Vector3::zero; // Vector3 r = Vector3::zero;
// float m13data[] = {3.0, 4.0, 5.0}; // // float m13data[] = {3.0, 4.0, 5.0};
// MatrixOf<float> m13 = MatrixOf<float>(1, 3, m13data); // // MatrixOf<float> m13 = MatrixOf<float>(1, 3, m13data);
// Vector3 r = MatrixOf<float>::Multiply(&m13, v); // // Vector3 r = MatrixOf<float>::Multiply(&m13, v);
float m33data[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; // float m33data[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
MatrixOf<float> m33 = MatrixOf<float>(3, 3, m33data); // MatrixOf<float> m33 = MatrixOf<float>(3, 3, m33data);
r = MatrixOf<float>::Multiply(&m33, v); // r = MatrixOf<float>::Multiply(&m33, v);
EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(1.0f, 2.0f, 3.0f)), 0); // EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(1.0f, 2.0f, 3.0f)), 0);
} // }
#endif // #endif

158
Participant.cpp
View File

@ -19,34 +19,9 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
Participant::Participant() { Participant::Participant() {
Thing::CreateRoot(this); Thing::CreateRoot(this);
//this->Add(this->root);
} }
/* Participant::~Participant() {}
Participant::Participant(const char* ipAddress, int port) {
Thing::CreateRoot(this);
//this->Add(this->root);
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64)
strncpy_s(addressString, stringLength, ipAddress,
addressLength); // Leave space for null terminator
#else
strncpy(addressString, ipAddress, addressLength);
#endif
addressString[addressLength] = '\0';
this->ipAddress = addressString;
this->port = port;
}
*/
Participant::~Participant() {
// registry.Remove(this);
// delete[] this->ipAddress;
}
void Participant::Update(bool recurse) { void Participant::Update(bool recurse) {
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
@ -57,47 +32,19 @@ void Participant::Update(bool recurse) {
bool Participant::Send(IMessage* msg) { bool Participant::Send(IMessage* msg) {
std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff) std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff)
<< " to base Participant without communcation support " << std::endl; << " to base Participant without communcation support "
<< std::endl;
return true; return true;
} }
/*
bool Participant::Send(IMessage* msg) {
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
return true;
// std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
// << " to " << this->ipAddress << std::endl;
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->SendTo(this, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->SendTo(this, bufferSize);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
return thisArduino->SendTo(this, bufferSize);
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
return thisEspIdf->SendTo(this, bufferSize);
#else
return false;
#endif
}
*/
Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
if (thing->owner->networkId == networkId && thing->id == thingId) if (thing->owner->networkId == networkId && thing->id == thingId)
return thing; return thing;
} }
std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port std::cout << "Could not find thing " << "[" << (int)networkId << ": "
<< "[" << (int)thingId << "]\n"; << (int)thingId << "]\n";
return nullptr; return nullptr;
} }
@ -120,8 +67,7 @@ void Participant::Add(Thing* thing, bool checkId) {
this->things.push_back(thing); this->things.push_back(thing);
#endif #endif
std::cout << "Add thing with generated ID " std::cout << "Add thing with generated ID "
//<< this->ipAddress << ":" << this->port << "[" << (int)networkId << ": " << (int)thing->id << "]\n";
<< "[" << (int)thing->id << "]\n";
} else { } else {
Thing* foundThing = Get(thing->owner->networkId, thing->id); Thing* foundThing = Get(thing->owner->networkId, thing->id);
if (foundThing == nullptr) { if (foundThing == nullptr) {
@ -130,13 +76,11 @@ void Participant::Add(Thing* thing, bool checkId) {
#else #else
this->things.push_back(thing); this->things.push_back(thing);
#endif #endif
std::cout << "Add thing " << //this->ipAddress << ":" << this->port << std::cout << "Add thing [" << (int)networkId << ": " << (int)thing->id
"[" << "]\n";
<< (int)thing->id << "]\n";
} else { } else {
std::cout << "Did not add, existing thing " std::cout << "Did not add, existing thing "
//<< this->ipAddress << ":" << this->port << "[" << (int)networkId << ": " << (int)thing->id << "]\n";
<< "[" << (int)thing->id << "]\n";
} }
} }
} }
@ -164,88 +108,4 @@ void Participant::Remove(Thing* thing) {
#pragma endregion #pragma endregion
// #pragma region ParticipantRegistry
// /*
// Participant* ParticipantRegistry::Get(const char* ipAddress,
// unsigned int port) {
// #if !defined(NO_STD)
// for (Participant* participant : ParticipantRegistry::participants) {
// if (participant == nullptr)
// continue;
// if (strcmp(participant->ipAddress, ipAddress) == 0 &&
// participant->port == port) {
// // std::cout << "found participant " << participant->ipAddress << ":"
// // << (int)participant->port << std::endl;
// return participant;
// }
// }
// std::cout << "Could not find participant " << ipAddress << ":" << (int)port
// << std::endl;
// #endif
// return nullptr;
// }
// */
// Participant* ParticipantRegistry::Get(unsigned char participantId) {
// #if !defined(NO_STD)
// for (Participant* participant : ParticipantRegistry::participants) {
// if (participant == nullptr)
// continue;
// if (participant->networkId == participantId)
// return participant;
// }
// std::cout << "Could not find participant " << (int)participantId << std::endl;
// #endif
// return nullptr;
// }
// // Participant* ParticipantRegistry::Add(const char* ipAddress,
// // unsigned int port) {
// // Participant* participant = new Participant(ipAddress, port);
// // Add(participant);
// // return participant;
// // }
// void ParticipantRegistry::Add(Participant* participant) {
// Participant* foundParticipant =
// Get(participant->networkId);
// //Get(participant->ipAddress, participant->port);
// if (foundParticipant == nullptr) {
// #if defined(NO_STD)
// // this->things[this->thingCount++] = thing;
// #else
// ParticipantRegistry::participants.push_back(participant);
// #endif
// // std::cout << "Add participant " << participant->ipAddress << ":"
// // << participant->port << "[" << (int)participant->networkId
// // << "]\n";
// // std::cout << "participants " <<
// // ParticipantRegistry::participants.size()
// // << "\n";
// // } else {
// // std::cout << "Did not add, existing participant " <<
// // participant->ipAddress
// // << ":" << participant->port << "[" <<
// // (int)participant->networkId
// // << "]\n";
// }
// }
// void ParticipantRegistry::Remove(Participant* participant) {
// // participants.remove(participant);
// }
// #if defined(NO_STD)
// Participant** ParticipantRegistry::GetAll() const {
// return ParticipantRegistry::participants;
// }
// #else
// const std::list<Participant*>& ParticipantRegistry::GetAll() const {
// return ParticipantRegistry::participants;
// }
// #endif
// #pragma endregion ParticipantRegistry
} // namespace RoboidControl } // namespace RoboidControl

55
Participant.h
View File

@ -7,55 +7,6 @@ namespace RoboidControl {
constexpr int MAX_THING_COUNT = 256; constexpr int MAX_THING_COUNT = 256;
/*
/// @brief class which manages all known participants
class ParticipantRegistry {
public:
/// @brief Retrieve a participant by its address
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found
//Participant* Get(const char* ipAddress, unsigned int port);
/// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found
Participant* Get(unsigned char networkID);
/// @brief Add a participant with the given details
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The added participant
//Participant* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(Participant* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(Participant* participant);
private:
#if defined(NO_STD)
public:
Participant** GetAll() const;
int count = 0;
private:
Participant** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<Participant*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<Participant*> participants;
#endif
};
*/
/// @brief A participant is a device which manages things. /// @brief A participant is a device which manages things.
/// It can communicate with other participant to synchronise the state of /// It can communicate with other participant to synchronise the state of
/// things. This class is used to register the things the participant is /// things. This class is used to register the things the participant is
@ -137,12 +88,6 @@ class Participant {
#pragma endregion Send #pragma endregion Send
// #pragma region Participant Registry
// public:
// static ParticipantRegistry registry;
// #pragma endregion Participant Registry
}; };
} // namespace RoboidControl } // namespace RoboidControl

187
Participants/ParticipantMQTT.cpp
View File

@ -13,9 +13,9 @@
namespace RoboidControl { namespace RoboidControl {
MQTTParticipantBase::MQTTParticipantBase(const char* ipAddress, int port) MQTTParticipantBase::MQTTParticipantBase(const char* ipAddress, int port)
: RemoteParticipantUDP("127.0.0.1", port) { : ParticipantSocket("127.0.0.1", port) {
this->name = "ParticipantUDP"; this->name = "ParticipantUDP";
this->remoteSite = new RemoteParticipantUDP(ipAddress, port); this->remoteSite = new ParticipantSocket(ipAddress, port);
// Participant::registry.Add(this); // Participant::registry.Add(this);
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
@ -33,11 +33,120 @@ void MQTTParticipantBase::SetupTCP() {
#define MQTT_CONNECT 0x10 #define MQTT_CONNECT 0x10
#define MQTT_CONNECT_ACK 0x20 #define MQTT_CONNECT_ACK 0x20
void MQTTParticipantBase::send_mqtt_connect(const char* client_id) { // MQTT CONNECT packet construction
std::vector<uint8_t> createConnectPacket(const std::string& clientId) {
std::vector<uint8_t> packet;
// Fixed header
packet.push_back(0x10); // CONNECT packet type
// Remaining length calculation
int remainingLength =
2 + 4 + // Protocol name length and name (MQTT)
1 + // Protocol level
1 + // Connect flags
2 + // Keep alive
2 + clientId.length(); // Client ID
// Encode remaining length
do {
uint8_t encodedByte = remainingLength % 128;
remainingLength /= 128;
if (remainingLength > 0) {
encodedByte |= 128;
}
packet.push_back(encodedByte);
} while (remainingLength > 0);
// Protocol name (MQTT)
packet.push_back(0x00);
packet.push_back(0x04);
packet.push_back('M');
packet.push_back('Q');
packet.push_back('T');
packet.push_back('T');
// Protocol level (v3.1.1)
packet.push_back(0x04);
// Connect flags
packet.push_back(0x02); // Clean session
// Keep alive (10 seconds)
packet.push_back(0x00);
packet.push_back(0x0A);
// Client ID
packet.push_back(0x00);
packet.push_back(static_cast<uint8_t>(clientId.length()));
packet.insert(packet.end(), clientId.begin(), clientId.end());
return packet;
}
void MQTTParticipantBase::send_mqtt_connect(const char* clientId) {
std::vector<uint8_t> packet;
// Fixed header
packet.push_back(0x10); // CONNECT packet type
// Calculate client ID length
size_t clientIdLength = strlen(clientId);
// Remaining length calculation
int remainingLength =
2 + 4 + // Protocol name length and name (MQTT)
1 + // Protocol level
1 + // Connect flags
2 + // Keep alive
2 + clientIdLength; // Client ID
// Encode remaining length
do {
uint8_t encodedByte = remainingLength % 128;
remainingLength /= 128;
if (remainingLength > 0) {
encodedByte |= 128;
}
packet.push_back(encodedByte);
} while (remainingLength > 0);
// Protocol name (MQTT)
packet.push_back(0x00);
packet.push_back(0x04);
packet.push_back('M');
packet.push_back('Q');
packet.push_back('T');
packet.push_back('T');
// Protocol level (v3.1.1)
packet.push_back(0x04);
// Connect flags
packet.push_back(0x02); // Clean session
// Keep alive (10 seconds)
packet.push_back(0x00);
packet.push_back(0x0A);
// Client ID
packet.push_back(0x00);
packet.push_back(static_cast<uint8_t>(clientIdLength));
packet.insert(packet.end(), clientId, clientId + clientIdLength);
for (int i = 0; i < packet.size(); i++)
this->buffer[i] = packet.data()[i];
int buffersize = packet.size();
SendTCP(buffersize);
//send(sock, (const char*)connectPacket.data(), connectPacket.size(), 0);
/*
MQTTConnectPacket packet; MQTTConnectPacket packet;
packet.fixed_header = MQTT_CONNECT; packet.fixed_header = MQTT_CONNECT;
packet.remaining_length = // 14; packet.remaining_length = // 14;
2 + 4 + 2 + strlen(client_id) + 2 + 4 + 2 + strlen(clientId) +
3; // Protocol name + protocol level + connect 3; // Protocol name + protocol level + connect
// flags + keep alive + client ID // flags + keep alive + client ID
packet.protocol_name_length = 4; // "MQTT" packet.protocol_name_length = 4; // "MQTT"
@ -60,10 +169,10 @@ void MQTTParticipantBase::send_mqtt_connect(const char* client_id) {
this->buffer[index++] = packet.connect_flags; this->buffer[index++] = packet.connect_flags;
this->buffer[index++] = packet.keep_alive & 0xFF; // LSB this->buffer[index++] = packet.keep_alive & 0xFF; // LSB
this->buffer[index++] = (packet.keep_alive >> 8) & 0xFF; // MSB this->buffer[index++] = (packet.keep_alive >> 8) & 0xFF; // MSB
size_t client_id_length = strlen(client_id); size_t client_id_length = strlen(clientId);
this->buffer[index++] = (client_id_length >> 8) & 0xFF; // MSB this->buffer[index++] = (client_id_length >> 8) & 0xFF; // MSB
this->buffer[index++] = client_id_length & 0xFF; // LSB this->buffer[index++] = client_id_length & 0xFF; // LSB
memcpy(&this->buffer[index], client_id, client_id_length); memcpy(&this->buffer[index], clientId, client_id_length);
index += client_id_length; index += client_id_length;
for (int ix = 0; ix < index; ix++) for (int ix = 0; ix < index; ix++)
@ -72,11 +181,70 @@ void MQTTParticipantBase::send_mqtt_connect(const char* client_id) {
// Send the MQTT connect packet // Send the MQTT connect packet
SendTCP(index); SendTCP(index);
*/
std::cout << "Send connect, client ID = " << client_id << std::endl; std::cout << "Send connect, client ID = " << clientId << std::endl;
} }
uint16_t packetId;
// Generate packet ID (incremental)
uint16_t getNextPacketId() {
return ++packetId;
}
std::vector<uint8_t> createSubscribePacket(const char* topic, uint8_t qos) {
std::vector<uint8_t> packet;
// Fixed header
packet.push_back(0x82); // SUBSCRIBE packet type with flags
// Calculate packet length
size_t topicLength = strlen(topic);
int remainingLength =
2 + // Packet ID
2 + // Topic length
topicLength +
1; // QoS byte
// Encode remaining length
do {
uint8_t encodedByte = remainingLength % 128;
remainingLength /= 128;
if (remainingLength > 0) {
encodedByte |= 128;
}
packet.push_back(encodedByte);
} while (remainingLength > 0);
// Packet ID (2 bytes)
uint16_t packetIdentifier = getNextPacketId();
packet.push_back((packetIdentifier >> 8) & 0xFF); // High byte
packet.push_back(packetIdentifier & 0xFF); // Low byte
// Topic length (2 bytes)
packet.push_back((topicLength >> 8) & 0xFF);
packet.push_back(topicLength & 0xFF);
// Topic string
packet.insert(packet.end(), topic, topic + topicLength);
// QoS (1 byte)
packet.push_back(qos);
return packet;
}
void MQTTParticipantBase::sendSubscribe(const char* topic) { void MQTTParticipantBase::sendSubscribe(const char* topic) {
std::vector<uint8_t> packet = createSubscribePacket(topic, 0);
for (int i = 0; i < packet.size(); i++)
this->buffer[i] = packet.data()[i];
int buffersize = packet.size();
SendTCP(buffersize);
/*
// Packet Identifier (2 bytes) // Packet Identifier (2 bytes)
static unsigned short packetId = static unsigned short packetId =
1; // Increment this for each new subscription 1; // Increment this for each new subscription
@ -109,6 +277,7 @@ void MQTTParticipantBase::sendSubscribe(const char* topic) {
// Send the SUBSCRIBE packet // Send the SUBSCRIBE packet
SendTCP(7 + topicLength); SendTCP(7 + topicLength);
*/
std::cout << "Send subscribe to topic: " << topic << std::endl; std::cout << "Send subscribe to topic: " << topic << std::endl;
} }
@ -128,7 +297,7 @@ int MQTTParticipantBase::ReceiveTCP() {
void MQTTParticipantBase::ReceiveData(unsigned char bufferSize) { void MQTTParticipantBase::ReceiveData(unsigned char bufferSize) {
std::cout << " receivemsg\n"; std::cout << " receivemsg\n";
// std::cout << "receive msg " << (int)msgId << "\n"; // std::cout << "receive msg " << (int)msgId << "\n";
// std::cout << " buffer size = " <<(int) bufferSize << "\n"; std::cout << " buffer size = " <<(int) bufferSize << "\n";
}; };
// void ParticipantMQTT::receiveMessages(int sock) { // void ParticipantMQTT::receiveMessages(int sock) {

5
Participants/ParticipantMQTT.h
View File

@ -43,7 +43,7 @@ namespace RoboidControl {
/// participant is created which can be obtained using /// participant is created which can be obtained using
/// RoboidControl::IsolatedParticipant::Isolated(). /// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing() /// @sa RoboidControl::Thing::Thing()
class MQTTParticipantBase : public RemoteParticipantUDP { class MQTTParticipantBase : public ParticipantSocket {
#pragma region Init #pragma region Init
public: public:
@ -60,7 +60,7 @@ protected:
public: public:
/// @brief The remote site when this participant is connected to a site /// @brief The remote site when this participant is connected to a site
RemoteParticipantUDP* remoteSite = nullptr; ParticipantSocket* remoteSite = nullptr;
protected: protected:
@ -128,3 +128,4 @@ protected:
} // namespace RoboidControl } // namespace RoboidControl
#include "Posix/PosixMQTT.h" #include "Posix/PosixMQTT.h"
#include "Windows/WindowsMQTT.h"

24
Participants/ParticipantSocket.cpp Normal file
View File

@ -0,0 +1,24 @@
#include "ParticipantSocket.h"
#include <string.h>
namespace RoboidControl {
ParticipantSocket::ParticipantSocket(const char* ipAddress, int port) {
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64)
strncpy_s(addressString, stringLength, ipAddress,
addressLength); // Leave space for null terminator
#else
strncpy(addressString, ipAddress, addressLength);
#endif
addressString[addressLength] = '\0';
this->ipAddress = addressString;
this->port = port;
}
}

28
Participants/ParticipantSocket.h Normal file
View File

@ -0,0 +1,28 @@
#pragma once
#include "Participant.h"
namespace RoboidControl {
class ParticipantSocket : public Participant {
public:
/// @brief Create a new participant with the given communcation info
/// @param ipAddress The IP address of the participant
/// @param port The UDP port of the participant
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
ParticipantSocket(const char* ipAddress, int port);
/// @brief The Ip Address of a participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
unsigned int port = 0;
bool Send(IMessage* msg) override;
};
}

232
Participants/ParticipantUDP.cpp
View File

@ -12,112 +12,11 @@
#include "Things/DistanceSensor.h" #include "Things/DistanceSensor.h"
#include "Things/TouchSensor.h" #include "Things/TouchSensor.h"
#include <string.h> // #include <string.h>
namespace RoboidControl { namespace RoboidControl {
#pragma region ParticipantRegistry bool ParticipantSocket::Send(IMessage* msg) {
ParticipantRegistry ParticipantUDPGeneric::registry;
RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress,
unsigned int port) {
#if !defined(NO_STD)
for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
if (participant == nullptr)
continue;
if (strcmp(participant->ipAddress, ipAddress) == 0 &&
participant->port == port) {
// std::cout << "found participant " << participant->ipAddress << ":"
// << (int)participant->port << std::endl;
return participant;
}
}
std::cout << "Could not find participant " << ipAddress << ":" << (int)port
<< std::endl;
#endif
return nullptr;
}
RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) {
#if !defined(NO_STD)
for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
if (participant == nullptr)
continue;
if (participant->networkId == participantId)
return participant;
}
std::cout << "Could not find participant " << (int)participantId << std::endl;
#endif
return nullptr;
}
RemoteParticipantUDP* ParticipantRegistry::Add(const char* ipAddress,
unsigned int port) {
RemoteParticipantUDP* participant = new RemoteParticipantUDP(ipAddress, port);
Add(participant);
return participant;
}
void ParticipantRegistry::Add(RemoteParticipantUDP* participant) {
Participant* foundParticipant = Get(participant->networkId);
// Get(participant->ipAddress, participant->port);
if (foundParticipant == nullptr) {
#if defined(NO_STD)
// this->things[this->thingCount++] = thing;
#else
ParticipantRegistry::participants.push_back(participant);
#endif
// std::cout << "Add participant " << participant->ipAddress << ":"
// << participant->port << "[" << (int)participant->networkId
// << "]\n";
// std::cout << "participants " <<
// ParticipantRegistry::participants.size()
// << "\n";
// } else {
// std::cout << "Did not add, existing participant " <<
// participant->ipAddress
// << ":" << participant->port << "[" <<
// (int)participant->networkId
// << "]\n";
}
}
void ParticipantRegistry::Remove(RemoteParticipantUDP* participant) {
// participants.remove(participant);
}
#if defined(NO_STD)
RemoteParticipantUDP** ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#else
const std::list<RemoteParticipantUDP*>& ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#endif
#pragma endregion ParticipantRegistry
RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) {
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64)
strncpy_s(addressString, stringLength, ipAddress,
addressLength); // Leave space for null terminator
#else
strncpy(addressString, ipAddress, addressLength);
#endif
addressString[addressLength] = '\0';
this->ipAddress = addressString;
this->port = port;
}
bool RemoteParticipantUDP::Send(IMessage* msg) {
// No message is actually sent, because this class has no networking // No message is actually sent, because this class has no networking
// implementation // implementation
return false; return false;
@ -126,7 +25,7 @@ bool RemoteParticipantUDP::Send(IMessage* msg) {
#pragma region Init #pragma region Init
ParticipantUDPGeneric::ParticipantUDPGeneric(int port) ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
: RemoteParticipantUDP("127.0.0.1", port) { : ParticipantSocket("127.0.0.1", port) {
this->name = "ParticipantUDP"; this->name = "ParticipantUDP";
this->remoteSite = nullptr; this->remoteSite = nullptr;
if (this->port == 0) if (this->port == 0)
@ -145,12 +44,12 @@ ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress, ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
int port, int port,
int localPort) int localPort)
: RemoteParticipantUDP("127.0.0.1", localPort) { : ParticipantSocket("127.0.0.1", localPort) {
this->name = "ParticipantUDP"; this->name = "ParticipantUDP";
if (this->port == 0) if (this->port == 0)
this->isIsolated = true; this->isIsolated = true;
else else
this->remoteSite = new RemoteParticipantUDP(ipAddress, port); this->remoteSite = new ParticipantSocket(ipAddress, port);
registry.Add(this); registry.Add(this);
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
@ -196,7 +95,7 @@ void ParticipantUDPGeneric::Update(bool recurse) {
this->nextPublishMe = currentTimeMs + this->publishInterval; this->nextPublishMe = currentTimeMs + this->publishInterval;
} }
//this->ReceiveUDP(); this->ReceiveUDP();
} }
UpdateMyThings(); UpdateMyThings();
@ -311,8 +210,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
unsigned int senderPort) { unsigned int senderPort) {
// std::cout << "Receive data from " << senderIpAddress << ":" << senderPort // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
// << std::endl; // << std::endl;
RemoteParticipantUDP* sender = ParticipantSocket* sender = this->registry.Get(senderIpAddress, senderPort);
this->registry.Get(senderIpAddress, senderPort);
if (sender == nullptr) { if (sender == nullptr) {
sender = this->registry.Add(senderIpAddress, senderPort); sender = this->registry.Add(senderIpAddress, senderPort);
#if !defined(NO_STD) #if !defined(NO_STD)
@ -325,7 +223,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
} }
void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize, void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
RemoteParticipantUDP* sender) { ParticipantSocket* sender) {
unsigned char msgId = this->buffer[0]; unsigned char msgId = this->buffer[0];
// std::cout << "receive msg " << (int)msgId << "\n"; // std::cout << "receive msg " << (int)msgId << "\n";
// std::cout << " buffer size = " <<(int) bufferSize << "\n"; // std::cout << " buffer size = " <<(int) bufferSize << "\n";
@ -398,7 +296,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
#endif #endif
} }
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
ParticipantMsg* msg) { ParticipantMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
@ -406,7 +304,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
#endif #endif
} }
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
NetworkIdMsg* msg) { NetworkIdMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
@ -422,7 +320,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
} }
} }
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
InvestigateMsg* msg) { InvestigateMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
@ -430,16 +328,15 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
#endif #endif
} }
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender, ThingMsg* msg) {
ThingMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
<< (int)msg->parentId << "\n"; << (int)msg->parentId << "\n";
#endif #endif
RemoteParticipantUDP* owner = registry.Get(msg->networkId); ParticipantSocket* owner = registry.Get(msg->networkId);
if (owner == nullptr) { if (owner == nullptr) {
owner = new RemoteParticipantUDP(sender->ipAddress, sender->port); owner = new ParticipantSocket(sender->ipAddress, sender->port);
owner->networkId = msg->networkId; owner->networkId = msg->networkId;
registry.Add(owner); registry.Add(owner);
} }
@ -461,7 +358,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
thing->SetParent(nullptr); thing->SetParent(nullptr);
} }
Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner, Thing* ParticipantUDPGeneric::ProcessNewThing(ParticipantSocket* owner,
ThingMsg* msg, ThingMsg* msg,
bool isRemote) { bool isRemote) {
switch (msg->thingType) { switch (msg->thingType) {
@ -476,8 +373,7 @@ Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
} }
} }
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender, NameMsg* msg) {
NameMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/" std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "] "; << (int)msg->thingId << "] ";
@ -509,7 +405,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
#endif #endif
} }
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
ModelUrlMsg* msg) { ModelUrlMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
@ -517,8 +413,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
#endif #endif
} }
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender, PoseMsg* msg) {
PoseMsg* msg) {
#if !defined(DEBUG) && !defined(NO_STD) #if !defined(DEBUG) && !defined(NO_STD)
std::cout << this->name << ": process PoseMsg [" << (int)this->networkId std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
<< "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
@ -541,8 +436,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
thing->SetAngularVelocity(msg->angularVelocity); thing->SetAngularVelocity(msg->angularVelocity);
} }
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender, BinaryMsg* msg) {
BinaryMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n"; << "/" << (int)msg->thingId << "]\n";
@ -564,15 +458,14 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
} }
} }
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender, TextMsg* msg) {
TextMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " " std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
<< (int)msg->text << "\n"; << (int)msg->text << "\n";
#endif #endif
} }
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
DestroyMsg* msg) { DestroyMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/" std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
@ -587,4 +480,87 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
// Receive // Receive
#pragma endregion #pragma endregion
#pragma region Registry
ParticipantUDP::Registry ParticipantUDPGeneric::registry;
ParticipantSocket* ParticipantUDP::Registry::Get(const char* ipAddress, unsigned int port) {
#if !defined(NO_STD)
for (ParticipantSocket* participant : Registry::participants) {
if (participant == nullptr)
continue;
if (strcmp(participant->ipAddress, ipAddress) == 0 &&
participant->port == port) {
// std::cout << "found participant " << participant->ipAddress << ":"
// << (int)participant->port << std::endl;
return participant;
}
}
std::cout << "Could not find participant " << ipAddress << ":" << (int)port
<< std::endl;
#endif
return nullptr;
}
ParticipantSocket* ParticipantUDP::Registry::Get(unsigned char participantId) {
#if !defined(NO_STD)
for (ParticipantSocket* participant : Registry::participants) {
if (participant == nullptr)
continue;
if (participant->networkId == participantId)
return participant;
}
std::cout << "Could not find participant " << (int)participantId << std::endl;
#endif
return nullptr;
}
ParticipantSocket* ParticipantUDP::Registry::Add(const char* ipAddress, unsigned int port) {
ParticipantSocket* participant = new ParticipantSocket(ipAddress, port);
Add(participant);
return participant;
}
void ParticipantUDP::Registry::Add(ParticipantSocket* participant) {
Participant* foundParticipant = Get(participant->networkId);
// Get(participant->ipAddress, participant->port);
if (foundParticipant == nullptr) {
#if defined(NO_STD)
// this->things[this->thingCount++] = thing;
#else
Registry::participants.push_back(participant);
#endif
// std::cout << "Add participant " << participant->ipAddress << ":"
// << participant->port << "[" << (int)participant->networkId
// << "]\n";
// std::cout << "participants " <<
// Registry::participants.size()
// << "\n";
// } else {
// std::cout << "Did not add, existing participant " <<
// participant->ipAddress
// << ":" << participant->port << "[" <<
// (int)participant->networkId
// << "]\n";
}
}
void ParticipantUDP::Registry::Remove(ParticipantSocket* participant) {
// participants.remove(participant);
}
#if defined(NO_STD)
RemoteParticipantUDP** Registry::GetAll() const {
return Registry::participants;
}
#else
const std::list<ParticipantSocket*>& ParticipantUDP::Registry::GetAll() const {
return Registry::participants;
}
#endif
// Registry
#pragma endregion Registry
} // namespace RoboidControl } // namespace RoboidControl

153
Participants/ParticipantUDP.h
View File

@ -10,7 +10,8 @@
#include "Messages/PoseMsg.h" #include "Messages/PoseMsg.h"
#include "Messages/TextMsg.h" #include "Messages/TextMsg.h"
#include "Messages/ThingMsg.h" #include "Messages/ThingMsg.h"
#include "Participant.h"
#include "ParticipantSocket.h"
#if !defined(NO_STD) #if !defined(NO_STD)
#include <functional> #include <functional>
@ -31,74 +32,6 @@ namespace RoboidControl {
constexpr int MAX_SENDER_COUNT = 256; constexpr int MAX_SENDER_COUNT = 256;
class RemoteParticipantUDP : public Participant {
public:
/// @brief Create a new participant with the given communcation info
/// @param ipAddress The IP address of the participant
/// @param port The UDP port of the participant
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
RemoteParticipantUDP(const char* ipAddress, int port);
/// @brief The Ip Address of a participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
unsigned int port = 0;
bool Send(IMessage* msg) override;
};
/// @brief class which manages all known participants
class ParticipantRegistry {
public:
/// @brief Retrieve a participant by its address
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found
RemoteParticipantUDP* Get(const char* ipAddress, unsigned int port);
/// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found
RemoteParticipantUDP* Get(unsigned char networkID);
/// @brief Add a participant with the given details
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The added participant
RemoteParticipantUDP* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(RemoteParticipantUDP* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(RemoteParticipantUDP* participant);
private:
#if defined(NO_STD)
public:
RemoteParticipantUDP** GetAll() const;
int count = 0;
private:
RemoteParticipantUDP** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<RemoteParticipantUDP*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<RemoteParticipantUDP*> participants;
#endif
};
/// @brief A participant using UDP communication /// @brief A participant using UDP communication
/// A local participant is the local device which can communicate with /// A local participant is the local device which can communicate with
/// other participants It manages all local things and communcation with other /// other participants It manages all local things and communcation with other
@ -111,7 +44,7 @@ class ParticipantRegistry {
/// participant is created which can be obtained using /// participant is created which can be obtained using
/// RoboidControl::IsolatedParticipant::Isolated(). /// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing() /// @sa RoboidControl::Thing::Thing()
class ParticipantUDPGeneric : public RemoteParticipantUDP { class ParticipantUDPGeneric : public ParticipantSocket {
#pragma region Init #pragma region Init
public: public:
@ -139,7 +72,7 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
bool isIsolated = false; bool isIsolated = false;
/// @brief The remote site when this participant is connected to a site /// @brief The remote site when this participant is connected to a site
RemoteParticipantUDP* remoteSite = nullptr; ParticipantSocket* remoteSite = nullptr;
/// The interval in milliseconds for publishing (broadcasting) data on the /// The interval in milliseconds for publishing (broadcasting) data on the
/// local network /// local network
@ -192,36 +125,86 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
void ReceiveData(unsigned char bufferSize, void ReceiveData(unsigned char bufferSize,
char* senderIpAddress, char* senderIpAddress,
unsigned int senderPort); unsigned int senderPort);
void ReceiveData(unsigned char bufferSize, RemoteParticipantUDP* remoteParticipant); void ReceiveData(unsigned char bufferSize,
ParticipantSocket* remoteParticipant);
virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0; virtual void SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) = 0;
virtual void ReceiveUDP() = 0; virtual void ReceiveUDP() = 0;
virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg); virtual void Process(ParticipantSocket* sender, ParticipantMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg); virtual void Process(ParticipantSocket* sender, NetworkIdMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg); virtual void Process(ParticipantSocket* sender, InvestigateMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg); virtual void Process(ParticipantSocket* sender, ThingMsg* msg);
virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender, virtual Thing* ProcessNewThing(ParticipantSocket* sender,
ThingMsg* msg, ThingMsg* msg,
bool isRemote); bool isRemote);
virtual void Process(RemoteParticipantUDP* sender, NameMsg* msg); virtual void Process(ParticipantSocket* sender, NameMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, ModelUrlMsg* msg); virtual void Process(ParticipantSocket* sender, ModelUrlMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, PoseMsg* msg); virtual void Process(ParticipantSocket* sender, PoseMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, BinaryMsg* msg); virtual void Process(ParticipantSocket* sender, BinaryMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, TextMsg* msg); virtual void Process(ParticipantSocket* sender, TextMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, DestroyMsg* msg); virtual void Process(ParticipantSocket* sender, DestroyMsg* msg);
#pragma endregion Receive #pragma endregion Receive
public: public:
static ParticipantRegistry registry; /// @brief class which manages all known participants
class Registry {
public:
/// @brief Retrieve a participant by its address
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found
ParticipantSocket* Get(const char* ipAddress, unsigned int port);
/// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found
ParticipantSocket* Get(unsigned char networkID);
/// @brief Add a participant with the given details
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The added participant
ParticipantSocket* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(ParticipantSocket* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(ParticipantSocket* participant);
private:
#if defined(NO_STD)
public:
ParticipantSocket** GetAll() const;
int count = 0;
private:
ParticipantSocket** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<ParticipantSocket*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<ParticipantSocket*> participants;
#endif
};
static Registry registry;
}; };
} // namespace RoboidControl } // namespace RoboidControl
#include "EspIdf/EspIdfParticipant.h" #include "EspIdf/EspIdfParticipant.h"
#include "Posix/PosixParticipant.h" #include "Posix/PosixParticipant.h"
#include "Windows/WindowsParticipant.h"

6
Participants/SiteServer.cpp
View File

@ -56,7 +56,7 @@ void SiteServer::UpdateMyThings() {
#pragma region Receive #pragma region Receive
void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) { void SiteServer::Process(ParticipantSocket* sender, ParticipantMsg* msg) {
if (msg->networkId != sender->networkId) { if (msg->networkId != sender->networkId) {
// std::cout << this->name << " received New Client -> " << // std::cout << this->name << " received New Client -> " <<
// sender->ipAddress // sender->ipAddress
@ -67,9 +67,9 @@ void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
} }
} }
void SiteServer::Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {} void SiteServer::Process(ParticipantSocket* sender, NetworkIdMsg* msg) {}
void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* msg) { void SiteServer::Process(ParticipantSocket* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->networkId, msg->thingId); Thing* thing = sender->Get(msg->networkId, msg->thingId);
if (thing == nullptr) { if (thing == nullptr) {
// new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);

6
Participants/SiteServer.h
View File

@ -33,9 +33,9 @@ class SiteServer : public ParticipantUDP {
protected: protected:
unsigned long nextPublishMe = 0; unsigned long nextPublishMe = 0;
virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) override; virtual void Process(ParticipantSocket* sender, ParticipantMsg* msg) override;
virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override; virtual void Process(ParticipantSocket* sender, NetworkIdMsg* msg) override;
virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override; virtual void Process(ParticipantSocket* sender, ThingMsg* msg) override;
#pragma endregion Receive #pragma endregion Receive

27
Posix/PosixParticipant.cpp
View File

@ -128,32 +128,7 @@ void ParticipantUDP::ReceiveUDP() {
#endif #endif
} }
// int ParticipantUDP::ReceiveTCP() { bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) {
// #if defined(__unix__) || defined(__APPLE__)
// int bytesReceived = recv(sock, buffer, sizeof(buffer) - 1, 0);
// if (bytesReceived > 0) {
// buffer[bytesReceived] = '\0'; // Null-terminate the received data
// std::cout << "Received: " << buffer << std::endl;
// return bytesReceived;
// } else if (bytesReceived == 0) {
// // Connection has been gracefully closed
// std::cout << "Connection closed by the server." << std::endl;
// return 0;
// } else {
// if (errno == EAGAIN || errno == EWOULDBLOCK) {
// // No data available to read, continue the loop
// // std::cout << "No data available" << std::endl;
// } else {
// std::cerr << "Error receiving data: " << strerror(errno) << std::endl;
// }
// return 0;
// }
// #endif // _WIN32 || _WIN64
// return 0;
// }
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
int bufferSize) {
#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__)
// std::cout << "Send to " << remoteParticipant->ipAddress << ":" << // std::cout << "Send to " << remoteParticipant->ipAddress << ":" <<
// ntohs(remoteParticipant->port) // ntohs(remoteParticipant->port)

2
Posix/PosixParticipant.h
View File

@ -29,7 +29,7 @@ class ParticipantUDP : public ParticipantUDPGeneric {
// void Receive(); // void Receive();
int ReceiveTCP(); int ReceiveTCP();
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg) override; bool Publish(IMessage* msg) override;
protected: protected:

102
Windows/WindowsMQTT.cpp Normal file
View File

@ -0,0 +1,102 @@
#include "WindowsMQTT.h"
#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
#include <ws2tcpip.h>
#include <chrono>
#include <thread>
namespace RoboidControl {
MQTTParticipant::MQTTParticipant(const char* remoteIpAddress, int port)
: MQTTParticipantBase(remoteIpAddress, port) {
SetupTCP();
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
send_mqtt_connect("RoboidControl");
sendSubscribe("domoticz/out");
}
void MQTTParticipant::SetupTCP() {
// Initialize Winsock
WSADATA wsaData;
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
std::cerr << "WSAStartup failed: " << std::endl;
return;
}
// Create a TCP socket
this->sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
if (sock == INVALID_SOCKET) {
std::cerr << "TCP Socket creation failed: " << WSAGetLastError()
<< std::endl;
WSACleanup();
return;
}
// Define the server address
memset(&this->remote_addr, 0, sizeof(this->remote_addr));
this->remote_addr.sin_family = AF_INET;
this->remote_addr.sin_port = htons((u_short)this->remoteSite->port);
if (inet_pton(AF_INET, this->remoteSite->ipAddress, &this->remote_addr.sin_addr) <= 0) {
std::cerr << "Invalid address" << std::endl;
closesocket(sock);
WSACleanup();
return;
}
// Connect to the server
if (connect(sock, (sockaddr*)&this->remote_addr, sizeof(this->remote_addr)) ==
SOCKET_ERROR) {
std::cerr << "Connection failed: " << WSAGetLastError() << std::endl;
closesocket(sock);
WSACleanup();
return;
}
// Set the socket to non-blocking mode
u_long mode = 1; // 1 to enable non-blocking socket
if (ioctlsocket(sock, FIONBIO, &mode) != NO_ERROR) {
std::cerr << "Failed to set non-blocking mode: " << WSAGetLastError()
<< std::endl;
closesocket(sock);
WSACleanup();
return;
}
}
int MQTTParticipant::ReceiveTCP() {
int bytesReceived = recv(sock, buffer, sizeof(buffer) - 1, 0);
if (bytesReceived > 0) {
buffer[bytesReceived] = '\0'; // Null-terminate the received data
std::cout << "Received: " << buffer << std::endl;
return bytesReceived;
} else if (bytesReceived == 0) {
// Connection has been gracefully closed
std::cout << "Connection closed by the server." << std::endl;
return 0;
} else {
int error = WSAGetLastError();
if (error == WSAEWOULDBLOCK) {
// No data available, continue with other tasks
//std::cout << "No data available, continuing..." << std::endl;
// You can add a sleep or other logic here to avoid busy waiting
} else {
std::cerr << "Receive failed: " << error << std::endl;
}
return 0;
}
return 0;
}
void MQTTParticipant::SendTCP(int bufferSize) {
#if defined(_WIN32) || defined(_WIN64)
send(sock, this->buffer, bufferSize, 0);
#endif
}
} // namespace RoboidControl
#endif

27
Windows/WindowsMQTT.h Normal file
View File

@ -0,0 +1,27 @@
#pragma once
#if defined(_WIN32) || defined(_WIN64)
#include "Participants/ParticipantMQTT.h"
namespace RoboidControl {
class MQTTParticipant : public MQTTParticipantBase {
public:
MQTTParticipant(const char* ipAddress, int port = 1883);
protected:
void SetupTCP() override;
void SendTCP(int bufferSize) override;
int ReceiveTCP() override;
sockaddr_in remote_addr;
public:
int sock;
};
} // namespace RoboidControl
#endif

11
Windows/WindowsParticipant.cpp
View File

@ -9,9 +9,12 @@
namespace RoboidControl { namespace RoboidControl {
ParticipantUDP::ParticipantUDP() {} ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}
void ParticipantUDP::Setup(int localPort, ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
: ParticipantUDPGeneric(ipAddress, port, localPort) {}
void ParticipantUDP::SetupUDP(int localPort,
const char* remoteIpAddress, const char* remoteIpAddress,
int remotePort) { int remotePort) {
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
@ -128,7 +131,7 @@ void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) {
#endif // _WIN32 || _WIN64 #endif // _WIN32 || _WIN64
} }
void ParticipantUDP::Receive() { void ParticipantUDP::ReceiveUDP() {
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
// char ip_str[INET_ADDRSTRLEN]; // char ip_str[INET_ADDRSTRLEN];
// inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN); // inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
@ -195,7 +198,7 @@ int ParticipantUDP::ReceiveTCP() {
return 0; return 0;
} }
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) { bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) {
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
char ip_str[INET_ADDRSTRLEN]; char ip_str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN); inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);

9
Windows/WindowsParticipant.h
View File

@ -7,14 +7,15 @@ namespace RoboidControl {
class ParticipantUDP : public ParticipantUDPGeneric { class ParticipantUDP : public ParticipantUDPGeneric {
public: public:
ParticipantUDP(); ParticipantUDP(int port = 7681);
void Setup(int localPort, const char* remoteIpAddress, int remotePort); ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
void SetupTCP(const char* remoteIpAddres, int remotePort); void SetupTCP(const char* remoteIpAddres, int remotePort);
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
bool SendTCP(int bufferSize); bool SendTCP(int bufferSize);
bool Publish(IMessage* msg); bool Publish(IMessage* msg);
void Receive(); void ReceiveUDP();
int ReceiveTCP(); int ReceiveTCP();
protected: protected: