From a826817a0bc405391264e4857bda1703ebf64ac9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 30 Jun 2025 08:46:39 +0200 Subject: [PATCH 01/12] RemoteParticiapntUDP -> ParticipantSocket --- Arduino/ArduinoParticipant.cpp | 2 +- Arduino/ArduinoParticipant.h | 2 +- EspIdf/EspIdfParticipant.cpp | 2 +- EspIdf/EspIdfParticipant.h | 2 +- Participants/ParticipantSocket.cpp | 24 ++++++++++ Participants/ParticipantSocket.h | 28 +++++++++++ Participants/ParticipantUDP.cpp | 75 ++++++++++++------------------ Participants/ParticipantUDP.h | 68 ++++++++++----------------- Participants/SiteServer.cpp | 6 +-- Participants/SiteServer.h | 6 +-- Posix/PosixParticipant.cpp | 2 +- Posix/PosixParticipant.h | 2 +- Windows/WindowsParticipant.cpp | 2 +- Windows/WindowsParticipant.h | 2 +- 14 files changed, 120 insertions(+), 103 deletions(-) create mode 100644 Participants/ParticipantSocket.cpp create mode 100644 Participants/ParticipantSocket.h 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 44dc0e8..7f26c0b 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -182,7 +182,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/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 66adb7c..0f6581d 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -12,7 +12,7 @@ #include "Things/DistanceSensor.h" #include "Things/TouchSensor.h" -#include +// #include namespace RoboidControl { @@ -20,10 +20,10 @@ namespace RoboidControl { ParticipantRegistry ParticipantUDPGeneric::registry; -RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress, +ParticipantSocket* ParticipantRegistry::Get(const char* ipAddress, unsigned int port) { #if !defined(NO_STD) - for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) { + for (ParticipantSocket* participant : ParticipantRegistry::participants) { if (participant == nullptr) continue; if (strcmp(participant->ipAddress, ipAddress) == 0 && @@ -39,9 +39,9 @@ RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress, return nullptr; } -RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) { +ParticipantSocket* ParticipantRegistry::Get(unsigned char participantId) { #if !defined(NO_STD) - for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) { + for (ParticipantSocket* participant : ParticipantRegistry::participants) { if (participant == nullptr) continue; if (participant->networkId == participantId) @@ -52,14 +52,14 @@ RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) { return nullptr; } -RemoteParticipantUDP* ParticipantRegistry::Add(const char* ipAddress, +ParticipantSocket* ParticipantRegistry::Add(const char* ipAddress, unsigned int port) { - RemoteParticipantUDP* participant = new RemoteParticipantUDP(ipAddress, port); + ParticipantSocket* participant = new ParticipantSocket(ipAddress, port); Add(participant); return participant; } -void ParticipantRegistry::Add(RemoteParticipantUDP* participant) { +void ParticipantRegistry::Add(ParticipantSocket* participant) { Participant* foundParticipant = Get(participant->networkId); // Get(participant->ipAddress, participant->port); @@ -84,7 +84,7 @@ void ParticipantRegistry::Add(RemoteParticipantUDP* participant) { } } -void ParticipantRegistry::Remove(RemoteParticipantUDP* participant) { +void ParticipantRegistry::Remove(ParticipantSocket* participant) { // participants.remove(participant); } @@ -93,31 +93,14 @@ RemoteParticipantUDP** ParticipantRegistry::GetAll() const { return ParticipantRegistry::participants; } #else -const std::list& ParticipantRegistry::GetAll() const { +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 +109,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 +128,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; @@ -311,7 +294,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize, unsigned int senderPort) { // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort // << std::endl; - RemoteParticipantUDP* sender = + ParticipantSocket* sender = this->registry.Get(senderIpAddress, senderPort); if (sender == nullptr) { sender = this->registry.Add(senderIpAddress, senderPort); @@ -325,7 +308,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 +381,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 +389,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 +405,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 +413,16 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, #endif } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, +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 +444,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,7 +459,7 @@ Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner, } } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, NameMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/" @@ -509,7 +492,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,7 +500,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, #endif } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, PoseMsg* msg) { #if !defined(DEBUG) && !defined(NO_STD) std::cout << this->name << ": process PoseMsg [" << (int)this->networkId @@ -541,7 +524,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, thing->SetAngularVelocity(msg->angularVelocity); } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, BinaryMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId @@ -564,7 +547,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, } } -void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, +void ParticipantUDPGeneric::Process(ParticipantSocket* sender, TextMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " " @@ -572,7 +555,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, #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 << "/" diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 159062e..ffca6ac 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,26 +32,7 @@ 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 { @@ -59,43 +41,43 @@ class ParticipantRegistry { /// @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); + 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 - RemoteParticipantUDP* Get(unsigned char networkID); + 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 - RemoteParticipantUDP* Add(const char* ipAddress, unsigned int port); + ParticipantSocket* Add(const char* ipAddress, unsigned int port); /// @brief Add a participant /// @param participant The participant to add - void Add(RemoteParticipantUDP* participant); + void Add(ParticipantSocket* participant); /// @brief Remove a participant /// @param participant The participant to remove - void Remove(RemoteParticipantUDP* participant); + void Remove(ParticipantSocket* participant); private: #if defined(NO_STD) public: - RemoteParticipantUDP** GetAll() const; + ParticipantSocket** GetAll() const; int count = 0; private: - RemoteParticipantUDP** participants; + ParticipantSocket** participants; #else public: /// @brief Get all participants /// @return All participants - const std::list& GetAll() const; + const std::list& GetAll() const; private: /// @brief The list of known participants - std::list participants; + std::list participants; #endif }; @@ -111,7 +93,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 +121,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,27 +174,27 @@ 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 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 diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 9baa03e..2e6ee30 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 97de7bf..343abee 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -33,9 +33,9 @@ class SiteServer : public ParticipantUDPGeneric { 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 6ae6cef..8c11e98 100644 --- a/Posix/PosixParticipant.cpp +++ b/Posix/PosixParticipant.cpp @@ -90,7 +90,7 @@ void ParticipantUDP::Receive() { #endif } -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) // << "\n"; diff --git a/Posix/PosixParticipant.h b/Posix/PosixParticipant.h index 17dd23f..b4f85c8 100644 --- a/Posix/PosixParticipant.h +++ b/Posix/PosixParticipant.h @@ -9,7 +9,7 @@ class ParticipantUDP : public ParticipantUDPGeneric { public: 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); protected: diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index dfaa445..49d14e5 100644 --- a/Windows/WindowsParticipant.cpp +++ b/Windows/WindowsParticipant.cpp @@ -102,7 +102,7 @@ void ParticipantUDP::Receive() { #endif // _WIN32 || _WIN64 } -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 3d241ba..9ddc7cf 100644 --- a/Windows/WindowsParticipant.h +++ b/Windows/WindowsParticipant.h @@ -9,7 +9,7 @@ class ParticipantUDP : public ParticipantUDPGeneric { public: 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); protected: From a7a25bde6e64ed656196674f11770d83aa737045 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 30 Jun 2025 09:52:13 +0200 Subject: [PATCH 02/12] Added gyroscope --- Participant.cpp | 164 ++------------------------- Participant.h | 55 ---------- Participants/ParticipantUDP.cpp | 189 +++++++++++++++----------------- Participants/ParticipantUDP.h | 106 +++++++++--------- 4 files changed, 156 insertions(+), 358 deletions(-) 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/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 0f6581d..68d41e7 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -16,90 +16,6 @@ namespace RoboidControl { -#pragma region ParticipantRegistry - -ParticipantRegistry ParticipantUDPGeneric::registry; - -ParticipantSocket* ParticipantRegistry::Get(const char* ipAddress, - unsigned int port) { -#if !defined(NO_STD) - for (ParticipantSocket* 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; -} - -ParticipantSocket* ParticipantRegistry::Get(unsigned char participantId) { -#if !defined(NO_STD) - for (ParticipantSocket* 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; -} - -ParticipantSocket* ParticipantRegistry::Add(const char* ipAddress, - unsigned int port) { - ParticipantSocket* participant = new ParticipantSocket(ipAddress, port); - Add(participant); - return participant; -} - -void ParticipantRegistry::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 - 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(ParticipantSocket* 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 - bool ParticipantSocket::Send(IMessage* msg) { // No message is actually sent, because this class has no networking // implementation @@ -179,7 +95,7 @@ void ParticipantUDPGeneric::Update(bool recurse) { this->nextPublishMe = currentTimeMs + this->publishInterval; } - //this->ReceiveUDP(); + this->ReceiveUDP(); } UpdateMyThings(); @@ -191,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! @@ -294,8 +210,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize, unsigned int senderPort) { // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort // << std::endl; - ParticipantSocket* 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) @@ -413,8 +328,7 @@ void ParticipantUDPGeneric::Process(ParticipantSocket* sender, #endif } -void ParticipantUDPGeneric::Process(ParticipantSocket* 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 << " " @@ -459,8 +373,7 @@ Thing* ParticipantUDPGeneric::ProcessNewThing(ParticipantSocket* owner, } } -void ParticipantUDPGeneric::Process(ParticipantSocket* 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 << "] "; @@ -500,8 +413,7 @@ void ParticipantUDPGeneric::Process(ParticipantSocket* sender, #endif } -void ParticipantUDPGeneric::Process(ParticipantSocket* 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"; @@ -524,8 +436,7 @@ void ParticipantUDPGeneric::Process(ParticipantSocket* sender, thing->SetAngularVelocity(msg->angularVelocity); } -void ParticipantUDPGeneric::Process(ParticipantSocket* 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"; @@ -547,8 +458,7 @@ void ParticipantUDPGeneric::Process(ParticipantSocket* sender, } } -void ParticipantUDPGeneric::Process(ParticipantSocket* 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"; @@ -570,4 +480,87 @@ void ParticipantUDPGeneric::Process(ParticipantSocket* 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 ffca6ac..6ad8956 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -32,55 +32,6 @@ namespace RoboidControl { constexpr int MAX_SENDER_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 - 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 -}; - /// @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 @@ -174,9 +125,12 @@ class ParticipantUDPGeneric : public ParticipantSocket { void ReceiveData(unsigned char bufferSize, char* senderIpAddress, unsigned int senderPort); - void ReceiveData(unsigned char bufferSize, ParticipantSocket* 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; @@ -198,9 +152,55 @@ class ParticipantUDPGeneric : public ParticipantSocket { #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 From abeef72514beb68af8ec3b2b9db56f3c0c3aab29 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 30 Jun 2025 15:44:45 +0200 Subject: [PATCH 03/12] Somewhere on the way to porting the camera keypoints processing --- LinearAlgebra/Matrix.cpp | 35 +++++++++++++++++++++++++++- LinearAlgebra/Matrix.h | 49 +++++++++++++++++++++++++++++++++++---- LinearAlgebra/Vector2.cpp | 10 ++++++++ LinearAlgebra/Vector2.h | 11 +++++++++ 4 files changed, 100 insertions(+), 5 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index c7e53c7..5fa6fcd 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,5 +1,5 @@ #include "Matrix.h" -#if !defined(NO_STD) +#if !defined(NO_STD) #include #endif @@ -7,6 +7,8 @@ namespace LinearAlgebra { #pragma region Matrix1 +Matrix1::Matrix1() {} + Matrix1::Matrix1(int size) : size(size) { if (this->size == 0) data = nullptr; @@ -257,6 +259,37 @@ void Matrix2::UpdateSlice(int rowStart, } } +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..fcb8055 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -12,12 +12,33 @@ class Matrix1 { float* data = nullptr; int size = 0; + Matrix1(); Matrix1(int size); Matrix1(float* data, int size); static Matrix1 FromQuaternion(Quaternion q); Quaternion ToQuaternion(); + Matrix1 Slice(int start, int stop); + + void Set(unsigned int start, unsigned int stop, float value) { + if (stop > this->size) + stop = this->size; + + for (unsigned int ix = start; ix < stop; ix++) + this->data[ix] = value; + } + void Set(unsigned int start, unsigned int stop, Matrix1 source) { + if (stop > this->size) + stop = this->size; + if (stop > source.size) + stop = source.size; + // Do I need to check on start??? + unsigned int sourceIx = 0; + for (unsigned int ix = start; ix < stop; ix++, sourceIx++) + this->data[ix] = source.data[sourceIx]; + } + private: bool externalData = true; }; @@ -96,13 +117,33 @@ class Matrix2 { return r; } + 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; + } + Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop); void UpdateSlice(int rowStart, - int rowStop, - int colStart, - int colStop, - const Matrix2& m) const; + int rowStop, + int colStart, + int colStop, + const Matrix2& m) const; + + Matrix2 DeleteRows(int rowStart, int rowStop); + Matrix2 DeleteColumns(int colStart, int colStop); + // private: // move constructor and move assignment operator Matrix2(Matrix2&& other) noexcept; 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; From 7c06bc2122c99b64e006ed2938d1fd5c2969ee9e Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 30 Jun 2025 18:14:34 +0200 Subject: [PATCH 04/12] More code ported --- LinearAlgebra/Matrix.cpp | 77 ++++++++++++++++++++++++++++++++++++++++ LinearAlgebra/Matrix.h | 21 ++++------- 2 files changed, 83 insertions(+), 15 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index 5fa6fcd..78d3849 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -194,6 +194,13 @@ Matrix2 Matrix2::operator+=(const Matrix2& v) { return *this; } +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; +} + Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { Matrix2 r = Matrix2(this->nRows, B.nCols); @@ -223,6 +230,23 @@ 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; +} + Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) { Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart); @@ -259,6 +283,59 @@ 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, 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); diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index fcb8055..5c7bcdc 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -80,6 +80,8 @@ class Matrix2 { Matrix2 operator+(const Matrix2& v) const; 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) { Matrix2 r = Matrix2(m.nRows, m.nCols); @@ -117,22 +119,9 @@ class Matrix2 { return r; } - 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; - } + Matrix2 GetRows(int from, int to); + void SetRow(int rowIx, Matrix1 source) {} Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop); void UpdateSlice(int rowStart, @@ -141,6 +130,8 @@ class Matrix2 { int colStop, const Matrix2& m) const; + Matrix2 Inverse(); + Matrix2 DeleteRows(int rowStart, int rowStop); Matrix2 DeleteColumns(int colStart, int colStop); From 56c0f92ba4a165613d6b1bc0786e482241cfab78 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 1 Jul 2025 17:13:05 +0200 Subject: [PATCH 05/12] It compiles --- LinearAlgebra/Matrix.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index 78d3849..edde63e 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -36,6 +36,10 @@ 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) { + return Matrix1(); +} + // Matrix1 #pragma endregion @@ -289,7 +293,8 @@ Matrix2 Matrix2::Inverse() { 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, 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; } From bc79111c07e799a76b346f2f0451a7b5bb8bda56 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 1 Jul 2025 18:01:18 +0200 Subject: [PATCH 06/12] Added partial impl. triangulation --- LinearAlgebra/Matrix.cpp | 34 +++++++++++++++++++++++++++++++++- LinearAlgebra/Matrix.h | 7 ++++++- LinearAlgebra/Quaternion.cpp | 4 ++++ LinearAlgebra/Quaternion.h | 2 ++ 4 files changed, 45 insertions(+), 2 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index edde63e..73b18c8 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -22,6 +22,10 @@ Matrix1::Matrix1(float* data, int size) : data(data), size(size) { this->externalData = true; } +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; @@ -37,7 +41,15 @@ Quaternion LinearAlgebra::Matrix1::ToQuaternion() { } Matrix1 Matrix1::Slice(int start, int stop) { - return Matrix1(); + 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; } // Matrix1 @@ -251,6 +263,26 @@ Matrix2 Matrix2::GetRows(int from, int to) { 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.data[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); diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index 5c7bcdc..be94054 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -16,6 +16,8 @@ class Matrix1 { Matrix1(int size); Matrix1(float* data, int size); + Vector3 ToVector3(); + static Matrix1 FromQuaternion(Quaternion q); Quaternion ToQuaternion(); @@ -120,8 +122,11 @@ class Matrix2 { } Matrix2 GetRows(int from, int to); + Vector3 GetRow3(int rowIx); + + void SetRow(int rowIx, Matrix1 source); + void SetRow3(int rowIx, Vector3 v); - void SetRow(int rowIx, Matrix1 source) {} Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop); void UpdateSlice(int rowStart, diff --git a/LinearAlgebra/Quaternion.cpp b/LinearAlgebra/Quaternion.cpp index dda8c18..5bb5ecd 100644 --- a/LinearAlgebra/Quaternion.cpp +++ b/LinearAlgebra/Quaternion.cpp @@ -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 /// From 7f0363ed7bf3b22568878f7c4f7c6fd372a50a41 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 2 Jul 2025 12:37:15 +0200 Subject: [PATCH 07/12] Improved matrix access --- LinearAlgebra/Matrix.cpp | 15 +++++++++++++++ LinearAlgebra/Matrix.h | 6 ++++++ 2 files changed, 21 insertions(+) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index 73b18c8..6709b23 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -52,6 +52,13 @@ Matrix1 Matrix1::Slice(int start, int stop) { return result; } +const float& Matrix1::operator()(int ix) const { + return data[ix]; +} +float& Matrix1::operator()(int ix) { + return data[ix]; +} + // Matrix1 #pragma endregion @@ -147,6 +154,14 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) { return r; } +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::Clear() { for (int ix = 0; ix < this->nValues; ix++) this->data[ix] = 0; diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index be94054..de677de 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -21,6 +21,9 @@ class Matrix1 { static Matrix1 FromQuaternion(Quaternion q); Quaternion ToQuaternion(); + const float& operator()(int ix) const; + float& operator()(int ix); + Matrix1 Slice(int start, int stop); void Set(unsigned int start, unsigned int stop, float value) { @@ -66,6 +69,9 @@ class Matrix2 { static Matrix2 Zero(int nRows, int nCols); void Clear(); + const float& operator()(int row, int col) const; + float& operator()(int row, int col); + static Matrix2 Identity(int size); static Matrix2 Diagonal(float f, int size); From fb64bca385424e291798679cb1b296d6c24ce4ab Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 2 Jul 2025 15:03:04 +0200 Subject: [PATCH 08/12] Code is completely ported (I think) --- LinearAlgebra/Matrix.cpp | 13 +++++++++++-- LinearAlgebra/Matrix.h | 4 +++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index 6709b23..e792a36 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -52,6 +52,12 @@ Matrix1 Matrix1::Slice(int start, int stop) { return result; } +void Matrix1::UpdateSlice(int start, int stop, const Matrix1& m) const { + for (int ix = start; ix < stop; ix++) { + this->data[ix] = m.data[ix - start]; + } +} + const float& Matrix1::operator()(int ix) const { return data[ix]; } @@ -76,7 +82,7 @@ Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { } } -Matrix2::Matrix2(float* data, int nRows, int nCols) +Matrix2::Matrix2(int nRows, int nCols, float* data) : nRows(nRows), nCols(nCols), data(data) { this->nValues = nRows * nCols; this->externalData = true; @@ -161,7 +167,6 @@ float& Matrix2::operator()(int row, int col) { return data[row + this->nCols + col]; } - void Matrix2::Clear() { for (int ix = 0; ix < this->nValues; ix++) this->data[ix] = 0; @@ -311,6 +316,10 @@ 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, diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index de677de..9f4d92b 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -25,6 +25,7 @@ class Matrix1 { float& operator()(int ix); Matrix1 Slice(int start, int stop); + void UpdateSlice(int start, int stop, const Matrix1& m) const; void Set(unsigned int start, unsigned int stop, float value) { if (stop > this->size) @@ -58,7 +59,7 @@ class Matrix2 { Matrix2(); Matrix2(int nRows, int nCols); - Matrix2(float* data, int nRows, int nCols); + Matrix2(int nRows, int nCols, float* data); Matrix2(const Matrix2& m); Matrix2& operator=(const Matrix2& other); @@ -135,6 +136,7 @@ class Matrix2 { 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, From ca5abf097bdef5919c4b105462342e1d2e865378 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 2 Jul 2025 17:20:13 +0200 Subject: [PATCH 09/12] Running better, but crashing because memory is filling up... --- LinearAlgebra/Matrix.cpp | 46 ++++++++++++++++++++++++++++++++++------ 1 file changed, 40 insertions(+), 6 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index e792a36..d88c0ee 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -3,6 +3,11 @@ #include #endif +#include "esp_system.h" +#include "esp_log.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + namespace LinearAlgebra { #pragma region Matrix1 @@ -70,15 +75,44 @@ float& Matrix1::operator()(int ix) { #pragma region Matrix2 +// Function to check available heap memory +size_t getAvailableHeapMemory() { + // This is a simple example; you may need to implement a more robust method + // depending on your platform. For ESP32, you can use heap_4.c or similar. + return esp_get_free_heap_size(); // Example for ESP32 +} + Matrix2::Matrix2() {} Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { this->nValues = nRows * nCols; - if (this->nValues == 0) + if (this->nValues <= 0) { this->data = nullptr; - else { + this->externalData = false; + return; + } + // Check available heap memory before allocation + size_t availableMemory = getAvailableHeapMemory(); + if (availableMemory < this->nValues * sizeof(float)) { + std::cerr << "Error: Not enough memory to allocate Matrix2 of size " + << this->nValues << " floats." << std::endl; + this->data = nullptr; // Handle memory allocation failure + this->externalData = false; + return; + } + + UBaseType_t stack_size = + uxTaskGetStackHighWaterMark(NULL); // NULL to check the main task + if (stack_size < 1000) + std::cerr << "Stack High Water Mark: " << stack_size << std::endl; + + try { this->data = new float[this->nValues]; this->externalData = false; + } catch (const std::bad_alloc& e) { + std::cerr << "Memory allocation failed: " << e.what() << std::endl; + this->data = nullptr; // Handle memory allocation failure + this->externalData = false; } } @@ -161,10 +195,10 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) { } const float& Matrix2::operator()(int row, int col) const { - return data[row + this->nCols + col]; + return data[row * this->nCols + col]; } float& Matrix2::operator()(int row, int col) { - return data[row + this->nCols + col]; + return data[row * this->nCols + col]; } void Matrix2::Clear() { @@ -285,8 +319,8 @@ Matrix2 Matrix2::GetRows(int from, int to) { Vector3 Matrix2::GetRow3(int rowIx) { int cellIx = rowIx * this->nCols; - Vector3 row(this->data[cellIx, 0], this->data[cellIx, 1], - this->data[cellIx, 2]); + Vector3 row(this->data[cellIx + 0], this->data[cellIx + 1], + this->data[cellIx + 2]); return row; } From c3fa3d595718a9f077214b17727b7b05ad68a5f6 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 3 Jul 2025 12:12:22 +0200 Subject: [PATCH 10/12] Fixed memory leak --- LinearAlgebra/Matrix.cpp | 58 +++++++++++++++++++--------------------- LinearAlgebra/Matrix.h | 9 ++++--- 2 files changed, 32 insertions(+), 35 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index d88c0ee..994b24d 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -3,8 +3,8 @@ #include #endif -#include "esp_system.h" #include "esp_log.h" +#include "esp_system.h" #include "freertos/FreeRTOS.h" #include "freertos/task.h" @@ -86,6 +86,8 @@ Matrix2::Matrix2() {} Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { this->nValues = nRows * nCols; + //printf("Allocate %d\n", this->nValues); + if (this->nValues <= 0) { this->data = nullptr; this->externalData = false; @@ -153,8 +155,10 @@ 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 { @@ -165,27 +169,27 @@ Matrix2 Matrix2::Clone() const { } // 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); @@ -258,7 +262,7 @@ 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; @@ -359,17 +363,9 @@ void Matrix2::UpdateSlice(int rowStart, 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)]; diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index 9f4d92b..8959820 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -87,7 +87,7 @@ 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; @@ -148,10 +148,11 @@ class Matrix2 { Matrix2 DeleteRows(int rowStart, int rowStop); Matrix2 DeleteColumns(int colStart, int colStop); - // private: + // We don't want these because they make impliciti copies which is inefficient + // move constructor and move assignment operator - Matrix2(Matrix2&& other) noexcept; - Matrix2& operator=(Matrix2&& other) noexcept; + //Matrix2(Matrix2&& other) noexcept; + //Matrix2& operator=(Matrix2&& other) noexcept; static Matrix2 Omega(const Vector3& v); From 205dbeb877885f185da971e5f57254854e96cffa Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 3 Jul 2025 13:06:46 +0200 Subject: [PATCH 11/12] Cleanup --- LinearAlgebra/Matrix.cpp | 37 ++++++++++++++++++++++++++++++------- LinearAlgebra/Matrix.h | 33 +++++++++++++++++++-------------- 2 files changed, 49 insertions(+), 21 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index 994b24d..5f21ac8 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -27,6 +27,18 @@ Matrix1::Matrix1(float* data, int size) : data(data), size(size) { this->externalData = true; } +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; +} + Vector3 Matrix1::ToVector3() { return Vector3(this->data[0], this->data[1], this->data[2]); } @@ -86,7 +98,7 @@ Matrix2::Matrix2() {} Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { this->nValues = nRows * nCols; - //printf("Allocate %d\n", this->nValues); + // printf("Allocate %d\n", this->nValues); if (this->nValues <= 0) { this->data = nullptr; @@ -156,7 +168,7 @@ Matrix2& Matrix2::operator=(const Matrix2& m) { Matrix2::~Matrix2() { if (!this->externalData) { - //printf("Deallocate %d\n", this->nValues); + // printf("Deallocate %d\n", this->nValues); delete[] data; } } @@ -174,7 +186,8 @@ Matrix2 Matrix2::Clone() const { // nCols(other.nCols), // nValues(other.nValues), // 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 +// avoid // // double deletion // } @@ -275,7 +288,17 @@ Matrix2 Matrix2::operator-(const Matrix2& v) const { return r; } -Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { +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; @@ -306,7 +329,7 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { Matrix2 Matrix2::GetRows(int from, int to) { if (from < 0 || to >= this->nRows) - std::cerr << "Slice index out of range." << "std::endl"; + std::cerr << "Slice index out of range." << std::endl; Matrix2 result = Matrix2(to - from, this->nCols); int resultRowIx = 0; @@ -330,8 +353,8 @@ Vector3 Matrix2::GetRow3(int rowIx) { 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.data[ix]; + for (int ix = 0; ix < source.Size(); ix++) + this->data[cellIx + ix] = source(ix); } void Matrix2::SetRow3(int rowIx, Vector3 v) { diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index 8959820..b03f32e 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -9,13 +9,13 @@ 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); + int Size() const; + + static Matrix1 FromVector3(Vector3 v); Vector3 ToVector3(); static Matrix1 FromQuaternion(Quaternion q); @@ -45,6 +45,10 @@ class Matrix1 { this->data[ix] = source.data[sourceIx]; } + protected: + float* data = nullptr; + int size = 0; + private: bool externalData = true; }; @@ -105,15 +109,16 @@ 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); + // { + // 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) { Matrix2 r = Matrix2(m.nRows, m.nCols); @@ -151,8 +156,8 @@ class Matrix2 { // We don't want these because they make impliciti copies which is inefficient // move constructor and move assignment operator - //Matrix2(Matrix2&& other) noexcept; - //Matrix2& operator=(Matrix2&& other) noexcept; + // Matrix2(Matrix2&& other) noexcept; + // Matrix2& operator=(Matrix2&& other) noexcept; static Matrix2 Omega(const Vector3& v); From 662a9340c2302809936e35182da0529d2f8e98d2 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 3 Jul 2025 14:18:20 +0200 Subject: [PATCH 12/12] Cleanup --- LinearAlgebra/Matrix.cpp | 106 ++++++++++++++++++++-------------- LinearAlgebra/Matrix.h | 107 ++++++++++++++--------------------- LinearAlgebra/Quaternion.cpp | 6 +- 3 files changed, 109 insertions(+), 110 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index 5f21ac8..c71e86a 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -15,7 +15,7 @@ namespace LinearAlgebra { Matrix1::Matrix1() {} Matrix1::Matrix1(int size) : size(size) { - if (this->size == 0) + if (this->size <= 0) data = nullptr; else { this->data = new float[size](); @@ -23,11 +23,26 @@ 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; } -int Matrix1::Size() const { +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; } @@ -39,6 +54,28 @@ Matrix1 Matrix1::FromVector3(Vector3 v) { 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]); } @@ -69,9 +106,18 @@ Matrix1 Matrix1::Slice(int start, int stop) { return result; } -void Matrix1::UpdateSlice(int start, int stop, const Matrix1& m) const { - for (int ix = start; ix < stop; ix++) { - this->data[ix] = m.data[ix - start]; +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]; } } @@ -87,13 +133,6 @@ float& Matrix1::operator()(int ix) { #pragma region Matrix2 -// Function to check available heap memory -size_t getAvailableHeapMemory() { - // This is a simple example; you may need to implement a more robust method - // depending on your platform. For ESP32, you can use heap_4.c or similar. - return esp_get_free_heap_size(); // Example for ESP32 -} - Matrix2::Matrix2() {} Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { @@ -105,32 +144,12 @@ Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { this->externalData = false; return; } - // Check available heap memory before allocation - size_t availableMemory = getAvailableHeapMemory(); - if (availableMemory < this->nValues * sizeof(float)) { - std::cerr << "Error: Not enough memory to allocate Matrix2 of size " - << this->nValues << " floats." << std::endl; - this->data = nullptr; // Handle memory allocation failure - this->externalData = false; - return; - } - UBaseType_t stack_size = - uxTaskGetStackHighWaterMark(NULL); // NULL to check the main task - if (stack_size < 1000) - std::cerr << "Stack High Water Mark: " << stack_size << std::endl; - - try { - this->data = new float[this->nValues]; - this->externalData = false; - } catch (const std::bad_alloc& e) { - std::cerr << "Memory allocation failed: " << e.what() << std::endl; - this->data = nullptr; // Handle memory allocation failure - this->externalData = false; - } + this->data = new float[this->nValues]; + this->externalData = false; } -Matrix2::Matrix2(int nRows, int nCols, float* data) +Matrix2::Matrix2(int nRows, int nCols, float* data, float autoDestruct) : nRows(nRows), nCols(nCols), data(data) { this->nValues = nRows * nCols; this->externalData = true; @@ -173,11 +192,12 @@ Matrix2::~Matrix2() { } } -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 @@ -218,9 +238,9 @@ float& Matrix2::operator()(int row, int col) { return data[row * this->nCols + col]; } -void Matrix2::Clear() { +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) { diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index b03f32e..dafd105 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -11,9 +11,16 @@ class Matrix1 { public: Matrix1(); Matrix1(int size); - Matrix1(float* data, int size); + Matrix1(int size, float* data); + Matrix1(const Matrix1& m); // Make a clone - int Size() const; + ~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(); @@ -24,27 +31,10 @@ class Matrix1 { 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; - void Set(unsigned int start, unsigned int stop, float value) { - if (stop > this->size) - stop = this->size; - - for (unsigned int ix = start; ix < stop; ix++) - this->data[ix] = value; - } - void Set(unsigned int start, unsigned int stop, Matrix1 source) { - if (stop > this->size) - stop = this->size; - if (stop > source.size) - stop = source.size; - // Do I need to check on start??? - unsigned int sourceIx = 0; - for (unsigned int ix = start; ix < stop; ix++, sourceIx++) - this->data[ix] = source.data[sourceIx]; - } - protected: float* data = nullptr; int size = 0; @@ -56,34 +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(int nRows, int nCols, float* data); + 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); - static Matrix2 Identity(int size); + Matrix2 GetRows(int from, int to); + Vector3 GetRow3(int rowIx); - static Matrix2 Diagonal(float f, int size); + void SetRow(int rowIx, Matrix1 source); + void SetRow3(int rowIx, Vector3 v); - static Matrix2 SkewMatrix(const Vector3& v); + Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop); - Matrix2 Transpose() const; + 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; @@ -110,15 +113,6 @@ class Matrix2 { } 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) { Matrix2 r = Matrix2(m.nRows, m.nCols); @@ -133,34 +127,19 @@ class Matrix2 { return r; } - 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 Inverse(); - - Matrix2 DeleteRows(int rowStart, int rowStop); - Matrix2 DeleteColumns(int colStart, int colStop); - - // We don't want these because they make impliciti copies which is inefficient - - // move constructor and move assignment operator + // 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 5bb5ecd..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; }