diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index 4897569..1e8f396 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -88,7 +88,7 @@ void ParticipantUDP::Receive() { #endif } -bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) { #if defined(ARDUINO) && defined(HAS_WIFI) // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":" // << remoteParticipant->port << "\n"; diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h index 7531302..6f0f5c1 100644 --- a/Arduino/ArduinoParticipant.h +++ b/Arduino/ArduinoParticipant.h @@ -9,7 +9,7 @@ class ParticipantUDP : public ParticipantUDPGeneric { public: void Setup(); void Receive(); - bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); + bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index f4e0c90..55041ae 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -181,7 +181,7 @@ bool ParticipantUDP::Send(IMessage* msg) { return this->SendTo(this->remoteSite, bufferSize); } -bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, +bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) { #if defined(IDF_VER) uint16_t port = ntohs(dest_addr.sin_port); diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index 1c74ebf..7ac2302 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -23,7 +23,7 @@ class ParticipantUDP : public ParticipantUDPGeneric { void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); - bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); + bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); // bool SendTest(); diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index c7e53c7..9f6dd63 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,14 +1,21 @@ #include "Matrix.h" -#if !defined(NO_STD) +#if !defined(NO_STD) #include #endif +// #include "esp_log.h" +// #include "esp_system.h" +// #include "freertos/FreeRTOS.h" +// #include "freertos/task.h" + namespace LinearAlgebra { #pragma region Matrix1 +Matrix1::Matrix1() {} + Matrix1::Matrix1(int size) : size(size) { - if (this->size == 0) + if (this->size <= 0) data = nullptr; else { 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; } +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 r = Matrix1(4); 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]); } +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 #pragma endregion @@ -43,15 +137,19 @@ Matrix2::Matrix2() {} Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { this->nValues = nRows * nCols; - if (this->nValues == 0) + // printf("Allocate %d\n", this->nValues); + + if (this->nValues <= 0) { this->data = nullptr; - else { - this->data = new float[this->nValues]; this->externalData = false; + return; } + + this->data = new float[this->nValues]; + 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) { this->nValues = nRows * nCols; this->externalData = true; @@ -88,39 +186,43 @@ Matrix2& Matrix2::operator=(const Matrix2& m) { } Matrix2::~Matrix2() { - if (!this->externalData) + if (!this->externalData) { + // printf("Deallocate %d\n", this->nValues); 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; +unsigned int Matrix2::NRows() { + return this->nRows; +} + +unsigned int Matrix2::NCols() { + return this->nCols; } // Move constructor -Matrix2::Matrix2(Matrix2&& other) noexcept - : nRows(other.nRows), - nCols(other.nCols), - nValues(other.nValues), - data(other.data) { - other.data = nullptr; // Set the other object's pointer to nullptr to avoid - // double deletion -} +// Matrix2::Matrix2(Matrix2&& other) noexcept +// : nRows(other.nRows), +// nCols(other.nCols), +// nValues(other.nValues), +// data(other.data) { +// other.data = nullptr; // Set the other object's pointer to nullptr to +// avoid +// // double deletion +// } // Move assignment operator -Matrix2& Matrix2::operator=(Matrix2&& other) noexcept { - if (this != &other) { - delete[] data; // Clean up current data - nRows = other.nRows; - nCols = other.nCols; - nValues = other.nValues; - data = other.data; - other.data = nullptr; // Avoid double deletion - } - return *this; -} +// Matrix2& Matrix2::operator=(Matrix2&& other) noexcept { +// if (this != &other) { +// delete[] data; // Clean up current data +// nRows = other.nRows; +// nCols = other.nCols; +// nValues = other.nValues; +// data = other.data; +// other.data = nullptr; // Avoid double deletion +// } +// return *this; +// } Matrix2 Matrix2::Zero(int nRows, int nCols) { Matrix2 r = Matrix2(nRows, nCols); @@ -129,9 +231,16 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) { 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++) - this->data[ix] = 0; + this->data[ix] = value; } Matrix2 Matrix2::Identity(int size) { @@ -186,13 +295,30 @@ Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const { return r; } -Matrix2 Matrix2::operator+=(const Matrix2& v) { +Matrix2& Matrix2::operator+=(const Matrix2& v) { for (int ix = 0; ix < this->nValues; ix++) this->data[ix] += v.data[ix]; 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); int ACols = this->nCols; @@ -221,6 +347,43 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { 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 r = Matrix2(rowStop - rowStart, colStop - colStart); @@ -234,22 +397,18 @@ Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) { 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, int rowStop, int colStart, int colStop, const Matrix2& m) const { - // for (int i = rowStart; i < rowStop; i++) { - // for (int j = colStart; j < colStop; j++) - // this->data[i * this->nCols + j] = - // m.data[(i - rowStart) * m.nCols + (j - colStart)]; - // } - - int rRowDataIx = rowStart * this->nCols; int mRowDataIx = 0; for (int rowIx = rowStart; rowIx < rowStop; rowIx++) { - rRowDataIx = rowIx * this->nCols; - // rRowDataIx += this->nCols; + int rRowDataIx = rowIx * this->nCols; mRowDataIx += m.nCols; for (int colIx = colStart; colIx < colStop; colIx++) { 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 /// @param v The vector /// @return 4x4 Omega matrix diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index ef72922..dafd105 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -9,15 +9,36 @@ namespace LinearAlgebra { /// @brief A 1-dimensional matrix or vector of arbitrary size class Matrix1 { public: - float* data = nullptr; - int size = 0; - + Matrix1(); 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); 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: bool externalData = true; }; @@ -25,31 +46,47 @@ class Matrix1 { /// @brief A 2-dimensional matrix of arbitrary size class Matrix2 { public: - int nRows = 0; - int nCols = 0; - int nValues = 0; - float* data = nullptr; - Matrix2(); 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& operator=(const Matrix2& other); ~Matrix2(); - Matrix2 Clone() const; + unsigned int NRows(); + unsigned int NCols(); static Matrix2 Zero(int nRows, int nCols); - void Clear(); + void Fill(float value); static Matrix2 Identity(int size); - static Matrix2 Diagonal(float f, int size); - static Matrix2 SkewMatrix(const Vector3& v); 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; @@ -57,7 +94,9 @@ class Matrix2 { /// @param m The matrix to add to this matrix /// @return The result of the addition 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; friend Matrix2 operator*(const Matrix2& m, float f) { @@ -73,15 +112,7 @@ class Matrix2 { return r; } - 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 Matrix1 operator*(const Matrix2& m, const Matrix1& v); friend Matrix2 operator/(const Matrix2& m, float f) { Matrix2 r = Matrix2(m.nRows, m.nCols); @@ -96,20 +127,19 @@ class Matrix2 { 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 - Matrix2(Matrix2&& other) noexcept; - Matrix2& operator=(Matrix2&& other) noexcept; + // move constructor and move assignment operator + // Matrix2(Matrix2&& other) noexcept; + // Matrix2& operator=(Matrix2&& other) noexcept; static Matrix2 Omega(const Vector3& v); + public: + int nRows = 0; + int nCols = 0; +protected: + int nValues = 0; + float* data = nullptr; + private: bool externalData = true; }; diff --git a/LinearAlgebra/Quaternion.cpp b/LinearAlgebra/Quaternion.cpp index dda8c18..0592d55 100644 --- a/LinearAlgebra/Quaternion.cpp +++ b/LinearAlgebra/Quaternion.cpp @@ -99,14 +99,12 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) { } Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() { - Matrix2 r = Matrix2(3, 3); - float x = this->x; float y = this->y; float z = this->z; float w = this->w; - float* data = r.data; + float* data = new float[9]; // r.data; data[0 * 3 + 0] = 1 - 2 * (y * y + z * z); data[0 * 3 + 1] = 2 * (x * y - w * z); data[0 * 3 + 2] = 2 * (x * z + w * y); @@ -117,6 +115,8 @@ Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() { data[2 * 3 + 1] = 2 * (y * z + w * x); data[2 * 3 + 2] = 1 - 2 * (x * x + y * y); + Matrix2 r = Matrix2(3, 3, data, true); + 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); } +Quaternion Quaternion::Reflect(Quaternion q) { + return Quaternion(-q.x, -q.y, -q.z, q.w); +} + Quaternion Quaternion::LookRotation(const Vector3& forward) { Vector3 up = Vector3(0, 1, 0); return LookRotation(forward, up); diff --git a/LinearAlgebra/Quaternion.h b/LinearAlgebra/Quaternion.h index 1687dc9..9c12c2c 100644 --- a/LinearAlgebra/Quaternion.h +++ b/LinearAlgebra/Quaternion.h @@ -126,6 +126,8 @@ struct Quaternion : Quat { /// needed The inverted quaternion static Quaternion Inverse(Quaternion quaternion); + static Quaternion Reflect(Quaternion q); + /// /// A rotation which looks in the given direction /// diff --git a/LinearAlgebra/Vector2.cpp b/LinearAlgebra/Vector2.cpp index 1e34a99..f0d7894 100644 --- a/LinearAlgebra/Vector2.cpp +++ b/LinearAlgebra/Vector2.cpp @@ -180,3 +180,13 @@ Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) { Vector2 v = v1 + (v2 - v1) * f; return v; } + + +#pragma region Vector2Of + +template +Vector2Of::Vector2Of() { + this->horizontal = 0; + this->vertical = 0; +} +#pragma endregion Vector2Of \ No newline at end of file diff --git a/LinearAlgebra/Vector2.h b/LinearAlgebra/Vector2.h index 04fa669..24aff28 100644 --- a/LinearAlgebra/Vector2.h +++ b/LinearAlgebra/Vector2.h @@ -201,6 +201,17 @@ struct Vector2 : Vec2 { static Vector2 Lerp(const Vector2& v1, const Vector2& v2, float f); }; +template +struct Vector2Of { + public: + Vector2Of(); + + T horizontal = 0; + T vertical = 0; +}; + +using Vector2Int = Vector2Of; + } // namespace LinearAlgebra using namespace LinearAlgebra; diff --git a/LinearAlgebra/test/Matrix_test.cc b/LinearAlgebra/test/Matrix_test.cc index 4cb66b8..225a157 100644 --- a/LinearAlgebra/test/Matrix_test.cc +++ b/LinearAlgebra/test/Matrix_test.cc @@ -1,215 +1,215 @@ -#if GTEST -#include -#include -#include +// #if GTEST +// #include +// #include +// #include -#include "Matrix.h" +// #include "Matrix.h" -TEST(Matrix2, Zero) { - // Test case 1: 2x2 zero matrix - Matrix2 zeroMatrix = Matrix2::Zero(2, 2); - EXPECT_TRUE(zeroMatrix.nRows == 2); - EXPECT_TRUE(zeroMatrix.nCols == 2); - for (int i = 0; i < zeroMatrix.nValues; ++i) { - EXPECT_TRUE(zeroMatrix.data[i] == 0.0f); - } - std::cout << "Test case 1 passed: 2x2 zero matrix\n"; +// TEST(Matrix2, Zero) { +// // Test case 1: 2x2 zero matrix +// Matrix2 zeroMatrix = Matrix2::Zero(2, 2); +// EXPECT_TRUE(zeroMatrix.nRows == 2); +// EXPECT_TRUE(zeroMatrix.nCols == 2); +// for (int i = 0; i < zeroMatrix.nValues; ++i) { +// EXPECT_TRUE(zeroMatrix.data[i] == 0.0f); +// } +// std::cout << "Test case 1 passed: 2x2 zero matrix\n"; - // Test case 2: 3x3 zero matrix - zeroMatrix = Matrix2::Zero(3, 3); - EXPECT_TRUE(zeroMatrix.nRows == 3); - EXPECT_TRUE(zeroMatrix.nCols == 3); - for (int i = 0; i < zeroMatrix.nValues; ++i) { - EXPECT_TRUE(zeroMatrix.data[i] == 0.0f); - } - std::cout << "Test case 2 passed: 3x3 zero matrix\n"; +// // Test case 2: 3x3 zero matrix +// zeroMatrix = Matrix2::Zero(3, 3); +// EXPECT_TRUE(zeroMatrix.nRows == 3); +// EXPECT_TRUE(zeroMatrix.nCols == 3); +// for (int i = 0; i < zeroMatrix.nValues; ++i) { +// EXPECT_TRUE(zeroMatrix.data[i] == 0.0f); +// } +// std::cout << "Test case 2 passed: 3x3 zero matrix\n"; - // Test case 3: 1x1 zero matrix - zeroMatrix = Matrix2::Zero(1, 1); - EXPECT_TRUE(zeroMatrix.nRows == 1); - EXPECT_TRUE(zeroMatrix.nCols == 1); - EXPECT_TRUE(zeroMatrix.data[0] == 0.0f); - std::cout << "Test case 3 passed: 1x1 zero matrix\n"; +// // Test case 3: 1x1 zero matrix +// zeroMatrix = Matrix2::Zero(1, 1); +// EXPECT_TRUE(zeroMatrix.nRows == 1); +// EXPECT_TRUE(zeroMatrix.nCols == 1); +// EXPECT_TRUE(zeroMatrix.data[0] == 0.0f); +// std::cout << "Test case 3 passed: 1x1 zero matrix\n"; - // Test case 4: 0x0 matrix (edge case) - zeroMatrix = Matrix2::Zero(0, 0); - EXPECT_TRUE(zeroMatrix.nRows == 0); - EXPECT_TRUE(zeroMatrix.nCols == 0); - EXPECT_TRUE(zeroMatrix.data == nullptr); - std::cout << "Test case 4 passed: 0x0 matrix\n"; -} +// // Test case 4: 0x0 matrix (edge case) +// zeroMatrix = Matrix2::Zero(0, 0); +// EXPECT_TRUE(zeroMatrix.nRows == 0); +// EXPECT_TRUE(zeroMatrix.nCols == 0); +// EXPECT_TRUE(zeroMatrix.data == nullptr); +// std::cout << "Test case 4 passed: 0x0 matrix\n"; +// } -TEST(Matrix2, Multiplication) { - // Test 1: Multiplying two 2x2 matrices - float dataA[] = {1, 2, 3, 4}; - float dataB[] = {5, 6, 7, 8}; - Matrix2 A(dataA, 2, 2); - Matrix2 B(dataB, 2, 2); +// TEST(Matrix2, Multiplication) { +// // Test 1: Multiplying two 2x2 matrices +// float dataA[] = {1, 2, 3, 4}; +// float dataB[] = {5, 6, 7, 8}; +// Matrix2 A(dataA, 2, 2); +// Matrix2 B(dataB, 2, 2); - Matrix2 result = A * B; +// Matrix2 result = A * B; - float expectedData[] = {19, 22, 43, 50}; - for (int i = 0; i < 4; ++i) - EXPECT_TRUE(result.data[i] == expectedData[i]); - std::cout << "Test 1 passed: 2x2 matrix multiplication.\n"; +// float expectedData[] = {19, 22, 43, 50}; +// for (int i = 0; i < 4; ++i) +// EXPECT_TRUE(result.data[i] == expectedData[i]); +// std::cout << "Test 1 passed: 2x2 matrix multiplication.\n"; - // Test 2: Multiplying a 3x2 matrix with a 2x3 matrix - float dataC[] = {1, 2, 3, 4, 5, 6}; - float dataD[] = {7, 8, 9, 10, 11, 12}; - Matrix2 C(dataC, 3, 2); - Matrix2 D(dataD, 2, 3); +// // Test 2: Multiplying a 3x2 matrix with a 2x3 matrix +// float dataC[] = {1, 2, 3, 4, 5, 6}; +// float dataD[] = {7, 8, 9, 10, 11, 12}; +// Matrix2 C(dataC, 3, 2); +// Matrix2 D(dataD, 2, 3); - Matrix2 result2 = C * D; +// Matrix2 result2 = C * D; - float expectedData2[] = {27, 30, 33, 61, 68, 75, 95, 106, 117}; - for (int i = 0; i < 9; ++i) - EXPECT_TRUE(result2.data[i] == expectedData2[i]); - std::cout << "Test 2 passed: 3x2 * 2x3 matrix multiplication.\n"; +// float expectedData2[] = {27, 30, 33, 61, 68, 75, 95, 106, 117}; +// for (int i = 0; i < 9; ++i) +// EXPECT_TRUE(result2.data[i] == expectedData2[i]); +// std::cout << "Test 2 passed: 3x2 * 2x3 matrix multiplication.\n"; - // Test 3: Multiplying with a zero matrix - Matrix2 zeroMatrix = Matrix2::Zero(2, 2); - Matrix2 result3 = A * zeroMatrix; +// // Test 3: Multiplying with a zero matrix +// Matrix2 zeroMatrix = Matrix2::Zero(2, 2); +// Matrix2 result3 = A * zeroMatrix; - for (int i = 0; i < 4; ++i) - EXPECT_TRUE(result3.data[i] == 0); - std::cout << "Test 3 passed: Multiplication with zero matrix.\n"; +// for (int i = 0; i < 4; ++i) +// EXPECT_TRUE(result3.data[i] == 0); +// std::cout << "Test 3 passed: Multiplication with zero matrix.\n"; - // Test 4: Multiplying with an identity matrix - Matrix2 identityMatrix = Matrix2::Identity(2); - Matrix2 result4 = A * identityMatrix; +// // Test 4: Multiplying with an identity matrix +// Matrix2 identityMatrix = Matrix2::Identity(2); +// Matrix2 result4 = A * identityMatrix; - for (int i = 0; i < 4; ++i) - EXPECT_TRUE(result4.data[i] == A.data[i]); - std::cout << "Test 4 passed: Multiplication with identity matrix.\n"; +// for (int i = 0; i < 4; ++i) +// EXPECT_TRUE(result4.data[i] == A.data[i]); +// std::cout << "Test 4 passed: Multiplication with identity matrix.\n"; -} +// } -TEST(MatrixSingle, Init) { - // zero - MatrixOf m0 = MatrixOf(0, 0); +// TEST(MatrixSingle, Init) { +// // zero +// MatrixOf m0 = MatrixOf(0, 0); - // one - float data1[] = {1.0F}; - MatrixOf m1 = MatrixOf(1, 1, data1); +// // one +// float data1[] = {1.0F}; +// MatrixOf m1 = MatrixOf(1, 1, data1); - // two - float data2[] = {1.0F, 2.0F, 3.0F, 4.0F}; - MatrixOf m2 = MatrixOf(2, 2, data2); +// // two +// float data2[] = {1.0F, 2.0F, 3.0F, 4.0F}; +// MatrixOf m2 = MatrixOf(2, 2, data2); - // negative - // MatrixOf m_1 = MatrixOf(-1, -1); - // parameters are unsigned -} +// // negative +// // MatrixOf m_1 = MatrixOf(-1, -1); +// // parameters are unsigned +// } -TEST(MatrixSingle, Transpose) { - float data1[] = {1.0F}; - MatrixOf m = MatrixOf(1, 1, data1); +// TEST(MatrixSingle, Transpose) { +// float data1[] = {1.0F}; +// MatrixOf m = MatrixOf(1, 1, data1); - MatrixOf r = MatrixOf(1, 1); - m.Transpose(&r); +// MatrixOf r = MatrixOf(1, 1); +// m.Transpose(&r); - // 2 x 2 +// // 2 x 2 - float data3[] = {1.0F, 2.0F, 3.0F, 4.0F}; - MatrixOf m22 = MatrixOf(2, 2, data3); - EXPECT_EQ(m22.RowCount(), 2); - EXPECT_EQ(m22.ColCount(), 2); +// float data3[] = {1.0F, 2.0F, 3.0F, 4.0F}; +// MatrixOf m22 = MatrixOf(2, 2, data3); +// EXPECT_EQ(m22.RowCount(), 2); +// EXPECT_EQ(m22.ColCount(), 2); - float data4[] = {0.0F, 0.0F, 0.0F, 0.0F}; - MatrixOf r22 = MatrixOf(2, 2, data4); - EXPECT_EQ(r22.RowCount(), 2); - EXPECT_EQ(r22.ColCount(), 2); +// float data4[] = {0.0F, 0.0F, 0.0F, 0.0F}; +// MatrixOf r22 = MatrixOf(2, 2, data4); +// EXPECT_EQ(r22.RowCount(), 2); +// EXPECT_EQ(r22.ColCount(), 2); - m22.Transpose(&r22); - EXPECT_EQ(r22.RowCount(), 2); - EXPECT_EQ(r22.ColCount(), 2); - EXPECT_FLOAT_EQ(r22.Get(0, 0), 1.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, 1), 4.0F); +// m22.Transpose(&r22); +// EXPECT_EQ(r22.RowCount(), 2); +// EXPECT_EQ(r22.ColCount(), 2); +// EXPECT_FLOAT_EQ(r22.Get(0, 0), 1.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, 1), 4.0F); - // 1 x 2 - float data12[] = {1.0F, 2.0F}; - MatrixOf m12 = MatrixOf(1, 2, data12); - EXPECT_EQ(m12.RowCount(), 1); - EXPECT_EQ(m12.ColCount(), 2); +// // 1 x 2 +// float data12[] = {1.0F, 2.0F}; +// MatrixOf m12 = MatrixOf(1, 2, data12); +// EXPECT_EQ(m12.RowCount(), 1); +// EXPECT_EQ(m12.ColCount(), 2); - float data21[] = {0.0F, 0.0F}; - MatrixOf r21 = MatrixOf(2, 1, data21); - EXPECT_EQ(r21.RowCount(), 2); - EXPECT_EQ(r21.ColCount(), 1); +// float data21[] = {0.0F, 0.0F}; +// MatrixOf r21 = MatrixOf(2, 1, data21); +// EXPECT_EQ(r21.RowCount(), 2); +// EXPECT_EQ(r21.ColCount(), 1); - m12.Transpose(&r21); - EXPECT_EQ(r21.RowCount(), 2); - EXPECT_EQ(r21.ColCount(), 1); - EXPECT_FLOAT_EQ(r21.Get(0, 0), 1.0F); - EXPECT_FLOAT_EQ(r21.Get(1, 0), 2.0F); +// m12.Transpose(&r21); +// EXPECT_EQ(r21.RowCount(), 2); +// EXPECT_EQ(r21.ColCount(), 1); +// EXPECT_FLOAT_EQ(r21.Get(0, 0), 1.0F); +// EXPECT_FLOAT_EQ(r21.Get(1, 0), 2.0F); - // changing dimensions, same size is okay - MatrixOf r12 = MatrixOf(1, 2, data21); - EXPECT_EQ(r12.RowCount(), 1); - EXPECT_EQ(r12.ColCount(), 2); +// // changing dimensions, same size is okay +// MatrixOf r12 = MatrixOf(1, 2, data21); +// EXPECT_EQ(r12.RowCount(), 1); +// EXPECT_EQ(r12.ColCount(), 2); - m12.Transpose(&r12); - EXPECT_EQ(r12.RowCount(), 2); - EXPECT_EQ(r12.ColCount(), 1); - EXPECT_FLOAT_EQ(r12.Get(0, 0), 1.0F); - EXPECT_FLOAT_EQ(r12.Get(0, 1), 2.0F); -} +// m12.Transpose(&r12); +// EXPECT_EQ(r12.RowCount(), 2); +// EXPECT_EQ(r12.ColCount(), 1); +// EXPECT_FLOAT_EQ(r12.Get(0, 0), 1.0F); +// EXPECT_FLOAT_EQ(r12.Get(0, 1), 2.0F); +// } -TEST(MatrixSingle, Multiply) { - float m12data[] = {1.0F, 2.0F}; - MatrixOf m12 = MatrixOf(1, 2, m12data); +// TEST(MatrixSingle, Multiply) { +// float m12data[] = {1.0F, 2.0F}; +// MatrixOf m12 = MatrixOf(1, 2, m12data); - EXPECT_EQ(m12.RowCount(), 1); - EXPECT_EQ(m12.ColCount(), 2); - EXPECT_FLOAT_EQ(m12.Get(0, 0), 1.0F); - EXPECT_FLOAT_EQ(m12.Get(0, 1), 2.0F); +// EXPECT_EQ(m12.RowCount(), 1); +// EXPECT_EQ(m12.ColCount(), 2); +// EXPECT_FLOAT_EQ(m12.Get(0, 0), 1.0F); +// EXPECT_FLOAT_EQ(m12.Get(0, 1), 2.0F); - float m21data[] = {3.0F, 4.0F}; - MatrixOf m21 = MatrixOf(2, 1, m21data); +// float m21data[] = {3.0F, 4.0F}; +// MatrixOf m21 = MatrixOf(2, 1, m21data); - EXPECT_EQ(m21.RowCount(), 2); - EXPECT_EQ(m21.ColCount(), 1); - EXPECT_FLOAT_EQ(m21.Get(0, 0), 3.0F); - EXPECT_FLOAT_EQ(m21.Get(1, 0), 4.0F); +// EXPECT_EQ(m21.RowCount(), 2); +// EXPECT_EQ(m21.ColCount(), 1); +// EXPECT_FLOAT_EQ(m21.Get(0, 0), 3.0F); +// EXPECT_FLOAT_EQ(m21.Get(1, 0), 4.0F); - float r11data[] = {0.0F}; - MatrixOf r11 = MatrixOf(1, 1, r11data); +// float r11data[] = {0.0F}; +// MatrixOf r11 = MatrixOf(1, 1, r11data); - EXPECT_EQ(r11.RowCount(), 1); - EXPECT_EQ(r11.ColCount(), 1); +// EXPECT_EQ(r11.RowCount(), 1); +// EXPECT_EQ(r11.ColCount(), 1); - MatrixOf::Multiply(&m12, &m21, &r11); - EXPECT_EQ(r11.RowCount(), 1); - EXPECT_EQ(r11.ColCount(), 1); - EXPECT_FLOAT_EQ(r11.Get(0, 0), 11.0F); +// MatrixOf::Multiply(&m12, &m21, &r11); +// EXPECT_EQ(r11.RowCount(), 1); +// EXPECT_EQ(r11.ColCount(), 1); +// EXPECT_FLOAT_EQ(r11.Get(0, 0), 11.0F); - float r22data[] = {0.0F, 0.0F, 0.0F, 0.0F}; - MatrixOf r22 = MatrixOf(2, 2, r22data); +// float r22data[] = {0.0F, 0.0F, 0.0F, 0.0F}; +// MatrixOf r22 = MatrixOf(2, 2, r22data); - MatrixOf::Multiply(&m21, &m12, &r22); - EXPECT_EQ(r22.RowCount(), 2); - EXPECT_EQ(r22.ColCount(), 2); - EXPECT_FLOAT_EQ(r22.Get(0, 0), 3.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, 1), 8.0F); -} +// MatrixOf::Multiply(&m21, &m12, &r22); +// EXPECT_EQ(r22.RowCount(), 2); +// EXPECT_EQ(r22.ColCount(), 2); +// EXPECT_FLOAT_EQ(r22.Get(0, 0), 3.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, 1), 8.0F); +// } -TEST(MatrixSingle, Multiply_Vector3) { - Vector3 v = Vector3(1.0, 2.0, 3.0); - Vector3 r = Vector3::zero; +// TEST(MatrixSingle, Multiply_Vector3) { +// Vector3 v = Vector3(1.0, 2.0, 3.0); +// Vector3 r = Vector3::zero; - // float m13data[] = {3.0, 4.0, 5.0}; - // MatrixOf m13 = MatrixOf(1, 3, m13data); - // Vector3 r = MatrixOf::Multiply(&m13, v); +// // float m13data[] = {3.0, 4.0, 5.0}; +// // MatrixOf m13 = MatrixOf(1, 3, m13data); +// // Vector3 r = MatrixOf::Multiply(&m13, v); - float m33data[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; - MatrixOf m33 = MatrixOf(3, 3, m33data); - r = MatrixOf::Multiply(&m33, v); - EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(1.0f, 2.0f, 3.0f)), 0); -} +// float m33data[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; +// MatrixOf m33 = MatrixOf(3, 3, m33data); +// r = MatrixOf::Multiply(&m33, v); +// EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(1.0f, 2.0f, 3.0f)), 0); +// } -#endif \ No newline at end of file +// #endif \ No newline at end of file diff --git a/Participant.cpp b/Participant.cpp index ba3c46e..d219ecc 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -19,34 +19,9 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) { Participant::Participant() { Thing::CreateRoot(this); - //this->Add(this->root); } -/* -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; -} +Participant::~Participant() {} void Participant::Update(bool recurse) { for (Thing* thing : this->things) { @@ -56,48 +31,20 @@ void Participant::Update(bool recurse) { } bool Participant::Send(IMessage* msg) { - std::cout << "sending message " << (static_cast(this->buffer[0]) & 0xff) - << " to base Participant without communcation support " << std::endl; + std::cout << "sending message " << (static_cast(this->buffer[0]) & 0xff) + << " to base Participant without communcation support " + << std::endl; return true; } -/* -bool Participant::Send(IMessage* msg) { - int bufferSize = msg->Serialize(this->buffer); - if (bufferSize <= 0) - return true; - - // std::cout << "send msg " << (static_cast(this->buffer[0]) & 0xff) - // << " to " << this->ipAddress << std::endl; - -#if defined(_WIN32) || defined(_WIN64) - Windows::ParticipantUDP* thisWindows = - static_cast(this); - return thisWindows->SendTo(this, bufferSize); -#elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = static_cast(this); - return thisPosix->SendTo(this, bufferSize); -#elif defined(ARDUINO) - Arduino::ParticipantUDP* thisArduino = - static_cast(this); - return thisArduino->SendTo(this, bufferSize); -#elif defined(IDF_VER) - EspIdf::ParticipantUDP* thisEspIdf = - static_cast(this); - return thisEspIdf->SendTo(this, bufferSize); -#else - return false; -#endif -} -*/ Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { for (Thing* thing : this->things) { if (thing->owner->networkId == networkId && thing->id == thingId) return thing; } - std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port - << "[" << (int)thingId << "]\n"; + std::cout << "Could not find thing " << "[" << (int)networkId << ": " + << (int)thingId << "]\n"; return nullptr; } @@ -119,9 +66,8 @@ void Participant::Add(Thing* thing, bool checkId) { thing->id = highestIx + 1; this->things.push_back(thing); #endif - std::cout << "Add thing with generated ID " - //<< this->ipAddress << ":" << this->port - << "[" << (int)thing->id << "]\n"; + std::cout << "Add thing with generated ID " + << "[" << (int)networkId << ": " << (int)thing->id << "]\n"; } else { Thing* foundThing = Get(thing->owner->networkId, thing->id); if (foundThing == nullptr) { @@ -130,13 +76,11 @@ void Participant::Add(Thing* thing, bool checkId) { #else this->things.push_back(thing); #endif - std::cout << "Add thing " << //this->ipAddress << ":" << this->port << - "[" - << (int)thing->id << "]\n"; + std::cout << "Add thing [" << (int)networkId << ": " << (int)thing->id + << "]\n"; } else { - std::cout << "Did not add, existing thing " - //<< this->ipAddress << ":" << this->port - << "[" << (int)thing->id << "]\n"; + std::cout << "Did not add, existing thing " + << "[" << (int)networkId << ": " << (int)thing->id << "]\n"; } } } @@ -164,88 +108,4 @@ void Participant::Remove(Thing* thing) { #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& ParticipantRegistry::GetAll() const { -// return ParticipantRegistry::participants; -// } -// #endif - -// #pragma endregion ParticipantRegistry - } // namespace RoboidControl diff --git a/Participant.h b/Participant.h index d3797dc..84c88d5 100644 --- a/Participant.h +++ b/Participant.h @@ -7,55 +7,6 @@ namespace RoboidControl { 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& GetAll() const; - - private: - /// @brief The list of known participants - std::list participants; -#endif -}; -*/ - /// @brief A participant is a device which manages things. /// It can communicate with other participant to synchronise the state of /// things. This class is used to register the things the participant is @@ -137,12 +88,6 @@ class Participant { #pragma endregion Send -// #pragma region Participant Registry - -// public: -// static ParticipantRegistry registry; - -// #pragma endregion Participant Registry }; } // namespace RoboidControl diff --git a/Participants/ParticipantMQTT.cpp b/Participants/ParticipantMQTT.cpp index 4a3b6f3..cd960f4 100644 --- a/Participants/ParticipantMQTT.cpp +++ b/Participants/ParticipantMQTT.cpp @@ -13,9 +13,9 @@ namespace RoboidControl { MQTTParticipantBase::MQTTParticipantBase(const char* ipAddress, int port) - : RemoteParticipantUDP("127.0.0.1", port) { + : ParticipantSocket("127.0.0.1", port) { this->name = "ParticipantUDP"; - this->remoteSite = new RemoteParticipantUDP(ipAddress, port); + this->remoteSite = new ParticipantSocket(ipAddress, port); // Participant::registry.Add(this); this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; @@ -33,11 +33,120 @@ void MQTTParticipantBase::SetupTCP() { #define MQTT_CONNECT 0x10 #define MQTT_CONNECT_ACK 0x20 -void MQTTParticipantBase::send_mqtt_connect(const char* client_id) { - MQTTConnectPacket packet; + // MQTT CONNECT packet construction + std::vector createConnectPacket(const std::string& clientId) { + std::vector 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(clientId.length())); + packet.insert(packet.end(), clientId.begin(), clientId.end()); + + return packet; + } + + +void MQTTParticipantBase::send_mqtt_connect(const char* clientId) { + + std::vector 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(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; packet.fixed_header = MQTT_CONNECT; packet.remaining_length = // 14; - 2 + 4 + 2 + strlen(client_id) + + 2 + 4 + 2 + strlen(clientId) + 3; // Protocol name + protocol level + connect // flags + keep alive + client ID 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.keep_alive & 0xFF; // LSB 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 & 0xFF; // LSB - memcpy(&this->buffer[index], client_id, client_id_length); + memcpy(&this->buffer[index], clientId, client_id_length); index += client_id_length; 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 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 createSubscribePacket(const char* topic, uint8_t qos) { + std::vector 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) { + std::vector 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) static unsigned short packetId = 1; // Increment this for each new subscription @@ -109,6 +277,7 @@ void MQTTParticipantBase::sendSubscribe(const char* topic) { // Send the SUBSCRIBE packet SendTCP(7 + topicLength); + */ std::cout << "Send subscribe to topic: " << topic << std::endl; } @@ -128,7 +297,7 @@ int MQTTParticipantBase::ReceiveTCP() { void MQTTParticipantBase::ReceiveData(unsigned char bufferSize) { std::cout << " receivemsg\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) { diff --git a/Participants/ParticipantMQTT.h b/Participants/ParticipantMQTT.h index 76dd935..0190def 100644 --- a/Participants/ParticipantMQTT.h +++ b/Participants/ParticipantMQTT.h @@ -43,7 +43,7 @@ namespace RoboidControl { /// participant is created which can be obtained using /// RoboidControl::IsolatedParticipant::Isolated(). /// @sa RoboidControl::Thing::Thing() -class MQTTParticipantBase : public RemoteParticipantUDP { +class MQTTParticipantBase : public ParticipantSocket { #pragma region Init public: @@ -60,7 +60,7 @@ protected: public: /// @brief The remote site when this participant is connected to a site - RemoteParticipantUDP* remoteSite = nullptr; + ParticipantSocket* remoteSite = nullptr; protected: @@ -127,4 +127,5 @@ protected: } // namespace RoboidControl -#include "Posix/PosixMQTT.h" \ No newline at end of file +#include "Posix/PosixMQTT.h" +#include "Windows/WindowsMQTT.h" \ No newline at end of file diff --git a/Participants/ParticipantSocket.cpp b/Participants/ParticipantSocket.cpp new file mode 100644 index 0000000..9b5c224 --- /dev/null +++ b/Participants/ParticipantSocket.cpp @@ -0,0 +1,24 @@ +#include "ParticipantSocket.h" + +#include + +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; +} + +} \ No newline at end of file diff --git a/Participants/ParticipantSocket.h b/Participants/ParticipantSocket.h new file mode 100644 index 0000000..82b8678 --- /dev/null +++ b/Participants/ParticipantSocket.h @@ -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; +}; + +} \ No newline at end of file diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 4f30b80..1e0f16e 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -12,112 +12,11 @@ #include "Things/DistanceSensor.h" #include "Things/TouchSensor.h" -#include +// #include namespace RoboidControl { -#pragma region ParticipantRegistry - -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& 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) { +bool ParticipantSocket::Send(IMessage* msg) { // No message is actually sent, because this class has no networking // implementation return false; @@ -126,7 +25,7 @@ bool RemoteParticipantUDP::Send(IMessage* msg) { #pragma region Init ParticipantUDPGeneric::ParticipantUDPGeneric(int port) - : RemoteParticipantUDP("127.0.0.1", port) { + : ParticipantSocket("127.0.0.1", port) { this->name = "ParticipantUDP"; this->remoteSite = nullptr; if (this->port == 0) @@ -145,12 +44,12 @@ ParticipantUDPGeneric::ParticipantUDPGeneric(int port) ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress, int port, int localPort) - : RemoteParticipantUDP("127.0.0.1", localPort) { + : ParticipantSocket("127.0.0.1", localPort) { this->name = "ParticipantUDP"; if (this->port == 0) this->isIsolated = true; else - this->remoteSite = new RemoteParticipantUDP(ipAddress, port); + this->remoteSite = new ParticipantSocket(ipAddress, port); registry.Add(this); this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; @@ -196,7 +95,7 @@ void ParticipantUDPGeneric::Update(bool recurse) { this->nextPublishMe = currentTimeMs + this->publishInterval; } - //this->ReceiveUDP(); + this->ReceiveUDP(); } UpdateMyThings(); @@ -208,7 +107,7 @@ void ParticipantUDPGeneric::UpdateMyThings() { if (thing == nullptr) // || thing->GetParent() != nullptr) continue; - // Why don't we do recursive? + // Why don't we do recursive? // Because when a thing creates a thing in the update, // that new thing is not sent out (because of hierarchyChanged) // before it is updated itself: it is immediatedly updated! @@ -311,8 +210,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize, unsigned int senderPort) { // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort // << std::endl; - RemoteParticipantUDP* sender = - this->registry.Get(senderIpAddress, senderPort); + ParticipantSocket* sender = this->registry.Get(senderIpAddress, senderPort); if (sender == nullptr) { sender = this->registry.Add(senderIpAddress, senderPort); #if !defined(NO_STD) @@ -325,7 +223,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize, } void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize, - RemoteParticipantUDP* sender) { + ParticipantSocket* sender) { unsigned char msgId = this->buffer[0]; // std::cout << "receive msg " << (int)msgId << "\n"; // std::cout << " buffer size = " <<(int) bufferSize << "\n"; @@ -398,7 +296,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize, #endif } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, ParticipantMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId @@ -406,7 +304,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, #endif } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, NetworkIdMsg* msg) { #if defined(DEBUG) 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) { #if defined(DEBUG) std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId @@ -430,16 +328,15 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, #endif } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, - ThingMsg* msg) { +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, ThingMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " << (int)msg->parentId << "\n"; #endif - RemoteParticipantUDP* owner = registry.Get(msg->networkId); + ParticipantSocket* owner = registry.Get(msg->networkId); if (owner == nullptr) { - owner = new RemoteParticipantUDP(sender->ipAddress, sender->port); + owner = new ParticipantSocket(sender->ipAddress, sender->port); owner->networkId = msg->networkId; registry.Add(owner); } @@ -461,7 +358,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, thing->SetParent(nullptr); } -Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner, +Thing* ParticipantUDPGeneric::ProcessNewThing(ParticipantSocket* owner, ThingMsg* msg, bool isRemote) { switch (msg->thingType) { @@ -476,8 +373,7 @@ Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner, } } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, - NameMsg* msg) { +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, NameMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/" << (int)msg->thingId << "] "; @@ -509,7 +405,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, #endif } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, ModelUrlMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId @@ -517,8 +413,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, #endif } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, - PoseMsg* msg) { +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, PoseMsg* msg) { #if !defined(DEBUG) && !defined(NO_STD) std::cout << this->name << ": process PoseMsg [" << (int)this->networkId << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; @@ -541,8 +436,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, thing->SetAngularVelocity(msg->angularVelocity); } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, - BinaryMsg* msg) { +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, BinaryMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n"; @@ -564,15 +458,14 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, } } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, - TextMsg* msg) { +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, TextMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " " << (int)msg->text << "\n"; #endif } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, DestroyMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/" @@ -587,4 +480,87 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, // Receive #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& ParticipantUDP::Registry::GetAll() const { + return Registry::participants; +} +#endif + +// Registry +#pragma endregion Registry + } // namespace RoboidControl diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 86eba25..dfd3e84 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -10,7 +10,8 @@ #include "Messages/PoseMsg.h" #include "Messages/TextMsg.h" #include "Messages/ThingMsg.h" -#include "Participant.h" + +#include "ParticipantSocket.h" #if !defined(NO_STD) #include @@ -31,74 +32,6 @@ namespace RoboidControl { 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& GetAll() const; - - private: - /// @brief The list of known participants - std::list participants; -#endif -}; - /// @brief A participant using UDP communication /// A local participant is the local device which can communicate with /// 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 /// RoboidControl::IsolatedParticipant::Isolated(). /// @sa RoboidControl::Thing::Thing() -class ParticipantUDPGeneric : public RemoteParticipantUDP { +class ParticipantUDPGeneric : public ParticipantSocket { #pragma region Init public: @@ -139,7 +72,7 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP { bool isIsolated = false; /// @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 /// local network @@ -192,36 +125,86 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP { void ReceiveData(unsigned char bufferSize, char* senderIpAddress, 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 Process(RemoteParticipantUDP* sender, ParticipantMsg* msg); - virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg); - virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg); + virtual void Process(ParticipantSocket* sender, ParticipantMsg* msg); + virtual void Process(ParticipantSocket* sender, NetworkIdMsg* msg); + virtual void Process(ParticipantSocket* sender, InvestigateMsg* msg); - virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg); - virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender, + virtual void Process(ParticipantSocket* sender, ThingMsg* msg); + virtual Thing* ProcessNewThing(ParticipantSocket* sender, ThingMsg* msg, bool isRemote); - virtual void Process(RemoteParticipantUDP* sender, NameMsg* msg); - virtual void Process(RemoteParticipantUDP* sender, ModelUrlMsg* msg); - virtual void Process(RemoteParticipantUDP* sender, PoseMsg* msg); - virtual void Process(RemoteParticipantUDP* sender, BinaryMsg* msg); - virtual void Process(RemoteParticipantUDP* sender, TextMsg* msg); - virtual void Process(RemoteParticipantUDP* sender, DestroyMsg* msg); + virtual void Process(ParticipantSocket* sender, NameMsg* msg); + virtual void Process(ParticipantSocket* sender, ModelUrlMsg* msg); + virtual void Process(ParticipantSocket* sender, PoseMsg* msg); + virtual void Process(ParticipantSocket* sender, BinaryMsg* msg); + virtual void Process(ParticipantSocket* sender, TextMsg* msg); + virtual void Process(ParticipantSocket* sender, DestroyMsg* msg); #pragma endregion Receive -public: - static ParticipantRegistry registry; + public: + /// @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& GetAll() const; + + private: + /// @brief The list of known participants + std::list participants; +#endif + }; + + static Registry registry; }; } // namespace RoboidControl #include "EspIdf/EspIdfParticipant.h" #include "Posix/PosixParticipant.h" +#include "Windows/WindowsParticipant.h" \ No newline at end of file diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 42f5da9..14808f9 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -56,7 +56,7 @@ void SiteServer::UpdateMyThings() { #pragma region Receive -void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) { +void SiteServer::Process(ParticipantSocket* sender, ParticipantMsg* msg) { if (msg->networkId != sender->networkId) { // std::cout << this->name << " received New Client -> " << // 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); if (thing == nullptr) { // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index 1532843..ea21882 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -33,9 +33,9 @@ class SiteServer : public ParticipantUDP { protected: unsigned long nextPublishMe = 0; - virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) override; - virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override; - virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override; + virtual void Process(ParticipantSocket* sender, ParticipantMsg* msg) override; + virtual void Process(ParticipantSocket* sender, NetworkIdMsg* msg) override; + virtual void Process(ParticipantSocket* sender, ThingMsg* msg) override; #pragma endregion Receive diff --git a/Posix/PosixParticipant.cpp b/Posix/PosixParticipant.cpp index 6b8d98d..f431627 100644 --- a/Posix/PosixParticipant.cpp +++ b/Posix/PosixParticipant.cpp @@ -128,32 +128,7 @@ void ParticipantUDP::ReceiveUDP() { #endif } -// int ParticipantUDP::ReceiveTCP() { -// #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) { +bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) { #if defined(__unix__) || defined(__APPLE__) // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << // ntohs(remoteParticipant->port) diff --git a/Posix/PosixParticipant.h b/Posix/PosixParticipant.h index 8002ea0..bb84508 100644 --- a/Posix/PosixParticipant.h +++ b/Posix/PosixParticipant.h @@ -29,7 +29,7 @@ class ParticipantUDP : public ParticipantUDPGeneric { // void Receive(); int ReceiveTCP(); - bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); + bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize); bool Publish(IMessage* msg) override; protected: diff --git a/Windows/WindowsMQTT.cpp b/Windows/WindowsMQTT.cpp new file mode 100644 index 0000000..f20eee1 --- /dev/null +++ b/Windows/WindowsMQTT.cpp @@ -0,0 +1,102 @@ +#include "WindowsMQTT.h" + +#if defined(_WIN32) || defined(_WIN64) + +#include +#include + +#include +#include + +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 \ No newline at end of file diff --git a/Windows/WindowsMQTT.h b/Windows/WindowsMQTT.h new file mode 100644 index 0000000..9587a52 --- /dev/null +++ b/Windows/WindowsMQTT.h @@ -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 \ No newline at end of file diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index b2b8cdd..3c6fd17 100644 --- a/Windows/WindowsParticipant.cpp +++ b/Windows/WindowsParticipant.cpp @@ -9,9 +9,12 @@ 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, int remotePort) { #if defined(_WIN32) || defined(_WIN64) @@ -128,7 +131,7 @@ void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) { #endif // _WIN32 || _WIN64 } -void ParticipantUDP::Receive() { +void ParticipantUDP::ReceiveUDP() { #if defined(_WIN32) || defined(_WIN64) // char 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; } -bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) { #if defined(_WIN32) || defined(_WIN64) char ip_str[INET_ADDRSTRLEN]; inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN); diff --git a/Windows/WindowsParticipant.h b/Windows/WindowsParticipant.h index c0caf2d..b452f7e 100644 --- a/Windows/WindowsParticipant.h +++ b/Windows/WindowsParticipant.h @@ -7,14 +7,15 @@ namespace RoboidControl { class ParticipantUDP : public ParticipantUDPGeneric { public: - ParticipantUDP(); - void Setup(int localPort, const char* remoteIpAddress, int remotePort); + ParticipantUDP(int port = 7681); + 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); - bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); + bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize); bool SendTCP(int bufferSize); bool Publish(IMessage* msg); - void Receive(); + void ReceiveUDP(); int ReceiveTCP(); protected: