From a17c9339089cbe5dce6c9a94ad082d0fd3ee1082 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 17 Apr 2025 12:50:49 +0200 Subject: [PATCH 01/56] Fixes to make it work againg, but failing at udp.begin() --- Arduino/ArduinoParticipant.cpp | 17 +- Arduino/ArduinoParticipant.h | 4 +- Arduino/Things/DRV8833.cpp | 18 +- Arduino/Things/DRV8833.h | 12 +- LinearAlgebra/Matrix.cpp | 16 +- LocalParticipant.cpp | 366 -------------------------------- LocalParticipant.h | 140 ------------ Messages/BinaryMsg.cpp | 1 + Participants/ParticipantUDP.cpp | 17 +- 9 files changed, 61 insertions(+), 530 deletions(-) delete mode 100644 LocalParticipant.cpp delete mode 100644 LocalParticipant.h diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index 2b9330a..c0e4afe 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -19,12 +19,8 @@ namespace RoboidControl { namespace Arduino { -void ParticipantUDP::Setup(int localPort, - const char* remoteIpAddress, - int remotePort) { +void ParticipantUDP::Setup() { #if defined(ARDUINO) && defined(HAS_WIFI) - this->remoteIpAddress = remoteIpAddress; - this->remotePort = remotePort; GetBroadcastAddress(); #if defined(UNO_R4) @@ -38,9 +34,14 @@ void ParticipantUDP::Setup(int localPort, return; } #endif - udp.begin(localPort); + std::cout << "UDP.begin " << this->port << std::endl; - std::cout << "Wifi sync started to port " << this->remotePort << "\n"; + udp.begin(this->port); + + std::cout << "Wifi sync started local " << this->port; + if (this->remoteSite != nullptr) + std::cout << ", remote " << this->remoteSite->ipAddress << ":" + << this->remoteSite->port << "\n"; #endif } @@ -112,7 +113,7 @@ bool ParticipantUDP::Publish(IMessage* msg) { if (bufferSize <= 0) return true; - udp.beginPacket(this->broadcastIpAddress, this->remotePort); + udp.beginPacket(this->broadcastIpAddress, this->port); udp.write((unsigned char*)buffer, bufferSize); udp.endPacket(); diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h index 94196d7..da387be 100644 --- a/Arduino/ArduinoParticipant.h +++ b/Arduino/ArduinoParticipant.h @@ -11,15 +11,13 @@ namespace Arduino { class ParticipantUDP : public RoboidControl::ParticipantUDP { public: - void Setup(int localPort, const char* remoteIpAddress, int remotePort); + void Setup(); void Receive(); bool Send(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: #if defined(HAS_WIFI) - const char* remoteIpAddress = nullptr; - unsigned short remotePort = 0; char* broadcastIpAddress = nullptr; WiFiUDP udp; diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index d90492e..f3fc46e 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -5,18 +5,27 @@ namespace RoboidControl { namespace Arduino { -DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse) +#if (ESP32) +uint8_t DRV8833Motor::nextAvailablePwmChannel = 0; +#endif + +DRV8833Motor::DRV8833Motor(Participant* participant, + unsigned char pinIn1, + unsigned char pinIn2, + bool reverse) : Thing(participant) { this->pinIn1 = pinIn1; this->pinIn2 = pinIn2; #if (ESP32) - in1Ch = nextAvailablePwmChannel++; + in1Ch = DRV8833Motor::nextAvailablePwmChannel++; ledcSetup(in1Ch, 500, 8); ledcAttachPin(pinIn1, in1Ch); - in2Ch = nextAvailablePwmChannel++; + + in2Ch = DRV8833Motor::nextAvailablePwmChannel++; ledcSetup(in2Ch, 500, 8); ledcAttachPin(pinIn2, in2Ch); + #else pinMode(pinIn1, OUTPUT); // configure the in1 pin to output mode pinMode(pinIn2, OUTPUT); // configure the in1 pin to output mode @@ -47,7 +56,8 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) { if (this->reverse) motorSpeed = -motorSpeed; - // std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm " << rpm + // std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm + // " << rpm // << ", motor signal = " << (int)motorSignal << "\n"; #if (ESP32) diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index cbbfb3c..fbd03e7 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -1,5 +1,6 @@ #pragma once +#include #include "Thing.h" #include "Things/DifferentialDrive.h" @@ -16,7 +17,10 @@ class DRV8833Motor : public Thing { /// @param pinIn1 the pin number for the in1 signal /// @param pinIn2 the pin number for the in2 signal /// @param direction the forward turning direction of the motor - DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false); + DRV8833Motor(Participant* participant, + unsigned char pinIn1, + unsigned char pinIn2, + bool reverse = false); void SetMaxRPM(unsigned int rpm); virtual void SetAngularVelocity(Spherical velocity) override; @@ -27,6 +31,12 @@ class DRV8833Motor : public Thing { unsigned char pinIn1 = 255; unsigned char pinIn2 = 255; unsigned int maxRpm = 200; + +#if (ESP32) + uint8_t in1Ch; + uint8_t in2Ch; + static uint8_t nextAvailablePwmChannel; +#endif }; class DRV8833 : public Thing { diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index b725f55..f2daff0 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,5 +1,7 @@ #include "Matrix.h" +#if !defined(NO_STD) #include +#endif namespace LinearAlgebra { @@ -61,7 +63,9 @@ Matrix2::Matrix2(const Matrix2& m) this->data = nullptr; else { this->data = new float[this->nValues]; - std::copy(m.data, m.data + nValues, this->data); + + for (int ix = 0; ix < this->nValues; ++ix) + this->data[ix] = m.data[ix]; } } @@ -76,7 +80,8 @@ Matrix2& Matrix2::operator=(const Matrix2& m) { this->data = nullptr; else { this->data = new float[this->nValues]; - std::copy(m.data, m.data + this->nValues, this->data); + for (int ix = 0; ix < this->nValues; ++ix) + this->data[ix] = m.data[ix]; } } return *this; @@ -89,7 +94,8 @@ Matrix2::~Matrix2() { Matrix2 Matrix2::Clone() const { Matrix2 r = Matrix2(this->nRows, this->nCols); - std::copy(this->data, this->data + this->nValues, r.data); + for (int ix = 0; ix < this->nValues; ++ix) + r.data[ix] = this->data[ix]; return r; } @@ -158,8 +164,8 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) { Matrix2 Matrix2::Transpose() const { Matrix2 r = Matrix2(this->nCols, this->nRows); - for (uint rowIx = 0; rowIx < this->nRows; rowIx++) { - for (uint colIx = 0; colIx < this->nCols; colIx++) + for (int rowIx = 0; rowIx < this->nRows; rowIx++) { + for (int colIx = 0; colIx < this->nCols; colIx++) r.data[colIx * this->nCols + rowIx] = this->data[rowIx * this->nCols + colIx]; } diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp deleted file mode 100644 index 5cb4793..0000000 --- a/LocalParticipant.cpp +++ /dev/null @@ -1,366 +0,0 @@ -#include "LocalParticipant.h" - -#include "Thing.h" - -#include "Arduino/ArduinoParticipant.h" - -#if defined(_WIN32) || defined(_WIN64) -#include -#include -#include "Windows/WindowsParticipant.h" -#pragma comment(lib, "ws2_32.lib") - -#elif defined(__unix__) || defined(__APPLE__) -#include -#include // For fcntl -#include -#include -#include -#include -#include "Posix/PosixParticipant.h" -#endif - -#include - -namespace RoboidControl { - -// LocalParticipant::LocalParticipant() {} - -LocalParticipant::LocalParticipant(int port) { - this->ipAddress = "0.0.0.0"; - this->port = port; - if (this->port == 0) - this->isIsolated = true; -} - -LocalParticipant::LocalParticipant(const char* ipAddress, int port) { - this->ipAddress = "0.0.0.0"; // ipAddress; // maybe this is not needed - // anymore, keeping it to "0.0.0.0" - this->port = port; - if (this->port == 0) - this->isIsolated = true; - else - this->remoteSite = new Participant(ipAddress, port); -} - -static LocalParticipant* isolatedParticipant = nullptr; - -LocalParticipant* LocalParticipant::Isolated() { - if (isolatedParticipant == nullptr) - isolatedParticipant = new LocalParticipant(0); - return isolatedParticipant; -} - -void LocalParticipant::begin() { - if (this->isIsolated) - return; - - SetupUDP(this->port, this->ipAddress, this->port); -} - -void LocalParticipant::SetupUDP(int localPort, - const char* remoteIpAddress, - int remotePort) { -#if defined(_WIN32) || defined(_WIN64) - Windows::LocalParticipant* thisWindows = - static_cast(this); - thisWindows->Setup(localPort, remoteIpAddress, remotePort); -#elif defined(__unix__) || defined(__APPLE__) - Posix::LocalParticipant* thisPosix = - static_cast(this); - thisPosix->Setup(localPort, remoteIpAddress, remotePort); -#elif defined(ARDUINO) - Arduino::LocalParticipant* thisArduino = - static_cast(this); - thisArduino->Setup(localPort, remoteIpAddress, remotePort); -#endif - this->connected = true; -} - -void LocalParticipant::Update(unsigned long currentTimeMs) { - if (currentTimeMs == 0) { - currentTimeMs = Thing::GetTimeMs(); - // #if defined(ARDUINO) - // currentTimeMs = millis(); - // #elif defined(__unix__) || defined(__APPLE__) - // auto now = std::chrono::steady_clock::now(); - // auto ms = - // std::chrono::duration_cast(now.time_since_epoch()); - // currentTimeMs = static_cast(ms.count()); - // #endif - } - - if (this->isIsolated == false) { - if (this->connected == false) - begin(); - - if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { - ParticipantMsg* msg = new ParticipantMsg(this->networkId); - if (this->remoteSite == nullptr) - this->Publish(msg); - else - this->Send(this->remoteSite, msg); - delete msg; - - this->nextPublishMe = currentTimeMs + this->publishInterval; - } - this->ReceiveUDP(); - } - - for (Thing* thing : this->things) { - if (thing != nullptr) { - thing->Update(currentTimeMs); - if (this->isIsolated == false) { - PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - for (Participant* sender : this->senders) - this->Send(sender, poseMsg); - delete poseMsg; - } - } - } -} - -void LocalParticipant::ReceiveUDP() { -#if defined(_WIN32) || defined(_WIN64) - Windows::LocalParticipant* thisWindows = - static_cast(this); - thisWindows->Receive(); -#elif defined(__unix__) || defined(__APPLE__) - Posix::LocalParticipant* thisPosix = - static_cast(this); - thisPosix->Receive(); -#elif defined(ARDUINO) - Arduino::LocalParticipant* thisArduino = - static_cast(this); - thisArduino->Receive(); -#endif -} - -Participant* LocalParticipant::GetParticipant(const char* ipAddress, int port) { - for (Participant* sender : this->senders) { - if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port) - return sender; - } - return nullptr; -} - -Participant* LocalParticipant::AddParticipant(const char* ipAddress, int port) { - // std::cout << "New Participant " << ipAddress << ":" << port << "\n"; - Participant* participant = new Participant(ipAddress, port); -#if defined(NO_STD) - participant->networkId = this->senderCount; - this->senders[this->senderCount++] = participant; -#else - participant->networkId = (unsigned char)this->senders.size(); - this->senders.push_back(participant); -#endif - return participant; -} - -#pragma region Send - -void LocalParticipant::SendThingInfo(Participant* remoteParticipant, - Thing* thing) { - // std::cout << "Send thing info " << (int)thing->id << " \n"; - ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); - this->Send(remoteParticipant, thingMsg); - delete thingMsg; - NameMsg* nameMsg = new NameMsg(this->networkId, thing); - this->Send(remoteParticipant, nameMsg); - delete nameMsg; - ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing); - this->Send(remoteParticipant, modelMsg); - delete modelMsg; - PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true); - this->Send(remoteParticipant, poseMsg); - delete poseMsg; - BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing); - this->Send(remoteParticipant, customMsg); - delete customMsg; -} - -bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) { - int bufferSize = msg->Serialize(this->buffer); - if (bufferSize <= 0) - return true; - -#if defined(_WIN32) || defined(_WIN64) - Windows::LocalParticipant* thisWindows = - static_cast(this); - return thisWindows->Send(remoteParticipant, bufferSize); -#elif defined(__unix__) || defined(__APPLE__) - Posix::LocalParticipant* thisPosix = - static_cast(this); - return thisPosix->Send(remoteParticipant, bufferSize); -#elif defined(ARDUINO) - Arduino::LocalParticipant* thisArduino = - static_cast(this); - return thisArduino->Send(remoteParticipant, bufferSize); -#endif -} - -void LocalParticipant::PublishThingInfo(Thing* thing) { - // std::cout << "Publish thing info" << thing->networkId << "\n"; - // Strange, when publishing, the network id is irrelevant, because it is - // connected to a specific site... - ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); - this->Publish(thingMsg); - delete thingMsg; - NameMsg* nameMsg = new NameMsg(this->networkId, thing); - this->Publish(nameMsg); - delete nameMsg; - ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing); - this->Publish(modelMsg); - delete modelMsg; - PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true); - this->Publish(poseMsg); - delete poseMsg; - BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing); - this->Publish(customMsg); - delete customMsg; -} - -bool LocalParticipant::Publish(IMessage* msg) { -#if defined(_WIN32) || defined(_WIN64) - Windows::LocalParticipant* thisWindows = - static_cast(this); - return thisWindows->Publish(msg); -#elif defined(__unix__) || defined(__APPLE__) - Posix::LocalParticipant* thisPosix = - static_cast(this); - return thisPosix->Publish(msg); -#elif defined(ARDUINO) - Arduino::LocalParticipant* thisArduino = - static_cast(this); - return thisArduino->Publish(msg); -#endif -} - -// Send -#pragma endregion - -#pragma region Receive - -void LocalParticipant::ReceiveData(unsigned char packetSize, - char* senderIpAddress, - unsigned int senderPort) { - Participant* remoteParticipant = - this->GetParticipant(senderIpAddress, senderPort); - if (remoteParticipant == nullptr) { - remoteParticipant = this->AddParticipant(senderIpAddress, senderPort); - // std::cout << "New sender " << sender_ipAddress << ":" << sender_port - // << "\n"; - // std::cout << "New remote participant " << remoteParticipant->ipAddress - // << ":" << remoteParticipant->port << " " - // << (int)remoteParticipant->networkId << "\n"; - } - - ReceiveData(packetSize, remoteParticipant); -} - -void LocalParticipant::ReceiveData(unsigned char bufferSize, - Participant* remoteParticipant) { - unsigned char msgId = this->buffer[0]; - // std::cout << "receive msg " << (int)msgId << "\n"; - switch (msgId) { - case ParticipantMsg::id: { - ParticipantMsg* msg = new ParticipantMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case SiteMsg::id: { - SiteMsg* msg = new SiteMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case InvestigateMsg::id: { - InvestigateMsg* msg = new InvestigateMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case ThingMsg::id: { - ThingMsg* msg = new ThingMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case NameMsg::id: { - NameMsg* msg = new NameMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case PoseMsg::id: { - PoseMsg* msg = new PoseMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case BinaryMsg::id: { - BinaryMsg* msg = new BinaryMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - }; -} - -void LocalParticipant::Process(Participant* sender, ParticipantMsg* msg) {} - -void LocalParticipant::Process(Participant* sender, SiteMsg* msg) { - // std::cout << this->name << ": process NetworkId [" << (int)this->networkId - // << "/" << (int)msg->networkId << "]\n"; - if (this->networkId != msg->networkId) { - this->networkId = msg->networkId; - // std::cout << this->things.size() << " things\n"; - for (Thing* thing : this->things) - this->SendThingInfo(sender, thing); - } -} - -void LocalParticipant::Process(Participant* sender, InvestigateMsg* msg) {} - -void LocalParticipant::Process(Participant* sender, ThingMsg* msg) {} - -void LocalParticipant::Process(Participant* sender, NameMsg* msg) { - Thing* thing = sender->Get(msg->networkId, msg->thingId); - if (thing != nullptr) { - int nameLength = msg->nameLength; - int stringLen = nameLength + 1; - char* thingName = new char[stringLen]; -#if defined(_WIN32) || defined(_WIN64) - strncpy_s(thingName, stringLen, msg->name, - stringLen - 1); // Leave space for null terminator -#else - // Use strncpy with bounds checking for other platforms (Arduino, POSIX, - // ESP-IDF) - strncpy(thingName, msg->name, - stringLen - 1); // Leave space for null terminator - thingName[stringLen - 1] = '\0'; // Ensure null termination -#endif - thingName[nameLength] = '\0'; - thing->name = thingName; - // std::cout << "thing name = " << thing->name << " length = " << nameLength - // << "\n"; - } -} - -void LocalParticipant::Process(Participant* sender, PoseMsg* msg) {} - -void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) { - // std::cout << this->name << ": process Binary [" << (int)this->networkId << - // "/" - // << (int)msg->networkId << "]\n"; - Thing* thing = sender->Get(msg->networkId, msg->thingId); - if (thing != nullptr) - thing->ProcessBinary(msg->bytes); - else { - thing = this->Get(msg->networkId, msg->thingId); - if (thing != nullptr) - thing->ProcessBinary(msg->bytes); - // else - // std::cout << "custom msg for unknown thing [" << (int)msg->networkId - // << "/" << (int)msg->thingId << "]\n"; - } -} - -// Receive -#pragma endregion - -} // namespace RoboidControl diff --git a/LocalParticipant.h b/LocalParticipant.h deleted file mode 100644 index d0b34af..0000000 --- a/LocalParticipant.h +++ /dev/null @@ -1,140 +0,0 @@ -#pragma once - -#include "Messages/BinaryMsg.h" -#include "Messages/InvestigateMsg.h" -#include "Messages/ModelUrlMsg.h" -#include "Messages/NameMsg.h" -#include "Messages/ParticipantMsg.h" -#include "Messages/PoseMsg.h" -#include "Messages/SiteMsg.h" -#include "Messages/ThingMsg.h" -#include "Participant.h" - -#if !defined(NO_STD) -#include -#endif - -#if defined(_WIN32) || defined(_WIN64) -#include -#elif defined(__unix__) || defined(__APPLE__) -#include -#include -#include -#include -#elif defined(ARDUINO) -// #include -#endif - -namespace RoboidControl { - -constexpr int MAX_SENDER_COUNT = 256; - -/// @brief A local participant is the local device which can communicate with -/// other participants It manages all local things and communcation with other -/// participants. Each application has a local participant which is usually -/// explicit in the code. An participant can be isolated. In that case it is -/// standalong and does not communicate with other participants. -/// -/// It is possible to work with an hidden participant by creating things without -/// specifying a participant in the constructor. In that case an hidden isolated -/// participant is created which can be obtained using -/// RoboidControl::LocalParticipant::Isolated(). -/// @sa RoboidControl::Thing::Thing() -class LocalParticipant : public Participant { - public: - /// @brief Create a participant without connecting to a site - /// @param port The port on which the participant communicates - /// These participant typically broadcast Participant messages to let site - /// servers on the local network know their presence. Alternatively they can - /// broadcast information which can be used directly by other participants. - LocalParticipant(int port = 7681); - /// @brief Create a participant which will try to connect to a site. - /// @param ipAddress The IP address of the site - /// @param port The port used by the site - LocalParticipant(const char* ipAddress, int port = 7681); - // Note to self: one cannot specify the port used by the local participant - // now!! - - /// @brief Isolated participant is used when the application is run without - /// networking - /// @return A participant without networking support - static LocalParticipant* Isolated(); - - /// @brief True if the participant is running isolated. - /// Isolated participants do not communicate with other participants - bool isIsolated = false; - - /// The interval in milliseconds for publishing (broadcasting) data on the - /// local network - long publishInterval = 3000; // 3 seconds - - /// @brief The name of the participant - const char* name = "LocalParticipant"; - - // int localPort = 0; - - /// @brief The remote site when this participant is connected to a site - Participant* remoteSite = nullptr; - -#if defined(ARDUINO) - // const char* remoteIpAddress = nullptr; - // unsigned short remotePort = 0; - // char* broadcastIpAddress = nullptr; - - // WiFiUDP udp; -#else - -#if defined(__unix__) || defined(__APPLE__) - int sock; -#endif - sockaddr_in remote_addr; - sockaddr_in server_addr; - sockaddr_in broadcast_addr; - -#endif - - void begin(); - bool connected = false; - - virtual void Update(unsigned long currentTimeMs = 0); - - void SendThingInfo(Participant* remoteParticipant, Thing* thing); - void PublishThingInfo(Thing* thing); - - bool Send(Participant* remoteParticipant, IMessage* msg); - bool Publish(IMessage* msg); - - void ReceiveData(unsigned char bufferSize, - char* senderIpAddress, - unsigned int senderPort); - void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant); - -#if defined(NO_STD) - unsigned char senderCount = 0; - Participant* senders[MAX_SENDER_COUNT]; -#else - std::list senders; -#endif - - protected: - unsigned long nextPublishMe = 0; - - char buffer[1024]; - - void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort); - - Participant* GetParticipant(const char* ipAddress, int port); - Participant* AddParticipant(const char* ipAddress, int port); - - void ReceiveUDP(); - - virtual void Process(Participant* sender, ParticipantMsg* msg); - virtual void Process(Participant* sender, SiteMsg* msg); - virtual void Process(Participant* sender, InvestigateMsg* msg); - virtual void Process(Participant* sender, ThingMsg* msg); - virtual void Process(Participant* sender, NameMsg* msg); - virtual void Process(Participant* sender, PoseMsg* msg); - virtual void Process(Participant* sender, BinaryMsg* msg); -}; - -} // namespace RoboidControl diff --git a/Messages/BinaryMsg.cpp b/Messages/BinaryMsg.cpp index 0b0b940..d04ac2d 100644 --- a/Messages/BinaryMsg.cpp +++ b/Messages/BinaryMsg.cpp @@ -7,6 +7,7 @@ BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) { this->thingId = thing->id; this->thing = thing; unsigned char ix = BinaryMsg::length; + this->data = new char[255]; this->dataLength = this->thing->GenerateBinary(this->data, &ix); } diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index f643587..0783a40 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -69,7 +69,7 @@ void ParticipantUDP::SetupUDP(int localPort, #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = static_cast(this); - thisArduino->Setup(localPort, remoteIpAddress, remotePort); + thisArduino->Setup(); #elif defined(IDF_VER) EspIdf::ParticipantUDP* thisEspIdf = static_cast(this); @@ -255,8 +255,10 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize, Participant* sender = this->GetParticipant(senderIpAddress, senderPort); if (sender == nullptr) { sender = this->AddParticipant(senderIpAddress, senderPort); +#if !defined(NO_STD) std::cout << "New remote participant " << sender->ipAddress << ":" << sender->port << std::endl; +#endif } ReceiveData(packetSize, sender); @@ -317,9 +319,11 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize, } break; }; - // Check if the buffer has been read completely +// Check if the buffer has been read completely +#if !defined(NO_STD) if (bufferSize > 0) std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n"; +#endif } void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { @@ -353,7 +357,8 @@ void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) { void ParticipantUDP::Process(Participant* 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"; + << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " + << (int)msg->parentId << "\n"; #endif } @@ -380,9 +385,13 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { thingName[nameLength] = '\0'; thing->name = thingName; +#if !defined(NO_STD) std::cout << thing->name; +#endif } +#if !defined(NO_STD) std::cout << std::endl; +#endif } void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { @@ -408,11 +417,13 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { Thing* thing = sender->Get(msg->thingId); if (thing != nullptr) thing->ProcessBinary(msg->data); +#if !defined(NO_STD) else { std::cout << " unknown thing [" << (int)msg->networkId << "/" << (int)msg->thingId << "]"; } std::cout << std::endl; +#endif } // Receive From 8ff0fdd78f6cb4c4f44eb3cc96c750d53bc0b48f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 18 Apr 2025 14:36:07 +0200 Subject: [PATCH 02/56] Fix crash at udp.begin() --- Arduino/ArduinoParticipant.cpp | 28 +++++++++++++++------------- Arduino/ArduinoParticipant.h | 8 -------- Participants/ParticipantUDP.cpp | 8 ++++++-- 3 files changed, 21 insertions(+), 23 deletions(-) diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index c0e4afe..f74a746 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -19,6 +19,8 @@ namespace RoboidControl { namespace Arduino { +WiFiUDP* udp; + void ParticipantUDP::Setup() { #if defined(ARDUINO) && defined(HAS_WIFI) GetBroadcastAddress(); @@ -34,9 +36,9 @@ void ParticipantUDP::Setup() { return; } #endif - std::cout << "UDP.begin " << this->port << std::endl; - udp.begin(this->port); + udp = new WiFiUDP(); + udp->begin(this->port); std::cout << "Wifi sync started local " << this->port; if (this->remoteSite != nullptr) @@ -59,14 +61,14 @@ void ParticipantUDP::GetBroadcastAddress() { void ParticipantUDP::Receive() { #if defined(ARDUINO) && defined(HAS_WIFI) - int packetSize = udp.parsePacket(); + int packetSize = udp->parsePacket(); while (packetSize > 0) { - udp.read(buffer, packetSize); + udp->read(buffer, packetSize); - String senderAddress = udp.remoteIP().toString(); + String senderAddress = udp->remoteIP().toString(); char sender_ipAddress[16]; senderAddress.toCharArray(sender_ipAddress, 16); - unsigned int sender_port = udp.remotePort(); + unsigned int sender_port = udp->remotePort(); // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, // sender_port); if (remoteParticipant == nullptr) { @@ -82,7 +84,7 @@ void ParticipantUDP::Receive() { // ReceiveData(packetSize, remoteParticipant); ReceiveData(packetSize, sender_ipAddress, sender_port); - packetSize = udp.parsePacket(); + packetSize = udp->parsePacket(); } #endif } @@ -99,9 +101,9 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { delay(10); } n++; - udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port); - udp.write((unsigned char*)buffer, bufferSize); - } while (udp.endPacket() == 0 && n < 10); + udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port); + udp->write((unsigned char*)buffer, bufferSize); + } while (udp->endPacket() == 0 && n < 10); #endif return true; @@ -113,9 +115,9 @@ bool ParticipantUDP::Publish(IMessage* msg) { if (bufferSize <= 0) return true; - udp.beginPacket(this->broadcastIpAddress, this->port); - udp.write((unsigned char*)buffer, bufferSize); - udp.endPacket(); + udp->beginPacket(this->broadcastIpAddress, this->port); + udp->write((unsigned char*)buffer, bufferSize); + udp->endPacket(); // std::cout << "Publish to " << this->broadcastIpAddress << ":" // << this->remotePort << "\n"; diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h index da387be..5a9396f 100644 --- a/Arduino/ArduinoParticipant.h +++ b/Arduino/ArduinoParticipant.h @@ -2,10 +2,6 @@ #include "Participants/ParticipantUDP.h" -#if defined(HAS_WIFI) -#include -#endif - namespace RoboidControl { namespace Arduino { @@ -17,12 +13,8 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { bool Publish(IMessage* msg); protected: -#if defined(HAS_WIFI) char* broadcastIpAddress = nullptr; - WiFiUDP udp; -#endif - void GetBroadcastAddress(); }; diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 0783a40..65e28d9 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -104,14 +104,15 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { if (thing == nullptr) continue; - if (this->isIsolated == false) { + thing->Update(currentTimeMs, true); + + if (this->isIsolated == false && thing->owner != this) { PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); this->Send(thing->owner, poseMsg); BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); this->Send(thing->owner, binaryMsg); delete poseMsg; } - thing->Update(currentTimeMs, true); } } @@ -178,6 +179,8 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, } bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { + // std::cout << "send msg " << (int)this->buffer[0] << " to " + // << remoteParticipant->ipAddress << std::endl; int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) return true; @@ -224,6 +227,7 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) { } bool ParticipantUDP::Publish(IMessage* msg) { + // std::cout << "publish msg\n"; #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = static_cast(this); From 045826d3c2a9e5e892425b30fc90b624f5361cd9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 18 Apr 2025 17:24:33 +0200 Subject: [PATCH 03/56] Fixed parenting --- Arduino/Things/DRV8833.cpp | 29 +++++++++++-- Arduino/Things/DRV8833.h | 65 ++++++++++++++++------------- Arduino/Things/UltrasonicSensor.cpp | 7 ++++ Arduino/Things/UltrasonicSensor.h | 6 +++ Participants/ParticipantUDP.cpp | 7 ++-- Thing.cpp | 10 ++++- Thing.h | 1 + Things/TouchSensor.h | 3 ++ 8 files changed, 91 insertions(+), 37 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index f3fc46e..8e03f3c 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -9,11 +9,13 @@ namespace Arduino { uint8_t DRV8833Motor::nextAvailablePwmChannel = 0; #endif -DRV8833Motor::DRV8833Motor(Participant* participant, +DRV8833Motor::DRV8833Motor(DRV8833* driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse) - : Thing(participant) { + : Thing(driver->owner) { + this->parent = driver; + this->pinIn1 = pinIn1; this->pinIn2 = pinIn2; @@ -120,11 +122,30 @@ DRV8833::DRV8833(Participant* participant, if (pinStandby != 255) pinMode(pinStandby, OUTPUT); - this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA); + this->motorA = new DRV8833Motor(this, pinAIn1, pinAIn2, reverseA); this->motorA->name = "Motor A"; - this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB); + this->motorB = new DRV8833Motor(this, pinBIn1, pinBIn2, reverseB); this->motorB->name = "Motor B"; } +DRV8833::DRV8833(Thing* parent, + unsigned char pinAIn1, + unsigned char pinAIn2, + unsigned char pinBIn1, + unsigned char pinBIn2, + unsigned char pinStandby, + bool reverseA, + bool reverseB) + : DRV8833(parent->owner, + pinAIn1, + pinAIn2, + pinBIn1, + pinBIn2, + pinStandby, + reverseA, + reverseB) { + this->parent = parent; +} + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index fbd03e7..cbd63d6 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -7,6 +7,42 @@ namespace RoboidControl { namespace Arduino { +class DRV8833Motor; + +class DRV8833 : public Thing { + public: + /// @brief Setup a DRV8833 motor controller + /// @param pinAIn1 The pin number connected to the AIn1 port + /// @param pinAIn2 The pin number connected to the AIn2 port + /// @param pinBIn1 The pin number connected to the BIn1 port + /// @param pinBIn2 The pin number connceted to the BIn2 port + /// @param pinStandby The pin number connected to the standby port, 255 + /// indicated that the port is not connected + /// @param reverseA The forward turning direction of motor A + /// @param reverseB The forward turning direction of motor B + DRV8833(Participant* participant, + unsigned char pinAIn1, + unsigned char pinAIn2, + unsigned char pinBIn1, + unsigned char pinBIn2, + unsigned char pinStandby = 255, + bool reverseA = false, + bool reverseB = false); + DRV8833(Thing* parent, + unsigned char pinAIn1, + unsigned char pinAIn2, + unsigned char pinBIn1, + unsigned char pinBIn2, + unsigned char pinStandby = 255, + bool reverseA = false, + bool reverseB = false); + DRV8833Motor* motorA = nullptr; + DRV8833Motor* motorB = nullptr; + + protected: + unsigned char pinStandby = 255; +}; + /// @brief Support for a DRV8833 motor controller class DRV8833Motor : public Thing { public: @@ -17,7 +53,7 @@ class DRV8833Motor : public Thing { /// @param pinIn1 the pin number for the in1 signal /// @param pinIn2 the pin number for the in2 signal /// @param direction the forward turning direction of the motor - DRV8833Motor(Participant* participant, + DRV8833Motor(DRV8833* driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false); @@ -39,32 +75,5 @@ class DRV8833Motor : public Thing { #endif }; -class DRV8833 : public Thing { - public: - /// @brief Setup a DRV8833 motor controller - /// @param pinAIn1 The pin number connected to the AIn1 port - /// @param pinAIn2 The pin number connected to the AIn2 port - /// @param pinBIn1 The pin number connected to the BIn1 port - /// @param pinBIn2 The pin number connceted to the BIn2 port - /// @param pinStandby The pin number connected to the standby port, 255 - /// indicated that the port is not connected - /// @param reverseA The forward turning direction of motor A - /// @param reverseB The forward turning direction of motor B - DRV8833(Participant* participant, - unsigned char pinAIn1, - unsigned char pinAIn2, - unsigned char pinBIn1, - unsigned char pinBIn2, - unsigned char pinStandby = 255, - bool reverseA = false, - bool reverseB = false); - - DRV8833Motor* motorA = nullptr; - DRV8833Motor* motorB = nullptr; - - protected: - unsigned char pinStandby = 255; -}; - } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index d6deb64..0f89bd0 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -16,6 +16,13 @@ UltrasonicSensor::UltrasonicSensor(Participant* participant, pinMode(pinEcho, INPUT); // configure the echo pin to input mode } +UltrasonicSensor::UltrasonicSensor(Thing* parent, + unsigned char pinTrigger, + unsigned char pinEcho) + : UltrasonicSensor(parent->owner, pinTrigger, pinEcho) { + this->parent = parent; +} + float UltrasonicSensor::GetDistance() { // Start the ultrasonic 'ping' digitalWrite(pinTrigger, LOW); diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index a80be2b..3e9e815 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -8,6 +8,9 @@ namespace Arduino { /// @brief An HC-SR04 ultrasonic distance sensor class UltrasonicSensor : public TouchSensor { public: + // Inherit all constructors + using TouchSensor::TouchSensor; + /// @brief Setup an ultrasonic sensor /// @param participant The participant to use /// @param pinTrigger The pin number of the trigger signal @@ -15,6 +18,9 @@ class UltrasonicSensor : public TouchSensor { UltrasonicSensor(Participant* participant, unsigned char pinTrigger, unsigned char pinEcho); + UltrasonicSensor(Thing* parent, + unsigned char pinTrigger, + unsigned char pinEcho); // parameters diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 65e28d9..b82df34 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -179,12 +179,13 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, } bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { - // std::cout << "send msg " << (int)this->buffer[0] << " to " - // << remoteParticipant->ipAddress << std::endl; int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) return true; + std::cout << "send msg " << (int)this->buffer[0] << " to " + << remoteParticipant->ipAddress << std::endl; + #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = static_cast(this); @@ -227,7 +228,7 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) { } bool ParticipantUDP::Publish(IMessage* msg) { - // std::cout << "publish msg\n"; + std::cout << "publish msg\n"; #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = static_cast(this); diff --git a/Thing.cpp b/Thing.cpp index 86e4008..d1aeaa2 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -42,8 +42,14 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { this->angularVelocity = Spherical::zero; // std::cout << "add thing [" << (int)this->id << "] to owner " - // << this->owner->ipAddress << ":" << this->owner->port << std::endl; - this->owner->Add(this, false); + // << this->owner->ipAddress << ":" << this->owner->port << + // std::endl; + this->owner->Add(this, true); +} + +Thing::Thing(Thing* parent, int thingType, unsigned char thingId) + : Thing(parent->owner, thingType, thingId) { + this->parent = parent; } // Thing::Thing(Participant* owner, diff --git a/Thing.h b/Thing.h index febe7a9..d08bd04 100644 --- a/Thing.h +++ b/Thing.h @@ -58,6 +58,7 @@ class Thing { // unsigned char networkId, // unsigned char thingId, // Type thingType = Type::Undetermined); + Thing(Thing* parent, int thingType = 0, unsigned char thingId = 0); /// @brief The participant managing this thing Participant* owner; diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index 39638e7..4e6d483 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -7,6 +7,9 @@ namespace RoboidControl { /// @brief A sensor which can detect touches class TouchSensor : public Thing { public: + // Inherit all constructors + using Thing::Thing; + /// @brief Create a touch sensor with isolated participant TouchSensor(); /// @brief Create a touch sensor From 619695c90d4702e32857b30f6df50bda2c7c1813 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 22 Apr 2025 11:59:52 +0200 Subject: [PATCH 04/56] static participant list --- Participant.cpp | 58 +++++++++++++++++++++++++++++-- Participant.h | 10 +++++- Participants/ParticipantUDP.cpp | 60 ++++++++++++--------------------- Participants/ParticipantUDP.h | 16 ++++----- Participants/SiteServer.cpp | 10 +++--- 5 files changed, 100 insertions(+), 54 deletions(-) diff --git a/Participant.cpp b/Participant.cpp index 65a1f51..7166ad0 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -4,6 +4,8 @@ namespace RoboidControl { +std::list Participant::participants; + Participant::Participant() {} Participant::Participant(const char* ipAddress, int port) { @@ -34,12 +36,63 @@ void Participant::Update(unsigned long currentTimeMs) { } } +Participant* Participant::GetParticipant(const char* ipAddress, + unsigned int port) { + for (Participant* participant : Participant::participants) { + if (participant == nullptr) + continue; + if (strcmp(participant->ipAddress, ipAddress) == 0 && + participant->port == port) + return participant; + } + std::cout << "Could not find participant " << ipAddress << ":" << (int)port + << std::endl; + return nullptr; +} + +Participant* Participant::GetParticipant(unsigned char participantId) { + for (Participant* participant : Participant::participants) { + if (participant == nullptr) + continue; + if (participant->networkId == participantId) + return participant; + } + std::cout << "Could not find participant " << (int)participantId << std::endl; + return nullptr; +} + +Participant* Participant::AddParticipant(const char* ipAddress, unsigned int port) { + Participant* participant = new Participant(ipAddress, port); + Participant::AddParticipant(participant); + return participant; +} + +void Participant::AddParticipant(Participant* participant) { + Participant* foundParticipant = + Participant::GetParticipant(participant->networkId); + if (foundParticipant == nullptr) { +#if defined(NO_STD) + this->things[this->thingCount++] = thing; +#else + Participant::participants.push_back(participant); +#endif + std::cout << "Add participant " << participant->ipAddress << ":" + << participant->port << "[" << (int)participant->networkId + << "]\n"; + } else { + std::cout << "Did not add, existing thing " << participant->ipAddress << ":" + << participant->port << "[" << (int)participant->networkId + << "]\n"; + } +} + Thing* Participant::Get(unsigned char thingId) { for (Thing* thing : this->things) { if (thing->id == thingId) return thing; } - // std::cout << "Could not find thing " << this->ipAddress << ":" << this->port + // std::cout << "Could not find thing " << this->ipAddress << ":" << + // this->port // << "[" << (int)thingId << "]\n"; return nullptr; } @@ -64,7 +117,8 @@ void Participant::Add(Thing* thing, bool checkId) { #else this->things.push_back(thing); #endif - // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "[" + // std::cout << "Add thing " << this->ipAddress << ":" << this->port << + // "[" // << (int)thing->id << "]\n"; } else { // std::cout << "Did not add, existing thing " << this->ipAddress << ":" diff --git a/Participant.h b/Participant.h index 5fdffc4..be7133e 100644 --- a/Participant.h +++ b/Participant.h @@ -18,7 +18,7 @@ class Participant { const char* ipAddress = "0.0.0.0"; /// @brief The port number for UDP communication with the participant. This is /// 0 for isolated participants. - int port = 0; + unsigned int port = 0; /// @brief The network Id to identify the participant. /// @note This field is likely to disappear in future versions @@ -40,11 +40,19 @@ class Participant { unsigned char thingCount = 0; Thing* things[MAX_THING_COUNT]; #else + /// @brief The list of known participants + static std::list participants; + /// @brief The list of things managed by this participant std::list things; #endif public: + static Participant* GetParticipant(const char* ipAddress, unsigned int port); + static Participant* GetParticipant(unsigned char participantId); + static Participant* AddParticipant(const char* ipAddress, unsigned int port); + static void AddParticipant(Participant* participant); + /// @brief Find a thing managed by this participant /// @param networkId The network ID for the thing /// @param thingId The ID of the thing diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 4554483..da55b48 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -31,6 +31,7 @@ ParticipantUDP::ParticipantUDP(int port) { this->remoteSite = nullptr; if (this->port == 0) this->isIsolated = true; + Participant::AddParticipant(this); } ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) @@ -39,6 +40,7 @@ ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) this->isIsolated = true; else this->remoteSite = new Participant(ipAddress, port); + Participant::AddParticipant(this); } static ParticipantUDP* isolatedParticipant = nullptr; @@ -135,27 +137,6 @@ void ParticipantUDP::ReceiveUDP() { #endif } -Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) { - for (Participant* sender : this->senders) { - if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port) - return sender; - } - return nullptr; -} - -Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) { - // std::cout << "New Participant " << ipAddress << ":" << port << "\n"; - Participant* participant = new Participant(ipAddress, port); -#if defined(NO_STD) - participant->networkId = this->senderCount; - this->senders[this->senderCount++] = participant; -#else - participant->networkId = (unsigned char)this->senders.size(); - this->senders.push_back(participant); -#endif - return participant; -} - #pragma region Send void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, @@ -179,14 +160,12 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, } bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { - // std::cout << "send msg " << (int)this->buffer[0] << " to " - // << remoteParticipant->ipAddress << std::endl; int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) return true; - std::cout << "send msg " << (int)this->buffer[0] << " to " - << remoteParticipant->ipAddress << std::endl; + // std::cout << "send msg " << (int)this->buffer[0] << " to " + // << remoteParticipant->ipAddress << std::endl; #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = @@ -259,6 +238,8 @@ bool ParticipantUDP::Publish(IMessage* msg) { void ParticipantUDP::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) { + std::cout << "Receive data from " << senderIpAddress << ":" << senderPort + << std::endl; Participant* sender = this->GetParticipant(senderIpAddress, senderPort); if (sender == nullptr) { sender = this->AddParticipant(senderIpAddress, senderPort); @@ -346,12 +327,12 @@ void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { << " -> " << (int)msg->networkId << "\n"; #endif - if (this->networkId != msg->networkId) { + if (this->networkId != msg->networkId) this->networkId = msg->networkId; - // std::cout << this->things.size() << " things\n"; - for (Thing* thing : this->things) - this->SendThingInfo(sender, thing); - } + + // std::cout << this->things.size() << " things\n"; + for (Thing* thing : this->things) + this->SendThingInfo(sender, thing); } void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) { @@ -421,16 +402,19 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { << "/" << (int)msg->thingId << "] "; #endif - Thing* thing = sender->Get(msg->thingId); - if (thing != nullptr) - thing->ProcessBinary(msg->data); + Participant* owner = Participant::GetParticipant(msg->networkId); + if (owner != nullptr) { + Thing* thing = owner->Get(msg->thingId); + if (thing != nullptr) + thing->ProcessBinary(msg->data); #if !defined(NO_STD) - else { - std::cout << " unknown thing [" << (int)msg->networkId << "/" - << (int)msg->thingId << "]"; - } - std::cout << std::endl; + else { + std::cout << " unknown thing [" << (int)msg->networkId << "/" + << (int)msg->thingId << "]"; + } + std::cout << std::endl; #endif + } } // Receive diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index ec93af9..f50187e 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -112,12 +112,12 @@ class ParticipantUDP : public Participant { unsigned int senderPort); void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant); -#if defined(NO_STD) - unsigned char senderCount = 0; - Participant* senders[MAX_SENDER_COUNT]; -#else - std::list senders; -#endif +// #if defined(NO_STD) +// unsigned char senderCount = 0; +// Participant* senders[MAX_SENDER_COUNT]; +// #else +// std::list senders; +// #endif protected: unsigned long nextPublishMe = 0; @@ -126,8 +126,8 @@ class ParticipantUDP : public Participant { void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort); - Participant* GetParticipant(const char* ipAddress, int port); - Participant* AddParticipant(const char* ipAddress, int port); + //Participant* GetParticipant(const char* ipAddress, int port); + // Participant* AddParticipant(const char* ipAddress, int port); void ReceiveUDP(); diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 8bde494..e300371 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -16,11 +16,11 @@ SiteServer::SiteServer(int port) { this->ipAddress = "0.0.0.0"; this->port = port; -#if defined(NO_STD) - this->senders[this->senderCount++] = this; -#else - this->senders.push_back(this); -#endif +// #if defined(NO_STD) +// this->senders[this->senderCount++] = this; +// #else +// this->senders.push_back(this); +// #endif SetupUDP(port, ipAddress, 0); From e56f25f9a1065fc8b1dd6ea17ea6169fae92388f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 22 Apr 2025 17:47:06 +0200 Subject: [PATCH 05/56] Improved network sync --- Arduino/Things/UltrasonicSensor.cpp | 6 +-- Arduino/Things/UltrasonicSensor.h | 2 +- Messages/DestroyMsg.cpp | 19 +++++---- Messages/PoseMsg.cpp | 6 +-- Participant.cpp | 8 ++-- Participant.h | 2 +- Participants/ParticipantUDP.cpp | 65 +++++++++++++++++++++++++---- Participants/ParticipantUDP.h | 3 ++ Participants/SiteServer.cpp | 35 +++++++++++----- Participants/SiteServer.h | 20 +-------- Thing.cpp | 21 +++++----- Thing.h | 1 + Things/TouchSensor.cpp | 4 +- 13 files changed, 123 insertions(+), 69 deletions(-) diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 0f89bd0..0d1275c 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -51,10 +51,10 @@ float UltrasonicSensor::GetDistance() { // if (distance > 30) // distance = 0; - this->touchedSomething |= (this->distance <= this->touchDistance); + this->touchedSomething |= (this->distance > 0 && this->distance <= this->touchDistance); - // std::cout << "Ultrasonic " << this->distance << " " << - // this->touchedSomething << "\n"; + // std::cout << "Ultrasonic " << this->touchedSomething << " | " << (this->distance > 0) << " " << + // (this->distance <= this->touchDistance) << "\n"; return distance; } diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index 3e9e815..d67d50e 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -9,7 +9,7 @@ namespace Arduino { class UltrasonicSensor : public TouchSensor { public: // Inherit all constructors - using TouchSensor::TouchSensor; + //using TouchSensor::TouchSensor; /// @brief Setup an ultrasonic sensor /// @param participant The participant to use diff --git a/Messages/DestroyMsg.cpp b/Messages/DestroyMsg.cpp index 6141582..f41ba3b 100644 --- a/Messages/DestroyMsg.cpp +++ b/Messages/DestroyMsg.cpp @@ -2,20 +2,23 @@ namespace RoboidControl { -DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) { +DestroyMsg::DestroyMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; } -DestroyMsg::DestroyMsg(char* buffer) {} +DestroyMsg::DestroyMsg(char* buffer) { + this->networkId = buffer[1]; + this->thingId = buffer[2]; +} DestroyMsg::~DestroyMsg() {} -unsigned char DestroyMsg::Serialize(char *buffer) { - #if defined(DEBUG) - std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" << (int)this->thingId - << "] " << std::endl; -#endif +unsigned char DestroyMsg::Serialize(char* buffer) { +//#if defined(DEBUG) + std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" + << (int)this->thingId << "] " << std::endl; +//#endif unsigned char ix = 0; buffer[ix++] = this->id; buffer[ix++] = this->networkId; @@ -23,4 +26,4 @@ unsigned char DestroyMsg::Serialize(char *buffer) { return ix; } -} // namespace RoboidControl +} // namespace RoboidControl diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index c6d41aa..3edd823 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { this->thingId = thing->id; this->poseType = 0; - if (thing->positionUpdated || force) { + if (thing->positionUpdated || (force && thing->GetLinearVelocity().distance == 0)) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; } - if (thing->orientationUpdated || force) { + if (thing->orientationUpdated || (force && thing->GetAngularVelocity().distance == 0)) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; } @@ -45,7 +45,7 @@ unsigned char PoseMsg::Serialize(char* buffer) { if (this->poseType == 0) return 0; -#if defined(DEBUG) +#if defined(DEBUG) && DEBUG > 1 std::cout << "Send PoseMsg [" << (int)this->networkId << "/" << (int)this->thingId << "] " << (int)this->poseType << std::endl; #endif diff --git a/Participant.cpp b/Participant.cpp index 7166ad0..f1a7847 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -69,7 +69,7 @@ Participant* Participant::AddParticipant(const char* ipAddress, unsigned int por void Participant::AddParticipant(Participant* participant) { Participant* foundParticipant = - Participant::GetParticipant(participant->networkId); + Participant::GetParticipant(participant->ipAddress, participant->port); if (foundParticipant == nullptr) { #if defined(NO_STD) this->things[this->thingCount++] = thing; @@ -80,7 +80,7 @@ void Participant::AddParticipant(Participant* participant) { << participant->port << "[" << (int)participant->networkId << "]\n"; } else { - std::cout << "Did not add, existing thing " << participant->ipAddress << ":" + std::cout << "Did not add, existing participant " << participant->ipAddress << ":" << participant->port << "[" << (int)participant->networkId << "]\n"; } @@ -143,8 +143,8 @@ void Participant::Remove(Thing* thing) { this->thingCount = lastThingIx; #else this->things.remove_if([thing](Thing* obj) { return obj == thing; }); - std::cout << "Removing " << thing->networkId << "/" << thing->id - << " list size = " << this->things.size() << "\n"; + std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id + << "] list size = " << this->things.size() << "\n"; #endif } diff --git a/Participant.h b/Participant.h index be7133e..de9a66a 100644 --- a/Participant.h +++ b/Participant.h @@ -35,7 +35,7 @@ class Participant { virtual void Update(unsigned long currentTimeMs = 0); - protected: + public: #if defined(NO_STD) unsigned char thingCount = 0; Thing* things[MAX_THING_COUNT]; diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index da55b48..6394dc5 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -5,6 +5,7 @@ #include "Arduino/ArduinoParticipant.h" #include "EspIdf/EspIdfParticipant.h" + #if defined(_WIN32) || defined(_WIN64) #include #include @@ -102,18 +103,53 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { this->ReceiveUDP(); } + UpdateMyThings(currentTimeMs); + UpdateOtherThings(currentTimeMs); +} + +void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { for (Thing* thing : this->things) { - if (thing == nullptr) + if (thing == nullptr || thing->GetParent() != nullptr) continue; thing->Update(currentTimeMs, true); - if (this->isIsolated == false && thing->owner != this) { + if (this->isIsolated || this->networkId == 0) + continue; + + if (thing->isTerminated) { + DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); + this->Send(this->remoteSite, destroyMsg); + delete destroyMsg; + this->Remove(thing); + } else { + // Send to remote site PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - this->Send(thing->owner, poseMsg); - BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); - this->Send(thing->owner, binaryMsg); + this->Send(this->remoteSite, poseMsg); delete poseMsg; + BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); + this->Send(this->remoteSite, binaryMsg); + delete binaryMsg; + } + } +} + +void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) { + for (Participant* participant : Participant::participants) { + if (participant == nullptr || participant == this) + continue; + + participant->Update(currentTimeMs); + if (this->isIsolated) + continue; + + for (Thing* thing : participant->things) { + PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing); + this->Send(participant, poseMsg); + delete poseMsg; + BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing); + this->Send(participant, binaryMsg); + delete binaryMsg; } } } @@ -238,8 +274,8 @@ bool ParticipantUDP::Publish(IMessage* msg) { void ParticipantUDP::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) { - std::cout << "Receive data from " << senderIpAddress << ":" << senderPort - << std::endl; + // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort + // << std::endl; Participant* sender = this->GetParticipant(senderIpAddress, senderPort); if (sender == nullptr) { sender = this->AddParticipant(senderIpAddress, senderPort); @@ -394,6 +430,17 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { std::cout << this->name << ": process PoseMsg [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n"; #endif + Participant* owner = Participant::GetParticipant(msg->networkId); + if (owner == nullptr) + return; + + Thing* thing = owner->Get(msg->thingId); + if (thing == nullptr) + return; + + if ((msg->poseType & PoseMsg::Pose_Position) != 0) + thing->SetPosition(msg->position); + std::cout << "update position for" << (int)thing->id << std::endl; } void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { @@ -409,10 +456,14 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { thing->ProcessBinary(msg->data); #if !defined(NO_STD) else { +#if defined(DEBUG) std::cout << " unknown thing [" << (int)msg->networkId << "/" << (int)msg->thingId << "]"; +#endif } +#if defined(DEBUG) std::cout << std::endl; +#endif #endif } } diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index f50187e..375a3b0 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -2,6 +2,7 @@ #include "Messages/BinaryMsg.h" #include "Messages/InvestigateMsg.h" +#include "Messages/DestroyMsg.h" #include "Messages/ModelUrlMsg.h" #include "Messages/NameMsg.h" #include "Messages/ParticipantMsg.h" @@ -100,6 +101,8 @@ class ParticipantUDP : public Participant { bool connected = false; virtual void Update(unsigned long currentTimeMs = 0) override; + virtual void UpdateMyThings(unsigned long currentTimeMs); + virtual void UpdateOtherThings(unsigned long currentTimeMs); void SendThingInfo(Participant* remoteParticipant, Thing* thing); void PublishThingInfo(Thing* thing); diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index e300371..7484409 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -16,17 +16,31 @@ SiteServer::SiteServer(int port) { this->ipAddress = "0.0.0.0"; this->port = port; -// #if defined(NO_STD) -// this->senders[this->senderCount++] = this; -// #else -// this->senders.push_back(this); -// #endif - SetupUDP(port, ipAddress, 0); +} -#if !defined(NO_STD) - // Register((unsigned char)Thing::Type::TemperatureSensor); -#endif +void SiteServer::UpdateMyThings(unsigned long currentTimeMs) { + for (Thing* thing : this->things) { + if (thing == nullptr) + continue; + + thing->Update(currentTimeMs, true); + + if (this->isIsolated == false) { + // Send to all other participants + for (Participant* participant : Participant::participants) { + if (participant == nullptr || participant == this) + continue; + + PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); + this->Send(participant, poseMsg); + delete poseMsg; + BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); + this->Send(participant, binaryMsg); + delete binaryMsg; + } + } + } } void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { @@ -46,8 +60,7 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing* thing = sender->Get(msg->thingId); if (thing == nullptr) { // #if defined(NO_STD) - new Thing(sender, (Thing::Type)msg->thingType, - msg->thingId); + new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); // #else // auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType); // //Thing* newThing; diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index 0fe2974..819b97b 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -15,18 +15,7 @@ class SiteServer : public ParticipantUDP { public: SiteServer(int port = 7681); - // virtual void Update(unsigned long currentTimeMs = 0) override; - -// #if !defined(NO_STD) -// template -// void Register(unsigned char thingType) { -// thingMsgProcessors[thingType] = [](Participant* participant, -// unsigned char networkId, -// unsigned char thingId) { -// return new ThingClass(participant, networkId, thingId); -// }; -// }; -// #endif + virtual void UpdateMyThings(unsigned long currentTimeMs) override; protected: unsigned long nextPublishMe = 0; @@ -34,13 +23,6 @@ class SiteServer : public ParticipantUDP { virtual void Process(Participant* sender, ParticipantMsg* msg) override; virtual void Process(Participant* sender, SiteMsg* msg) override; virtual void Process(Participant* sender, ThingMsg* msg) override; - -// #if !defined(NO_STD) -// using ThingConstructor = std::function; -// std::unordered_map thingMsgProcessors; -// #endif }; } // namespace RoboidControl diff --git a/Thing.cpp b/Thing.cpp index d1aeaa2..4764435 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -17,12 +17,6 @@ namespace RoboidControl { -// LocalParticipant* Thing::CheckHiddenParticipant() { -// if (isolatedParticipant == nullptr) -// isolatedParticipant = new LocalParticipant(0); -// return isolatedParticipant; -// } - Thing::Thing(int thingType) : Thing(IsolatedParticipant::Isolated(), thingType) {} @@ -36,7 +30,9 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { this->networkId = 0; this->position = Spherical::zero; + this->positionUpdated = true; this->orientation = SwingTwist::identity; + this->orientationUpdated = true; this->linearVelocity = Spherical::zero; this->angularVelocity = Spherical::zero; @@ -70,6 +66,7 @@ Thing::Thing(Thing* parent, int thingType, unsigned char thingId) // } void Thing::Terminate() { + this->isTerminated = true; // Thing::Remove(this); } @@ -244,8 +241,10 @@ SwingTwist Thing::GetOrientation() { } void Thing::SetLinearVelocity(Spherical linearVelocity) { - this->linearVelocity = linearVelocity; - this->linearVelocityUpdated = true; + if (this->linearVelocity.distance != linearVelocity.distance) { + this->linearVelocity = linearVelocity; + this->linearVelocityUpdated = true; + } } Spherical Thing::GetLinearVelocity() { @@ -253,8 +252,10 @@ Spherical Thing::GetLinearVelocity() { } void Thing::SetAngularVelocity(Spherical angularVelocity) { - this->angularVelocity = angularVelocity; - this->angularVelocityUpdated = true; + if (this->angularVelocity.distance != angularVelocity.distance) { + this->angularVelocity = angularVelocity; + this->angularVelocityUpdated = true; + } } Spherical Thing::GetAngularVelocity() { diff --git a/Thing.h b/Thing.h index d08bd04..bd748c9 100644 --- a/Thing.h +++ b/Thing.h @@ -174,6 +174,7 @@ class Thing { public: /// @brief Terminated things are no longer updated void Terminate(); + bool isTerminated = false; /// @brief Sets the location from where the 3D model of this Thing can be /// loaded from diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index b5743f6..e068142 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -15,8 +15,8 @@ int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { } void TouchSensor::ProcessBinary(char* bytes) { - // if (bytes[0] == 1) - // std::cout << this->name << " is Touching something!\n"; + if (bytes[0] == 1) + std::cout << this->name << " is Touching something!\n"; this->touchedSomething |= (bytes[0] == 1); } From a292179f87071860f62f3a6f3e0d489238664c44 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 23 Apr 2025 12:47:40 +0200 Subject: [PATCH 06/56] Steps toward hierarchy changes --- Arduino/Things/DRV8833.cpp | 4 +- Arduino/Things/UltrasonicSensor.cpp | 2 +- Messages/PoseMsg.cpp | 4 +- Participant.cpp | 16 ++++++-- Participants/ParticipantUDP.cpp | 63 +++++++++++++++++------------ Thing.cpp | 26 ++++-------- Thing.h | 5 ++- Things/DifferentialDrive.cpp | 37 ++++++++--------- 8 files changed, 83 insertions(+), 74 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 8e03f3c..17579d8 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -14,7 +14,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver, unsigned char pinIn2, bool reverse) : Thing(driver->owner) { - this->parent = driver; + this->SetParent(driver); this->pinIn1 = pinIn1; this->pinIn2 = pinIn2; @@ -144,7 +144,7 @@ DRV8833::DRV8833(Thing* parent, pinStandby, reverseA, reverseB) { - this->parent = parent; + this->SetParent(parent); } } // namespace Arduino diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 0d1275c..646a4f8 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -20,7 +20,7 @@ UltrasonicSensor::UltrasonicSensor(Thing* parent, unsigned char pinTrigger, unsigned char pinEcho) : UltrasonicSensor(parent->owner, pinTrigger, pinEcho) { - this->parent = parent; + this->SetParent(parent); } float UltrasonicSensor::GetDistance() { diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index 3edd823..c476388 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { this->thingId = thing->id; this->poseType = 0; - if (thing->positionUpdated || (force && thing->GetLinearVelocity().distance == 0)) { + if (thing->positionUpdated || (force && thing->GetParent() != nullptr)) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; } - if (thing->orientationUpdated || (force && thing->GetAngularVelocity().distance == 0)) { + if (thing->orientationUpdated || (force && thing->GetParent() != nullptr)) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; } diff --git a/Participant.cpp b/Participant.cpp index f1a7847..bd38901 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -45,8 +45,8 @@ Participant* Participant::GetParticipant(const char* ipAddress, participant->port == port) return participant; } - std::cout << "Could not find participant " << ipAddress << ":" << (int)port - << std::endl; + // std::cout << "Could not find participant " << ipAddress << ":" << (int)port + // << std::endl; return nullptr; } @@ -57,7 +57,7 @@ Participant* Participant::GetParticipant(unsigned char participantId) { if (participant->networkId == participantId) return participant; } - std::cout << "Could not find participant " << (int)participantId << std::endl; + // std::cout << "Could not find participant " << (int)participantId << std::endl; return nullptr; } @@ -104,7 +104,15 @@ void Participant::Add(Thing* thing, bool checkId) { thing->id = this->thingCount + 1; this->things[this->thingCount++] = thing; #else - thing->id = (unsigned char)this->things.size() + 1; + // find highest id + int highestIx = 0; + for (Thing* thing : this->things) { + if (thing == nullptr) + continue; + if (thing->id > highestIx) + highestIx = thing->id; + } + thing->id = highestIx + 1; this->things.push_back(thing); #endif // std::cout << "Add thing with generated ID " << this->ipAddress << ":" diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 6394dc5..780701a 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -5,7 +5,6 @@ #include "Arduino/ArduinoParticipant.h" #include "EspIdf/EspIdfParticipant.h" - #if defined(_WIN32) || defined(_WIN64) #include #include @@ -109,28 +108,41 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { for (Thing* thing : this->things) { - if (thing == nullptr || thing->GetParent() != nullptr) + if (thing == nullptr) // || thing->GetParent() != nullptr) continue; - thing->Update(currentTimeMs, true); - - if (this->isIsolated || this->networkId == 0) - continue; - - if (thing->isTerminated) { - DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); - this->Send(this->remoteSite, destroyMsg); - delete destroyMsg; - this->Remove(thing); - } else { - // Send to remote site - PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - this->Send(this->remoteSite, poseMsg); - delete poseMsg; - BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); - this->Send(this->remoteSite, binaryMsg); - delete binaryMsg; + if (thing->hierarchyChanged) { + std::cout << "thing hierarchy changed " << (int)thing->id << std::endl; + if (!(this->isIsolated || this->networkId == 0)) { + ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); + this->Send(this->remoteSite, thingMsg); + delete thingMsg; + } } + + // 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! + thing->Update(currentTimeMs, false); + + if (!(this->isIsolated || this->networkId == 0)) { + if (thing->isTerminated) { + DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); + this->Send(this->remoteSite, destroyMsg); + delete destroyMsg; + } else { + // Send to remote site + PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); + this->Send(this->remoteSite, poseMsg); + delete poseMsg; + BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); + this->Send(this->remoteSite, binaryMsg); + delete binaryMsg; + } + } + if (thing->isTerminated) + this->Remove(thing); } } @@ -139,7 +151,7 @@ void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) { if (participant == nullptr || participant == this) continue; - participant->Update(currentTimeMs); + //participant->Update(currentTimeMs); if (this->isIsolated) continue; @@ -363,12 +375,13 @@ void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { << " -> " << (int)msg->networkId << "\n"; #endif - if (this->networkId != msg->networkId) + if (this->networkId != msg->networkId) { this->networkId = msg->networkId; - // std::cout << this->things.size() << " things\n"; - for (Thing* thing : this->things) - this->SendThingInfo(sender, thing); + // std::cout << this->things.size() << " things\n"; + for (Thing* thing : this->things) + this->SendThingInfo(sender, thing); + } } void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) { diff --git a/Thing.cpp b/Thing.cpp index 4764435..ca3a212 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -33,6 +33,7 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { this->positionUpdated = true; this->orientation = SwingTwist::identity; this->orientationUpdated = true; + this->hierarchyChanged = true; this->linearVelocity = Spherical::zero; this->angularVelocity = Spherical::zero; @@ -45,29 +46,11 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { Thing::Thing(Thing* parent, int thingType, unsigned char thingId) : Thing(parent->owner, thingType, thingId) { - this->parent = parent; + this->SetParent(parent); } -// Thing::Thing(Participant* owner, -// Type thingType, -// int thingId) { -// // no participant reference yet.. -// this->owner = owner; -// this->networkId = networkId; -// this->id = thingId; -// this->type = (unsigned char)thingType; - -// this->linearVelocity = Spherical::zero; -// this->angularVelocity = Spherical::zero; -// // std::cout << "Created thing " << (int)this->networkId << "/" << -// // (int)this->id -// // << "\n"; -// owner->Add(this, false); -// } - void Thing::Terminate() { this->isTerminated = true; - // Thing::Remove(this); } Thing* Thing::FindThing(const char* name) { @@ -94,6 +77,8 @@ void Thing::SetParent(Thing* parent) { this->parent = nullptr; } else parent->AddChild(this); + std::cout << "setting parent for " << (int) this->id << std::endl; + this->hierarchyChanged = true; } void Thing::SetParent(Thing* root, const char* name) { @@ -199,6 +184,9 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) { // OnPoseChanged callback this->positionUpdated = false; this->orientationUpdated = false; + // this->linearVelocityUpdated = false; + // this->angularVelocityUpdated = false; + this->hierarchyChanged = false; if (recursive) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { diff --git a/Thing.h b/Thing.h index bd748c9..0d45d65 100644 --- a/Thing.h +++ b/Thing.h @@ -107,7 +107,10 @@ class Thing { /// @return The found thing of nullptr when nothing is found Thing* GetChildByIndex(unsigned char ix); - protected: + /// @brief Indicator that the hierarchy of the thing has changed + bool hierarchyChanged = true; + + private: Thing* parent = nullptr; Thing** children = nullptr; diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index bd09157..ad7ad14 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -42,30 +42,27 @@ void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) { } void DifferentialDrive::Update(unsigned long currentMs, bool recursive) { - if (this->linearVelocityUpdated == false) - return; - // this assumes forward velocity only.... - float linearVelocity = this->GetLinearVelocity().distance; + if (this->linearVelocityUpdated) { + // this assumes forward velocity only.... + float linearVelocity = this->GetLinearVelocity().distance; - Spherical angularVelocity = this->GetAngularVelocity(); - float angularSpeed = angularVelocity.distance * Deg2Rad; // in degrees/sec - // Determine the rotation direction - if (angularVelocity.direction.horizontal.InDegrees() < 0) - angularSpeed = -angularSpeed; + Spherical angularVelocity = this->GetAngularVelocity(); + float angularSpeed = angularVelocity.distance * Deg2Rad; // in degrees/sec + // Determine the rotation direction + if (angularVelocity.direction.horizontal.InDegrees() < 0) + angularSpeed = -angularSpeed; - // wheel separation can be replaced by this->leftwheel->position->distance - float speedLeft = - (linearVelocity + angularSpeed * this->wheelSeparation / 2) / - this->wheelRadius * Rad2Deg; - float speedRight = - (linearVelocity - angularSpeed * this->wheelSeparation / 2) / - this->wheelRadius * Rad2Deg; + // wheel separation can be replaced by this->leftwheel->position->distance + float speedLeft = + (linearVelocity + angularSpeed * this->wheelSeparation / 2) / + this->wheelRadius * Rad2Deg; + float speedRight = + (linearVelocity - angularSpeed * this->wheelSeparation / 2) / + this->wheelRadius * Rad2Deg; - this->SetWheelVelocity(speedLeft, speedRight); + this->SetWheelVelocity(speedLeft, speedRight); + } Thing::Update(currentMs, recursive); - // std::cout << "lin. speed " << linearVelocity << " ang. speed " << - // angularVelocity.distance << " left wheel " - // << speedLeft << " right wheel " << speedRight << "\n"; } } // namespace RoboidControl \ No newline at end of file From 693c9b8b33baaccdf3d3af4bdf5cf1c73a3a15b2 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 23 Apr 2025 15:05:38 +0200 Subject: [PATCH 07/56] Object creation/destruction works --- Arduino/Things/DRV8833.cpp | 4 +- Messages/NameMsg.cpp | 7 ++-- Participant.cpp | 17 +------- Participants/ParticipantUDP.cpp | 69 ++++++++++++++++++++------------- Participants/SiteServer.cpp | 35 ++++++++++------- Thing.cpp | 15 +++++-- Thing.h | 9 ++++- Things/TouchSensor.cpp | 2 +- 8 files changed, 91 insertions(+), 67 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 17579d8..dfd659c 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -123,9 +123,9 @@ DRV8833::DRV8833(Participant* participant, pinMode(pinStandby, OUTPUT); this->motorA = new DRV8833Motor(this, pinAIn1, pinAIn2, reverseA); - this->motorA->name = "Motor A"; + this->motorA->SetName("Motor A"); this->motorB = new DRV8833Motor(this, pinBIn1, pinBIn2, reverseB); - this->motorB->name = "Motor B"; + this->motorB->SetName("Motor B"); } DRV8833::DRV8833(Thing* parent, diff --git a/Messages/NameMsg.cpp b/Messages/NameMsg.cpp index da91c79..84aba94 100644 --- a/Messages/NameMsg.cpp +++ b/Messages/NameMsg.cpp @@ -7,15 +7,16 @@ namespace RoboidControl { NameMsg::NameMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; - if (thing->name == nullptr) + const char* thingName = thing->GetName(); + if (thingName == nullptr) this->nameLength = 0; else - this->nameLength = (unsigned char)strlen(thing->name); + this->nameLength = (unsigned char)strlen(thingName); // the name string in the buffer is not \0 terminated! char* name = new char[this->nameLength + 1]; for (int i = 0; i < this->nameLength; i++) - name[i] = thing->name[i]; + name[i] = thingName[i]; name[this->nameLength] = '\0'; this->name = name; } diff --git a/Participant.cpp b/Participant.cpp index bd38901..53ee6d4 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -151,22 +151,9 @@ void Participant::Remove(Thing* thing) { this->thingCount = lastThingIx; #else this->things.remove_if([thing](Thing* obj) { return obj == thing; }); - std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id - << "] list size = " << this->things.size() << "\n"; + // std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id + // << "] list size = " << this->things.size() << "\n"; #endif } -// void Participant::UpdateAll(unsigned long currentTimeMs) { -// // Not very efficient, but it works for now. - -// for (Thing* thing : this->things) { -// if (thing != nullptr && thing->GetParent() == nullptr) { // update all -// root things -// // std::cout << " update " << (int)ix << " thingid " << (int)thing->id -// // << "\n"; -// thing->Update(currentTimeMs); -// } -// } -// } - } // namespace RoboidControl diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 780701a..efe27d2 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -80,6 +80,8 @@ void ParticipantUDP::SetupUDP(int localPort, this->connected = true; } +#pragma region Update + void ParticipantUDP::Update(unsigned long currentTimeMs) { if (currentTimeMs == 0) currentTimeMs = Thing::GetTimeMs(); @@ -112,12 +114,17 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { continue; if (thing->hierarchyChanged) { - std::cout << "thing hierarchy changed " << (int)thing->id << std::endl; if (!(this->isIsolated || this->networkId == 0)) { - ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); - this->Send(this->remoteSite, thingMsg); - delete thingMsg; + ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); + this->Send(this->remoteSite, thingMsg); + delete thingMsg; + + if (thing->nameChanged) { + NameMsg* nameMsg = new NameMsg(this->networkId, thing); + this->Send(this->remoteSite, nameMsg); + delete nameMsg; } + } } // Why don't we do recursive? @@ -127,12 +134,17 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { thing->Update(currentTimeMs, false); if (!(this->isIsolated || this->networkId == 0)) { - if (thing->isTerminated) { + if (thing->terminate) { DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); this->Send(this->remoteSite, destroyMsg); delete destroyMsg; } else { // Send to remote site + if (thing->nameChanged) { + NameMsg* nameMsg = new NameMsg(this->networkId, thing); + this->Send(this->remoteSite, nameMsg); + delete nameMsg; + } PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); this->Send(this->remoteSite, poseMsg); delete poseMsg; @@ -141,7 +153,7 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { delete binaryMsg; } } - if (thing->isTerminated) + if (thing->terminate) this->Remove(thing); } } @@ -151,7 +163,7 @@ void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) { if (participant == nullptr || participant == this) continue; - //participant->Update(currentTimeMs); + participant->Update(currentTimeMs); if (this->isIsolated) continue; @@ -166,24 +178,8 @@ void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) { } } -void ParticipantUDP::ReceiveUDP() { -#if defined(_WIN32) || defined(_WIN64) - Windows::ParticipantUDP* thisWindows = - static_cast(this); - thisWindows->Receive(); -#elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = static_cast(this); - thisPosix->Receive(); -#elif defined(ARDUINO) - Arduino::ParticipantUDP* thisArduino = - static_cast(this); - thisArduino->Receive(); -#elif defined(IDF_VER) - EspIdf::ParticipantUDP* thisEspIdf = - static_cast(this); - thisEspIdf->Receive(); -#endif -} +// Update +#pragma endregion #pragma region Send @@ -283,6 +279,25 @@ bool ParticipantUDP::Publish(IMessage* msg) { #pragma region Receive +void ParticipantUDP::ReceiveUDP() { +#if defined(_WIN32) || defined(_WIN64) + Windows::ParticipantUDP* thisWindows = + static_cast(this); + thisWindows->Receive(); +#elif defined(__unix__) || defined(__APPLE__) + Posix::ParticipantUDP* thisPosix = static_cast(this); + thisPosix->Receive(); +#elif defined(ARDUINO) + Arduino::ParticipantUDP* thisArduino = + static_cast(this); + thisArduino->Receive(); +#elif defined(IDF_VER) + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); + thisEspIdf->Receive(); +#endif +} + void ParticipantUDP::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) { @@ -420,10 +435,10 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { nameLength); // Leave space for null terminator #endif thingName[nameLength] = '\0'; - thing->name = thingName; + thing->SetName(thingName); #if !defined(NO_STD) - std::cout << thing->name; + std::cout << thing->GetName(); #endif } #if !defined(NO_STD) diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 7484409..39a859b 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -9,6 +9,8 @@ namespace RoboidControl { +#pragma region Init + SiteServer::SiteServer(int port) { this->name = "Site Server"; this->publishInterval = 0; @@ -19,6 +21,10 @@ SiteServer::SiteServer(int port) { SetupUDP(port, ipAddress, 0); } +#pragma endregion Init + +#pragma region Update + void SiteServer::UpdateMyThings(unsigned long currentTimeMs) { for (Thing* thing : this->things) { if (thing == nullptr) @@ -43,8 +49,12 @@ void SiteServer::UpdateMyThings(unsigned long currentTimeMs) { } } +#pragma endregion Update + +#pragma region Receive + void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { - if (msg->networkId == 0) { + if (msg->networkId != sender->networkId) { // std::cout << this->name << " received New Client -> " << // sender->ipAddress // << ":" << (int)sender->port << "\n"; @@ -59,21 +69,18 @@ void SiteServer::Process(Participant* sender, SiteMsg* msg) {} void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing* thing = sender->Get(msg->thingId); if (thing == nullptr) { - // #if defined(NO_STD) new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); - // #else - // auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType); - // //Thing* newThing; - // if (thingMsgProcessor != thingMsgProcessors.end()) // found item - // //newThing = - // thingMsgProcessor->second(sender, msg->networkId, - // msg->thingId); - // else - // //newThing = - // new Thing(sender, msg->networkId, msg->thingId, - // (Thing::Type)msg->thingType); - // #endif + + if (msg->parentId != 0) { + thing->SetParent(Get(msg->parentId)); + if (thing->GetParent() != nullptr) + std::cout << "Could not find parent [" << (int)msg->networkId << "/" + << (int)msg->parentId << "]\n"; + } else + thing->SetParent(nullptr); } } +#pragma endregion Receive + } // namespace RoboidControl diff --git a/Thing.cpp b/Thing.cpp index ca3a212..65579f6 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -49,8 +49,13 @@ Thing::Thing(Thing* parent, int thingType, unsigned char thingId) this->SetParent(parent); } -void Thing::Terminate() { - this->isTerminated = true; +void Thing::SetName(const char* name) { + this->name = name; + this->nameChanged = true; +} + +const char* Thing::GetName() const { + return this->name; } Thing* Thing::FindThing(const char* name) { @@ -77,7 +82,6 @@ void Thing::SetParent(Thing* parent) { this->parent = nullptr; } else parent->AddChild(this); - std::cout << "setting parent for " << (int) this->id << std::endl; this->hierarchyChanged = true; } @@ -164,6 +168,8 @@ void Thing::SetModel(const char* url) { this->modelUrl = url; } +#pragma region Update + unsigned long Thing::GetTimeMs() { #if defined(ARDUINO) return millis(); @@ -187,6 +193,7 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) { // this->linearVelocityUpdated = false; // this->angularVelocityUpdated = false; this->hierarchyChanged = false; + this->nameChanged = false; if (recursive) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { @@ -202,6 +209,8 @@ void Thing::UpdateThings(unsigned long currentTimeMs) { IsolatedParticipant::Isolated()->Update(currentTimeMs); } +#pragma endregion Update + int Thing::GenerateBinary(char* buffer, unsigned char* ix) { (void)buffer; (void)ix; diff --git a/Thing.h b/Thing.h index 0d45d65..21c557b 100644 --- a/Thing.h +++ b/Thing.h @@ -114,9 +114,14 @@ class Thing { Thing* parent = nullptr; Thing** children = nullptr; - public: /// @brief The name of the thing const char* name = nullptr; + + public: + void SetName(const char* name); + const char* GetName() const; + bool nameChanged = false; + /// @brief An URL pointing to the location where a model of the thing can be /// found const char* modelUrl = nullptr; @@ -177,7 +182,7 @@ class Thing { public: /// @brief Terminated things are no longer updated void Terminate(); - bool isTerminated = false; + bool terminate = false; /// @brief Sets the location from where the 3D model of this Thing can be /// loaded from diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index e068142..be98aeb 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -16,7 +16,7 @@ int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { void TouchSensor::ProcessBinary(char* bytes) { if (bytes[0] == 1) - std::cout << this->name << " is Touching something!\n"; + std::cout << this->GetName() << " is Touching something!\n"; this->touchedSomething |= (bytes[0] == 1); } From fe694190104d16837750091ecaef42eb74e9a97b Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 23 Apr 2025 17:50:40 +0200 Subject: [PATCH 08/56] Binary msg sending fix --- Messages/BinaryMsg.cpp | 4 +++- Participants/ParticipantUDP.cpp | 5 +---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/Messages/BinaryMsg.cpp b/Messages/BinaryMsg.cpp index d04ac2d..c831753 100644 --- a/Messages/BinaryMsg.cpp +++ b/Messages/BinaryMsg.cpp @@ -6,7 +6,7 @@ BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; this->thing = thing; - unsigned char ix = BinaryMsg::length; + unsigned char ix = 0; //BinaryMsg::length; this->data = new char[255]; this->dataLength = this->thing->GenerateBinary(this->data, &ix); } @@ -41,6 +41,8 @@ unsigned char BinaryMsg::Serialize(char* buffer) { buffer[ix++] = this->networkId; buffer[ix++] = this->thingId; buffer[ix++] = this->dataLength; + for (int dataIx = 0; dataIx < this->dataLength; dataIx++) + buffer[ix++] = this->data[dataIx]; return this->length + this->dataLength; } diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index efe27d2..247b7c9 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -474,7 +474,7 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId - << "/" << (int)msg->thingId << "] "; + << "/" << (int)msg->thingId << "]\n"; #endif Participant* owner = Participant::GetParticipant(msg->networkId); @@ -489,9 +489,6 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { << (int)msg->thingId << "]"; #endif } -#if defined(DEBUG) - std::cout << std::endl; -#endif #endif } } From df1a769d102b222b1f4910628fe20c1962d3a9b7 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 28 Apr 2025 18:13:46 +0200 Subject: [PATCH 09/56] Align participant documentation --- Participant.cpp | 2 -- Participant.h | 54 +++++++++++++++++++++++-------------------------- 2 files changed, 25 insertions(+), 31 deletions(-) diff --git a/Participant.cpp b/Participant.cpp index 53ee6d4..e423608 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -6,8 +6,6 @@ namespace RoboidControl { std::list Participant::participants; -Participant::Participant() {} - Participant::Participant(const char* ipAddress, int port) { // make a copy of the ip address string int addressLength = (int)strlen(ipAddress); diff --git a/Participant.h b/Participant.h index de9a66a..2facd6b 100644 --- a/Participant.h +++ b/Participant.h @@ -13,60 +13,56 @@ constexpr int MAX_THING_COUNT = 256; /// reference to remote participants. class Participant { public: - /// @brief The Ip Address of a participant. When the participant is local, - /// this contains 0.0.0.0 + /// @brief The Ip Address of a participant. const char* ipAddress = "0.0.0.0"; - /// @brief The port number for UDP communication with the participant. This is - /// 0 for isolated participants. + /// @brief The port number for UDP communication with the participant. unsigned int port = 0; - /// @brief The network Id to identify the participant. - /// @note This field is likely to disappear in future versions + /// @brief The network Id to identify the participant unsigned char networkId = 0; - /// @brief Default constructor - Participant(); /// @brief Create a new participant with the given communcation info /// @param ipAddress The IP address of the participant - /// @param port The port of the participant + /// @param port The UDP port of the participant Participant(const char* ipAddress, int port); /// @brief Destructor for the participant ~Participant(); - virtual void Update(unsigned long currentTimeMs = 0); - public: #if defined(NO_STD) unsigned char thingCount = 0; Thing* things[MAX_THING_COUNT]; #else - /// @brief The list of known participants - static std::list participants; - - /// @brief The list of things managed by this participant + /// @brief The things managed by this participant std::list things; #endif + /// @brief Find a thing managed by this participant + /// @param thingId The ID of the thing + /// @return The thing if found, nullptr when no thing has been found + Thing* Get(unsigned char thingId); + /// @brief Add a new thing for this participant. + /// @param thing The thing to add + /// @param checkId If true, the thing.id is regenerated if it is zero + void Add(Thing* thing, bool checkId = true); + /// @brief Remove a thing for this participant + /// @param thing The thing to remove + void Remove(Thing* thing); + + /// @brief Update all things for this participant + /// @param currentTimeMs The current time in milliseconds (optional) + virtual void Update(unsigned long currentTimeMs = 0); public: +#if defined(NO_STD) +#else + /// @brief The list of known participants + static std::list participants; +#endif static Participant* GetParticipant(const char* ipAddress, unsigned int port); static Participant* GetParticipant(unsigned char participantId); static Participant* AddParticipant(const char* ipAddress, unsigned int port); static void AddParticipant(Participant* participant); - /// @brief Find a thing managed by this participant - /// @param networkId The network ID for the thing - /// @param thingId The ID of the thing - /// @return The thing if found or nullptr when no thing has been found - /// @note The use of the network ID is likely to disappear in future versions. - Thing* Get(unsigned char thingId); - /// @brief Add a new thing for this participant. - /// @param thing The thing to add - /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing - /// Id will be assigned. - void Add(Thing* thing, bool checkId = true); - /// @brief Remove a thing for this participant - /// @param thing The thing to remove - void Remove(Thing* thing); }; } // namespace RoboidControl From 478d6d9431ac9721bec46f0471f82ffa70cf770b Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 28 Apr 2025 18:14:03 +0200 Subject: [PATCH 10/56] Unit test fixes --- Arduino/ArduinoParticipant.cpp | 2 ++ Participants/ParticipantUDP.cpp | 4 +--- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index f74a746..b283052 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -19,7 +19,9 @@ namespace RoboidControl { namespace Arduino { +#if defined(ARDUINO) && defined(HAS_WIFI) WiFiUDP* udp; +#endif void ParticipantUDP::Setup() { #if defined(ARDUINO) && defined(HAS_WIFI) diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 247b7c9..f946a00 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -25,9 +25,7 @@ namespace RoboidControl { -ParticipantUDP::ParticipantUDP(int port) { - this->ipAddress = "0.0.0.0"; - this->port = port; +ParticipantUDP::ParticipantUDP(int port) : Participant("0.0.0.0", port) { this->remoteSite = nullptr; if (this->port == 0) this->isIsolated = true; From 75a0cf350dd1b47041f8974918416e34eef0f119 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 29 Apr 2025 11:47:15 +0200 Subject: [PATCH 11/56] Aligned Thing implementation --- README.md | 3 +- Thing.cpp | 154 +++++++++++++++++++++++--------------------- Thing.h | 187 +++++++++++++++++++++++++++++++----------------------- 3 files changed, 189 insertions(+), 155 deletions(-) diff --git a/README.md b/README.md index 28e4abd..e0a3c1a 100644 --- a/README.md +++ b/README.md @@ -14,5 +14,4 @@ Supporting: # Basic components - RoboidControl::Thing -- RoboidControl::LocalParticipant -- RoboidControl::SiteServer \ No newline at end of file +- RoboidControl::Participant \ No newline at end of file diff --git a/Thing.cpp b/Thing.cpp index 65579f6..4fe968a 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -17,17 +17,17 @@ namespace RoboidControl { -Thing::Thing(int thingType) +#pragma region Init + +Thing::Thing(unsigned char thingType) : Thing(IsolatedParticipant::Isolated(), thingType) {} -Thing::Thing(Participant* owner, Type thingType, unsigned char thingId) - : Thing(owner, (unsigned char)thingType, thingId) {} - -Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { +Thing::Thing(Participant* owner, + unsigned char thingType, + unsigned char thingId) { this->owner = owner; this->id = thingId; this->type = thingType; - this->networkId = 0; this->position = Spherical::zero; this->positionUpdated = true; @@ -44,11 +44,13 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { this->owner->Add(this, true); } -Thing::Thing(Thing* parent, int thingType, unsigned char thingId) +Thing::Thing(Thing* parent, unsigned char thingType, unsigned char thingId) : Thing(parent->owner, thingType, thingId) { this->SetParent(parent); } +#pragma endregion Init + void Thing::SetName(const char* name) { this->name = name; this->nameChanged = true; @@ -58,22 +60,12 @@ const char* Thing::GetName() const { return this->name; } -Thing* Thing::FindThing(const char* name) { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing* child = this->children[childIx]; - if (child == nullptr || child->name == nullptr) - continue; - - if (strcmp(child->name, name) == 0) - return child; - - Thing* foundChild = child->FindThing(name); - if (foundChild != nullptr) - return foundChild; - } - return nullptr; +void Thing::SetModel(const char* url) { + this->modelUrl = url; } +#pragma region Hierarchy + void Thing::SetParent(Thing* parent) { if (parent == nullptr) { Thing* parentThing = this->parent; @@ -85,16 +77,20 @@ void Thing::SetParent(Thing* parent) { this->hierarchyChanged = true; } -void Thing::SetParent(Thing* root, const char* name) { - Thing* thing = root->FindThing(name); - if (thing != nullptr) - this->SetParent(thing); -} +// void Thing::SetParent(Thing* root, const char* name) { +// Thing* thing = root->FindChild(name); +// if (thing != nullptr) +// this->SetParent(thing); +// } Thing* Thing::GetParent() { return this->parent; } +Thing* Thing::GetChildByIndex(unsigned char ix) { + return this->children[ix]; +} + void Thing::AddChild(Thing* child) { unsigned char newChildCount = this->childCount + 1; Thing** newChildren = new Thing*[newChildCount]; @@ -143,7 +139,7 @@ Thing* Thing::RemoveChild(Thing* child) { return child; } -Thing* Thing::GetChild(unsigned char id, bool recursive) { +Thing* Thing::GetChild(unsigned char id, bool recurse) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { Thing* child = this->children[childIx]; if (child == nullptr) @@ -151,8 +147,8 @@ Thing* Thing::GetChild(unsigned char id, bool recursive) { if (child->id == id) return child; - if (recursive) { - Thing* foundChild = child->GetChild(id, recursive); + if (recurse) { + Thing* foundChild = child->GetChild(id, recurse); if (foundChild != nullptr) return foundChild; } @@ -160,13 +156,66 @@ Thing* Thing::GetChild(unsigned char id, bool recursive) { return nullptr; } -Thing* Thing::GetChildByIndex(unsigned char ix) { - return this->children[ix]; +Thing* Thing::FindChild(const char* name, bool recurse) { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + Thing* child = this->children[childIx]; + if (child == nullptr || child->name == nullptr) + continue; + + if (strcmp(child->name, name) == 0) + return child; + + Thing* foundChild = child->FindChild(name); + if (foundChild != nullptr) + return foundChild; + } + return nullptr; } -void Thing::SetModel(const char* url) { - this->modelUrl = url; +#pragma endregion Hierarchy + +#pragma region Pose + +void Thing::SetPosition(Spherical position) { + this->position = position; + this->positionUpdated = true; } +Spherical Thing::GetPosition() { + return this->position; +} + +void Thing::SetOrientation(SwingTwist orientation) { + this->orientation = orientation; + this->orientationUpdated = true; +} + +SwingTwist Thing::GetOrientation() { + return this->orientation; +} + +void Thing::SetLinearVelocity(Spherical linearVelocity) { + if (this->linearVelocity.distance != linearVelocity.distance) { + this->linearVelocity = linearVelocity; + this->linearVelocityUpdated = true; + } +} + +Spherical Thing::GetLinearVelocity() { + return this->linearVelocity; +} + +void Thing::SetAngularVelocity(Spherical angularVelocity) { + if (this->angularVelocity.distance != angularVelocity.distance) { + this->angularVelocity = angularVelocity; + this->angularVelocityUpdated = true; + } +} + +Spherical Thing::GetAngularVelocity() { + return this->angularVelocity; +} + +#pragma endregion Pose #pragma region Update @@ -220,43 +269,4 @@ void Thing::ProcessBinary(char* bytes) { (void)bytes; }; -void Thing::SetPosition(Spherical position) { - this->position = position; - this->positionUpdated = true; -} -Spherical Thing::GetPosition() { - return this->position; -} - -void Thing::SetOrientation(SwingTwist orientation) { - this->orientation = orientation; - this->orientationUpdated = true; -} - -SwingTwist Thing::GetOrientation() { - return this->orientation; -} - -void Thing::SetLinearVelocity(Spherical linearVelocity) { - if (this->linearVelocity.distance != linearVelocity.distance) { - this->linearVelocity = linearVelocity; - this->linearVelocityUpdated = true; - } -} - -Spherical Thing::GetLinearVelocity() { - return this->linearVelocity; -} - -void Thing::SetAngularVelocity(Spherical angularVelocity) { - if (this->angularVelocity.distance != angularVelocity.distance) { - this->angularVelocity = angularVelocity; - this->angularVelocityUpdated = true; - } -} - -Spherical Thing::GetAngularVelocity() { - return this->angularVelocity; -} - } // namespace RoboidControl \ No newline at end of file diff --git a/Thing.h b/Thing.h index 21c557b..2fac262 100644 --- a/Thing.h +++ b/Thing.h @@ -20,7 +20,7 @@ class ParticipantUDP; class Thing { public: /// @brief Predefined thing types - enum Type { + enum Type : unsigned char { Undetermined, // Sensor, Switch, @@ -38,54 +38,86 @@ class Thing { ExternalSensor, }; - /// @brief Create a new thing using an implicit local participant - /// @param thingType The type of thing - Thing(int thingType = Type::Undetermined); - /// @brief Create a new thing of the given type - /// @param thingType The predefined type of thing - Thing(Participant* participant, - Type thingType = Type::Undetermined, +#pragma region Init + + /// @brief Create a new thing without communication abilities + /// @param thingType The type of thing (can use Thing::Type) + Thing(unsigned char thingType = Type::Undetermined); + + /// @brief Create a new thing for a participant + /// @param owner The owning participant + /// @param thingType The type of thing (can use Thing::Type) + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + Thing(Participant* owner, + unsigned char thingType = Type::Undetermined, unsigned char thingId = 0); - /// @brief Create a new thing of the give type - /// @param thingType The custom type of the thing - Thing(Participant* participant, int thingType, unsigned char thingId = 0); - /// @brief Create a new thing for the given participant - /// @param participant The participant for which this thing is created - /// @param networkId The network ID of the thing - /// @param thingId The ID of the thing - /// @param thingType The type of thing - // Thing(Participant* participant, - // unsigned char networkId, - // unsigned char thingId, - // Type thingType = Type::Undetermined); - Thing(Thing* parent, int thingType = 0, unsigned char thingId = 0); + + /// @brief Create a new child thing + /// @param parent The parent thing + /// @param thingType The type of thing (can use Thing::Type) + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + /// @note The owner will be the same as the owner of the parent thing + Thing(Thing* parent, unsigned char thingType = 0, unsigned char thingId = 0); + +#pragma endregion Init + + public: + /// @brief Terminated things are no longer updated + void Terminate(); + bool terminate = false; + +#pragma region Properties /// @brief The participant managing this thing - Participant* owner; - /// @brief The network ID of this thing - /// @note This field will likely disappear in future versions - unsigned char networkId = 0; + Participant* owner = nullptr; /// @brief The ID of the thing unsigned char id = 0; /// @brief The type of Thing /// This can be either a Thing::Type of a byte value for custom types - unsigned char type = 0; + unsigned char type = Type::Undetermined; - /// @brief Find a thing by name - /// @param name Rhe name of the thing - /// @return The found thing or nullptr when nothing is found - Thing* FindThing(const char* name); + /// @brief The name of the thing + const char* name = nullptr; - /// @brief Sets the parent Thing - /// @param parent The Thing which should become the parnet - /// @remark This is equivalent to calling parent->AddChild(this); + public: + void SetName(const char* name); + const char* GetName() const; + bool nameChanged = false; + + /// @brief Sets the location from where the 3D model of this Thing can be + /// loaded from + /// @param url The url of the model + /// @remark Although the roboid implementation is not dependent on the model, + /// the only official supported model format is .obj + void SetModel(const char* url); + + /// @brief An URL pointing to the location where a model of the thing can be + /// found + const char* modelUrl = nullptr; + /// @brief The scale of the model (deprecated I think) + float modelScale = 1; + +#pragma endregion Properties + +#pragma region Hierarchy + + /// @brief Sets the parent of this Thing + /// @param parent The Thing which should become the parent virtual void SetParent(Thing* parent); - void SetParent(Thing* root, const char* name); - /// @brief Gets the parent Thing + /// @brief Gets the parent of this Thing /// @return The parent Thing Thing* GetParent(); + /// @brief The number of children + unsigned char childCount = 0; + /// @brief Get a child by index + /// @param ix The child index + /// @return The found thing of nullptr when nothing is found + Thing* GetChildByIndex(unsigned char ix); + /// @brief Add a child Thing to this Thing /// @param child The Thing which should become a child /// @remark When the Thing is already a child, it will not be added again @@ -95,17 +127,17 @@ class Thing { /// @return The removed child or nullptr if the child could not be found Thing* RemoveChild(Thing* child); - /// @brief The number of children - unsigned char childCount = 0; /// @brief Get a child by thing Id /// @param id The thing ID to find - /// @param recursive Look recursively through all descendants + /// @param recurse Look recursively through all descendants /// @return The found thing of nullptr when nothing is found - Thing* GetChild(unsigned char id, bool recursive = false); - /// @brief Get a child by index - /// @param ix The child index - /// @return The found thing of nullptr when nothing is found - Thing* GetChildByIndex(unsigned char ix); + Thing* GetChild(unsigned char id, bool recurse = false); + + /// @brief Find a thing by name + /// @param name The name of the thing + /// @param recurse Look recursively through all descendants + /// @return The found thing or nullptr when nothing is found + Thing* FindChild(const char* name, bool recurse = true); /// @brief Indicator that the hierarchy of the thing has changed bool hierarchyChanged = true; @@ -114,38 +146,27 @@ class Thing { Thing* parent = nullptr; Thing** children = nullptr; - /// @brief The name of the thing - const char* name = nullptr; +#pragma endregion Hierarchy + +#pragma region Pose public: - void SetName(const char* name); - const char* GetName() const; - bool nameChanged = false; - - /// @brief An URL pointing to the location where a model of the thing can be - /// found - const char* modelUrl = nullptr; - /// @brief The scale of the model (deprecated I think) - float modelScale = 1; - /// @brief Set the position of the thing /// @param position The new position in local space, in meters void SetPosition(Spherical position); /// @brief Get the position of the thing /// @return The position in local space, in meters Spherical GetPosition(); + /// @brief Boolean indicating that the thing has an updated position + bool positionUpdated = false; + /// @brief Set the orientation of the thing /// @param orientation The new orientation in local space void SetOrientation(SwingTwist orientation); /// @brief Get the orientation of the thing /// @return The orienation in local space SwingTwist GetOrientation(); - /// @brief The scale of the thing (deprecated I think) - // float scale = 1; // assuming uniform scale - - /// @brief boolean indicating if the position was updated - bool positionUpdated = false; - /// @brief boolean indicating if the orientation was updated + /// @brief Boolean indicating the thing has an updated orientation bool orientationUpdated = false; /// @brief Set the linear velocity of the thing @@ -155,58 +176,62 @@ class Thing { /// @brief Get the linear velocity of the thing /// @return The linear velocity in local space, in meters per second virtual Spherical GetLinearVelocity(); + /// @brief Boolean indicating the thing has an updated linear velocity + bool linearVelocityUpdated = false; + /// @brief Set the angular velocity of the thing /// @param angularVelocity the new angular velocity in local space virtual void SetAngularVelocity(Spherical angularVelocity); /// @brief Get the angular velocity of the thing /// @return The angular velocity in local space virtual Spherical GetAngularVelocity(); - bool linearVelocityUpdated = false; + /// @brief Boolean indicating the thing has an updated angular velocity bool angularVelocityUpdated = false; private: - /// @brief The position in local space + /// @brief The position of the thing in local space, in meters /// @remark When this Thing has a parent, the position is relative to the /// parent's position and orientation Spherical position; - /// @brief The orientation in local space + /// @brief The orientation of the thing in local space /// @remark When this Thing has a parent, the orientation is relative to the /// parent's orientation SwingTwist orientation; - /// @brief The linear velocity in local space + /// @brief The linear velocity of the thing in local space, in meters per second Spherical linearVelocity; - /// @brief The angular velocity in local spze + /// @brief The angular velocity of the thing in local space, in degrees per second Spherical angularVelocity; +#pragma endregion Pose + +#pragma region Update + public: - /// @brief Terminated things are no longer updated - void Terminate(); - bool terminate = false; - - /// @brief Sets the location from where the 3D model of this Thing can be - /// loaded from - /// @param url The url of the model - /// @remark Although the roboid implementation is not dependent on the model, - /// the only official supported model format is .obj - void SetModel(const char* url); - + /// @brief Get the current time in milliseconds + /// @return The current time in milliseconds static unsigned long GetTimeMs(); + /// @brief Updates the state of the thing void Update(bool recursive = false); /// @brief Updates the state of the thing - /// @param currentTimeMs The current clock time in milliseconds - virtual void Update(unsigned long currentTimeMs, bool recursive = false); + /// @param currentTimeMs The current clock time in milliseconds; if this is + /// zero, the current time is retrieved automatically + /// @param recurse When true, this will Update the descendants recursively + virtual void Update(unsigned long currentTimeMs, bool recurse = false); static void UpdateThings(unsigned long currentTimeMs); +#pragma endregion Update + + public: /// @brief Function used to generate binary data for this thing /// @param buffer The byte array for thw binary data /// @param ix The starting position for writing the binary data - /// @returns The size of the binary data + /// @return The size of the binary data virtual int GenerateBinary(char* buffer, unsigned char* ix); - // /// @brief FUnction used to process binary data received for this thing + /// @brief Function used to process binary data received for this thing /// @param bytes The binary data virtual void ProcessBinary(char* bytes); }; From 4db9164f2a6dfaa6fe9bdbc3793160c17c9f180f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 30 Apr 2025 11:40:44 +0200 Subject: [PATCH 12/56] Completed things docs --- Thing.h | 1 + Things/DifferentialDrive.cpp | 9 ++++++--- Things/DifferentialDrive.h | 10 ++++++++-- Things/DigitalSensor.cpp | 17 +++++++++++++++-- Things/DigitalSensor.h | 27 +++++++++++++++++++++------ Things/TemperatureSensor.h | 6 +++--- Things/TouchSensor.cpp | 12 ++++++------ Things/TouchSensor.h | 30 +++++++++++++++--------------- 8 files changed, 75 insertions(+), 37 deletions(-) diff --git a/Thing.h b/Thing.h index 2fac262..ebc546c 100644 --- a/Thing.h +++ b/Thing.h @@ -36,6 +36,7 @@ class Thing { Roboid, Humanoid, ExternalSensor, + DifferentialDrive }; #pragma region Init diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index ad7ad14..80567a1 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -1,10 +1,13 @@ #include "DifferentialDrive.h" namespace RoboidControl { -DifferentialDrive::DifferentialDrive() : Thing() {} -RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) - : Thing(participant) {} + DifferentialDrive::DifferentialDrive() : Thing(Thing::Type::DifferentialDrive) {} + +DifferentialDrive::DifferentialDrive(Participant* participant, unsigned char thingId) + : Thing(participant, Thing::Type::DifferentialDrive, thingId) {} + +DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) : Thing(parent, Thing::Type::DifferentialDrive, thingId) {} void DifferentialDrive::SetDriveDimensions(float wheelDiameter, float wheelSeparation) { diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index 03e6318..000d8fd 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -13,7 +13,14 @@ class DifferentialDrive : public Thing { DifferentialDrive(); /// @brief Create a differential drive with networking support /// @param participant The local participant - DifferentialDrive(Participant* participant); + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + DifferentialDrive(Participant* participant, unsigned char thingId = 0); + /// @brief Create a new child differential drive + /// @param parent The parent thing + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + DifferentialDrive(Thing* parent, unsigned char thingId = 0); /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters @@ -33,7 +40,6 @@ class DifferentialDrive : public Thing { /// Positive moves the robot in the forward direction. /// @param speedRight The speed of the right wheel in degrees per second. /// Positive moves the robot in the forward direction. - void SetWheelVelocity(float speedLeft, float speedRight); /// @copydoc RoboidControl::Thing::Update(unsigned long) diff --git a/Things/DigitalSensor.cpp b/Things/DigitalSensor.cpp index 3f460cb..528bd7a 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -2,8 +2,21 @@ namespace RoboidControl { -//DigitalSensor::DigitalSensor() {} +DigitalSensor::DigitalSensor() : Thing(Type::Switch) {} -DigitalSensor::DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant) {} +DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId) + : Thing(owner, Type::Switch, thingId) {} + +DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId) + : Thing(parent, Type::Switch) {} + +int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) { + bytes[(*ix)++] = state ? 1 : 0; + return 1; +} + +void DigitalSensor::ProcessBinary(char* bytes) { + this->state |= (bytes[0] == 1); +} } // namespace RoboidControl diff --git a/Things/DigitalSensor.h b/Things/DigitalSensor.h index 5e4bc8a..95ab1b3 100644 --- a/Things/DigitalSensor.h +++ b/Things/DigitalSensor.h @@ -7,15 +7,30 @@ namespace RoboidControl { /// @brief A digital (on/off, 1/0, true/false) sensor class DigitalSensor : public Thing { public: + /// @brief Create a digital sensor without communication abilities + DigitalSensor(); + /// @brief Create a digital sensor for a participant + /// @param owner The owning participant + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + DigitalSensor(Participant* owner, unsigned char thingId = 0); + /// @brief Create a new child digital sensor + /// @param parent The parent thing + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + DigitalSensor(Thing* parent, unsigned char thingId = 0); + /// @brief The sigital state bool state = 0; - /// @brief The default constructor - //DigitalSensor(); - /// @brief Create a temperature sensor with the given ID - /// @param networkId The network ID of the sensor - /// @param thingId The ID of the thing - DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId); + /// @brief Function used to generate binary data for this digital sensor + /// @param buffer The byte array for thw binary data + /// @param ix The starting position for writing the binary data + int GenerateBinary(char* bytes, unsigned char* ix) override; + /// @brief Function used to process binary data received for this digital + /// sensor + /// @param bytes The binary data to process + virtual void ProcessBinary(char* bytes) override; }; } // namespace RoboidControl diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index 8d58d9c..a447385 100644 --- a/Things/TemperatureSensor.h +++ b/Things/TemperatureSensor.h @@ -7,9 +7,6 @@ namespace RoboidControl { /// @brief A temperature sensor class TemperatureSensor : public Thing { public: - /// @brief The measured temperature - float temperature = 0; - /// @brief The default constructor //TemperatureSensor(); /// @brief Create a temperature sensor with the given ID @@ -17,6 +14,9 @@ class TemperatureSensor : public Thing { /// @param thingId The ID of the thing TemperatureSensor(Participant* participant, unsigned char thingId); + /// @brief The measured temperature + float temperature = 0; + /// @brief Manually override the measured temperature /// @param temperature The new temperature virtual void SetTemperature(float temperature); diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index be98aeb..cef0c9e 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -1,22 +1,22 @@ #include "TouchSensor.h" namespace RoboidControl { + TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {} -TouchSensor::TouchSensor(Participant* participant) - : Thing(participant, Thing::Type::TouchSensor) {} +TouchSensor::TouchSensor(Participant* owner, unsigned char thingId) + : Thing(owner, Thing::Type::TouchSensor, thingId) {} -TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) { +TouchSensor::TouchSensor(Thing* parent, unsigned char thingId) : Thing(parent->owner, Thing::Type::TouchSensor, thingId) { this->SetParent(parent); } int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { - return 0; + bytes[(*ix)++] = touchedSomething ? 1 : 0; + return 1; } void TouchSensor::ProcessBinary(char* bytes) { - if (bytes[0] == 1) - std::cout << this->GetName() << " is Touching something!\n"; this->touchedSomething |= (bytes[0] == 1); } diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index 4e6d483..42d6a0b 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -6,31 +6,31 @@ namespace RoboidControl { /// @brief A sensor which can detect touches class TouchSensor : public Thing { +// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor public: - // Inherit all constructors - using Thing::Thing; - - /// @brief Create a touch sensor with isolated participant + /// @brief Create a touch sensor without communication abilities TouchSensor(); - /// @brief Create a touch sensor - TouchSensor(Participant* participant); - /// @brief Create a temperature sensor with the given ID - /// @param networkId The network ID of the sensor - /// @param thingId The ID of the thing - TouchSensor(Thing* parent); - // TouchSensor(RemoteParticipant* participant, unsigned char networkId, - // unsigned char thingId); + /// @brief Create a touch sensor for a participant + /// @param owner The owning participant + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + TouchSensor(Participant* owner, unsigned char thingId = 0); + /// @brief Create a new child touch sensor + /// @param parent The parent thing + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + TouchSensor(Thing* parent, unsigned char thingId = 0); /// @brief Value which is true when the sensor is touching something, false /// otherwise bool touchedSomething = false; - /// @brief Function to create a binary message with the temperature + /// @brief Function used to generate binary data for this touch sensor /// @param buffer The byte array for thw binary data /// @param ix The starting position for writing the binary data int GenerateBinary(char* bytes, unsigned char* ix) override; - /// @brief Function to extract the temperature received in the binary message - /// @param bytes The binary data + /// @brief Function used to process binary data received for this touch sensor + /// @param bytes The binary data to process virtual void ProcessBinary(char* bytes) override; }; From db1265a135af236cccd451ffc08ed605082a98d3 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 30 Apr 2025 17:00:01 +0200 Subject: [PATCH 13/56] Aliged Participants --- Messages/NetworkIdMsg.cpp | 25 ++++++ Messages/{SiteMsg.h => NetworkIdMsg.h} | 8 +- Messages/SiteMsg.cpp | 25 ------ Participants/ParticipantUDP.cpp | 17 +++-- Participants/ParticipantUDP.h | 102 +++++++++---------------- Participants/SiteServer.cpp | 21 +++-- Participants/SiteServer.h | 18 ++++- 7 files changed, 104 insertions(+), 112 deletions(-) create mode 100644 Messages/NetworkIdMsg.cpp rename Messages/{SiteMsg.h => NetworkIdMsg.h} (82%) delete mode 100644 Messages/SiteMsg.cpp diff --git a/Messages/NetworkIdMsg.cpp b/Messages/NetworkIdMsg.cpp new file mode 100644 index 0000000..40592fc --- /dev/null +++ b/Messages/NetworkIdMsg.cpp @@ -0,0 +1,25 @@ +#include "NetworkIdMsg.h" + +namespace RoboidControl { + +NetworkIdMsg::NetworkIdMsg(const char* buffer) { + this->networkId = buffer[1]; +} + +NetworkIdMsg::NetworkIdMsg(unsigned char networkId) { + this->networkId = networkId; +} + +NetworkIdMsg::~NetworkIdMsg() {} + +unsigned char NetworkIdMsg::Serialize(char* buffer) { +#if defined(DEBUG) + std::cout << "Send NetworkIdMsg [" << (int)this->networkId << "] " << std::endl; +#endif + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->networkId; + return NetworkIdMsg::length; +} + +} // namespace RoboidControl diff --git a/Messages/SiteMsg.h b/Messages/NetworkIdMsg.h similarity index 82% rename from Messages/SiteMsg.h rename to Messages/NetworkIdMsg.h index b6e433f..083439b 100644 --- a/Messages/SiteMsg.h +++ b/Messages/NetworkIdMsg.h @@ -3,7 +3,7 @@ namespace RoboidControl { /// @brief A message communicating the network ID for that participant -class SiteMsg : public IMessage { +class NetworkIdMsg : public IMessage { public: /// @brief The message ID static const unsigned char id = 0xA1; @@ -14,11 +14,11 @@ public: /// @brief Create a new message for sending /// @param networkId The network ID for the participant - SiteMsg(unsigned char networkId); + NetworkIdMsg(unsigned char networkId); /// @copydoc RoboidControl::IMessage::IMessage(char*) - SiteMsg(const char *buffer); + NetworkIdMsg(const char *buffer); /// @brief Destructor for the message - virtual ~SiteMsg(); + virtual ~NetworkIdMsg(); /// @copydoc RoboidControl::IMessage::Serialize virtual unsigned char Serialize(char *buffer) override; diff --git a/Messages/SiteMsg.cpp b/Messages/SiteMsg.cpp deleted file mode 100644 index 598a841..0000000 --- a/Messages/SiteMsg.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "SiteMsg.h" - -namespace RoboidControl { - -SiteMsg::SiteMsg(const char* buffer) { - this->networkId = buffer[1]; -} - -SiteMsg::SiteMsg(unsigned char networkId) { - this->networkId = networkId; -} - -SiteMsg::~SiteMsg() {} - -unsigned char SiteMsg::Serialize(char* buffer) { -#if defined(DEBUG) - std::cout << "Send SiteMsg [" << (int)this->networkId << "] " << std::endl; -#endif - unsigned char ix = 0; - buffer[ix++] = this->id; - buffer[ix++] = this->networkId; - return SiteMsg::length; -} - -} // namespace RoboidControl diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index f946a00..cab013d 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -325,8 +325,8 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize, Process(sender, msg); delete msg; } break; - case SiteMsg::id: { - SiteMsg* msg = new SiteMsg(this->buffer); + case NetworkIdMsg::id: { + NetworkIdMsg* msg = new NetworkIdMsg(this->buffer); bufferSize -= msg->length; Process(sender, msg); delete msg; @@ -382,7 +382,7 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { #endif } -void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { +void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process SiteMsg " << (int)this->networkId << " -> " << (int)msg->networkId << "\n"; @@ -452,9 +452,9 @@ void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { } void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { -#if defined(DEBUG) +#if !defined(DEBUG) std::cout << this->name << ": process PoseMsg [" << (int)this->networkId - << "/" << (int)msg->networkId << "]\n"; + << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; #endif Participant* owner = Participant::GetParticipant(msg->networkId); if (owner == nullptr) @@ -466,7 +466,12 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { if ((msg->poseType & PoseMsg::Pose_Position) != 0) thing->SetPosition(msg->position); - std::cout << "update position for" << (int)thing->id << std::endl; + if ((msg->poseType & PoseMsg::Pose_Orientation) != 0) + thing->SetOrientation(msg->orientation); + if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0) + thing->SetLinearVelocity(msg->linearVelocity); + if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0) + thing->SetAngularVelocity(msg->angularVelocity); } void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 375a3b0..8c2efdf 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -1,13 +1,13 @@ #pragma once #include "Messages/BinaryMsg.h" -#include "Messages/InvestigateMsg.h" #include "Messages/DestroyMsg.h" +#include "Messages/InvestigateMsg.h" #include "Messages/ModelUrlMsg.h" #include "Messages/NameMsg.h" #include "Messages/ParticipantMsg.h" #include "Messages/PoseMsg.h" -#include "Messages/SiteMsg.h" +#include "Messages/NetworkIdMsg.h" #include "Messages/ThingMsg.h" #include "Participant.h" @@ -30,7 +30,8 @@ namespace RoboidControl { constexpr int MAX_SENDER_COUNT = 256; -/// @brief A local participant is the local device which can communicate with +/// @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 /// participants. Each application has a local participant which is usually /// explicit in the code. An participant can be isolated. In that case it is @@ -42,6 +43,8 @@ constexpr int MAX_SENDER_COUNT = 256; /// RoboidControl::IsolatedParticipant::Isolated(). /// @sa RoboidControl::Thing::Thing() class ParticipantUDP : public Participant { +#pragma region Init + public: /// @brief Create a participant without connecting to a site /// @param port The port on which the participant communicates @@ -52,11 +55,8 @@ class ParticipantUDP : public Participant { /// @brief Create a participant which will try to connect to a site. /// @param ipAddress The IP address of the site /// @param port The port used by the site - ParticipantUDP(const char* ipAddress, - int port = 7681, - int localPort = 7681); - // Note to self: one cannot specify the port used by the local participant - // now!! + /// @param localPort The port used by the local participant + ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681); /// @brief Isolated participant is used when the application is run without /// networking @@ -65,7 +65,14 @@ class ParticipantUDP : public Participant { /// @brief True if the participant is running isolated. /// Isolated participants do not communicate with other participants + +#pragma endregion Init + + /// @brief True if the participant is running isolated. + /// Isolated participants do not communicate with other participants bool isIsolated = false; + /// @brief The remote site when this participant is connected to a site + Participant* remoteSite = nullptr; /// The interval in milliseconds for publishing (broadcasting) data on the /// local network @@ -74,19 +81,10 @@ class ParticipantUDP : public Participant { /// @brief The name of the participant const char* name = "ParticipantUDP"; - // int localPort = 0; - - /// @brief The remote site when this participant is connected to a site - Participant* remoteSite = nullptr; - -#if defined(ARDUINO) - // const char* remoteIpAddress = nullptr; - // unsigned short remotePort = 0; - // char* broadcastIpAddress = nullptr; - - // WiFiUDP udp; -#else + protected: + char buffer[1024]; +#if !defined(ARDUINO) #if defined(__unix__) || defined(__APPLE__) int sock; #elif defined(_WIN32) || defined(_WIN64) @@ -94,48 +92,48 @@ class ParticipantUDP : public Participant { sockaddr_in server_addr; sockaddr_in broadcast_addr; #endif - #endif - + public: void begin(); bool connected = false; +#pragma region Update + + public: virtual void Update(unsigned long currentTimeMs = 0) override; + + protected: + unsigned long nextPublishMe = 0; + virtual void UpdateMyThings(unsigned long currentTimeMs); virtual void UpdateOtherThings(unsigned long currentTimeMs); +#pragma endregion Update + +#pragma region Send + void SendThingInfo(Participant* remoteParticipant, Thing* thing); void PublishThingInfo(Thing* thing); bool Send(Participant* remoteParticipant, IMessage* msg); bool Publish(IMessage* msg); +#pragma endregion Send + +#pragma region Receive + +protected: void ReceiveData(unsigned char bufferSize, char* senderIpAddress, unsigned int senderPort); void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant); -// #if defined(NO_STD) -// unsigned char senderCount = 0; -// Participant* senders[MAX_SENDER_COUNT]; -// #else -// std::list senders; -// #endif - - protected: - unsigned long nextPublishMe = 0; - - char buffer[1024]; - void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort); - //Participant* GetParticipant(const char* ipAddress, int port); - // Participant* AddParticipant(const char* ipAddress, int port); - void ReceiveUDP(); virtual void Process(Participant* sender, ParticipantMsg* msg); - virtual void Process(Participant* sender, SiteMsg* msg); + virtual void Process(Participant* sender, NetworkIdMsg* msg); virtual void Process(Participant* sender, InvestigateMsg* msg); virtual void Process(Participant* sender, ThingMsg* msg); virtual void Process(Participant* sender, NameMsg* msg); @@ -143,34 +141,8 @@ class ParticipantUDP : public Participant { virtual void Process(Participant* sender, PoseMsg* msg); virtual void Process(Participant* sender, BinaryMsg* msg); -#if !defined(NO_STD) -// public: -// using ThingConstructor = std::function; +#pragma endregion Receive -// template -// void Register(unsigned char thingType) { -// thingMsgProcessors[thingType] = [](Participant* participant, -// unsigned char networkId, -// unsigned char thingId) { -// return new ThingClass(participant, networkId, thingId); -// }; -// }; - -// template -// void Register2(unsigned char thingType, ThingConstructor f) { -// thingMsgProcessors[thingType] = [f](Participant* participant, -// unsigned char networkId, -// unsigned char thingId) { -// return f(participant, networkId, thingId); -// }; -// }; - -// protected: -// std::unordered_map thingMsgProcessors; - -#endif }; } // namespace RoboidControl diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 39a859b..18b5d6b 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -58,27 +58,26 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { // std::cout << this->name << " received New Client -> " << // sender->ipAddress // << ":" << (int)sender->port << "\n"; - SiteMsg* msg = new SiteMsg(sender->networkId); + NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId); this->Send(sender, msg); delete msg; } } -void SiteServer::Process(Participant* sender, SiteMsg* msg) {} +void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {} void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing* thing = sender->Get(msg->thingId); - if (thing == nullptr) { + if (thing == nullptr) new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); - if (msg->parentId != 0) { - thing->SetParent(Get(msg->parentId)); - if (thing->GetParent() != nullptr) - std::cout << "Could not find parent [" << (int)msg->networkId << "/" - << (int)msg->parentId << "]\n"; - } else - thing->SetParent(nullptr); - } + if (msg->parentId != 0) { + thing->SetParent(Get(msg->parentId)); + if (thing->GetParent() != nullptr) + std::cout << "Could not find parent [" << (int)msg->networkId << "/" + << (int)msg->parentId << "]\n"; + } else + thing->SetParent(nullptr); } #pragma endregion Receive diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index 819b97b..a4e6767 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -12,17 +12,33 @@ namespace RoboidControl { /// @brief A participant is device which can communicate with other participants class SiteServer : public ParticipantUDP { + +#pragma region Init + public: + /// @brief Create a new site server + /// @param port The port of which to receive the messages SiteServer(int port = 7681); +#pragma endregion Init + +#pragma region Update + virtual void UpdateMyThings(unsigned long currentTimeMs) override; +#pragma endregion Update + +#pragma region Receive + protected: unsigned long nextPublishMe = 0; virtual void Process(Participant* sender, ParticipantMsg* msg) override; - virtual void Process(Participant* sender, SiteMsg* msg) override; + virtual void Process(Participant* sender, NetworkIdMsg* msg) override; virtual void Process(Participant* sender, ThingMsg* msg) override; + +#pragma endregion Receive + }; } // namespace RoboidControl From 8d778ab41e03667a60fcf93c1450a5c0291a24fc Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 1 May 2025 11:01:51 +0200 Subject: [PATCH 14/56] Aligned Messages --- Messages/BinaryMsg.h | 10 ++++++---- Messages/DestroyMsg.cpp | 4 ++-- Messages/DestroyMsg.h | 8 +++++--- Messages/{Messages.cpp => IMessage.cpp} | 5 +---- Messages/IMessage.h | 16 ++++++++++++++++ Messages/InvestigateMsg.cpp | 4 ++-- Messages/InvestigateMsg.h | 11 +++++++---- Messages/LowLevelMessages.h | 14 ++++++++------ Messages/Messages.h | 22 ---------------------- Messages/ModelUrlMsg.h | 7 ++++--- Messages/NameMsg.h | 8 ++++---- Messages/NetworkIdMsg.h | 4 +++- Messages/ParticipantMsg.h | 2 +- Messages/PoseMsg.h | 9 ++++++--- Messages/TextMsg.h | 6 +----- Messages/ThingMsg.h | 11 +++++------ 16 files changed, 71 insertions(+), 70 deletions(-) rename Messages/{Messages.cpp => IMessage.cpp} (73%) create mode 100644 Messages/IMessage.h delete mode 100644 Messages/Messages.h diff --git a/Messages/BinaryMsg.h b/Messages/BinaryMsg.h index 10adb90..81838c0 100644 --- a/Messages/BinaryMsg.h +++ b/Messages/BinaryMsg.h @@ -1,15 +1,17 @@ #pragma once -#include "Messages.h" +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { -/// @brief Message to send thing-specific data +/// @brief A message containing binary data for custom communication class BinaryMsg : public IMessage { public: /// @brief The message ID static const unsigned char id = 0xB1; - /// @brief The length of the message without the binary data itslef + /// @brief The length of the message in bytes, excluding the binary data + /// For the total size of the message this.bytes.Length should be added to this value. static const unsigned length = 4; /// @brief The network ID of the thing @@ -23,7 +25,7 @@ class BinaryMsg : public IMessage { /// @brief The binary data which is communicated char* data = nullptr; - /// @brief Create a new message for sending + /// @brief Create a BinaryMsg /// @param networkId The network ID of the thing /// @param thing The thing for which binary data is sent BinaryMsg(unsigned char networkId, Thing* thing); diff --git a/Messages/DestroyMsg.cpp b/Messages/DestroyMsg.cpp index f41ba3b..948b1fe 100644 --- a/Messages/DestroyMsg.cpp +++ b/Messages/DestroyMsg.cpp @@ -15,10 +15,10 @@ DestroyMsg::DestroyMsg(char* buffer) { DestroyMsg::~DestroyMsg() {} unsigned char DestroyMsg::Serialize(char* buffer) { -//#if defined(DEBUG) +#if defined(DEBUG) std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" << (int)this->thingId << "] " << std::endl; -//#endif +#endif unsigned char ix = 0; buffer[ix++] = this->id; buffer[ix++] = this->networkId; diff --git a/Messages/DestroyMsg.h b/Messages/DestroyMsg.h index 74ad2aa..dde5f81 100644 --- a/Messages/DestroyMsg.h +++ b/Messages/DestroyMsg.h @@ -1,13 +1,15 @@ -#include "Messages.h" +#pragma once + +#include "IMessage.h" namespace RoboidControl { -/// @brief Message notifiying that a Thing no longer exists +/// @brief A Message notifiying that a Thing no longer exists class DestroyMsg : public IMessage { public: /// @brief The message ID static const unsigned char id = 0x20; - /// @brief The length of the message + /// @brief The length of the message in bytes static const unsigned length = 3; /// @brief The network ID of the thing unsigned char networkId; diff --git a/Messages/Messages.cpp b/Messages/IMessage.cpp similarity index 73% rename from Messages/Messages.cpp rename to Messages/IMessage.cpp index d9f1a8c..86ccef7 100644 --- a/Messages/Messages.cpp +++ b/Messages/IMessage.cpp @@ -1,7 +1,4 @@ -#include "Messages.h" - -#include "LowLevelMessages.h" -#include "string.h" +#include "IMessage.h" namespace RoboidControl { diff --git a/Messages/IMessage.h b/Messages/IMessage.h new file mode 100644 index 0000000..632287e --- /dev/null +++ b/Messages/IMessage.h @@ -0,0 +1,16 @@ +#pragma once + +namespace RoboidControl { + +/// @brief Root structure for all communcation messages +class IMessage { + public: + IMessage(); + /// @brief Serialize the message into a byte array for sending + /// @param buffer The buffer to serilize into + /// @return The length of the message in the buffer + virtual unsigned char Serialize(char* buffer); + +}; + +} // namespace RoboidControl diff --git a/Messages/InvestigateMsg.cpp b/Messages/InvestigateMsg.cpp index 6420e42..7b94c8d 100644 --- a/Messages/InvestigateMsg.cpp +++ b/Messages/InvestigateMsg.cpp @@ -7,9 +7,9 @@ InvestigateMsg::InvestigateMsg(char* buffer) { this->networkId = buffer[ix++]; this->thingId = buffer[ix++]; } -InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) { +InvestigateMsg::InvestigateMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; - this->thingId = thingId; + this->thingId = thing->id; } InvestigateMsg::~InvestigateMsg() {} diff --git a/Messages/InvestigateMsg.h b/Messages/InvestigateMsg.h index 3179ec4..cff3c32 100644 --- a/Messages/InvestigateMsg.h +++ b/Messages/InvestigateMsg.h @@ -1,4 +1,7 @@ -#include "Messages.h" +#pragma once + +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { @@ -14,10 +17,10 @@ class InvestigateMsg : public IMessage { /// @brief the ID of the thing unsigned char thingId; - /// @brief Create a new message for sending + /// @brief Create an investigate message /// @param networkId The network ID for the thing - /// @param thingId The ID of the thing - InvestigateMsg(unsigned char networkId, unsigned char thingId); + /// @param thing The thing for which the details are requested + InvestigateMsg(unsigned char networkId, Thing* thing); /// @copydoc RoboidControl::IMessage::IMessage(char*) InvestigateMsg(char* buffer); /// @brief Destructor for the message diff --git a/Messages/LowLevelMessages.h b/Messages/LowLevelMessages.h index bffe010..144e50c 100644 --- a/Messages/LowLevelMessages.h +++ b/Messages/LowLevelMessages.h @@ -1,3 +1,5 @@ +#pragma once + #include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/SwingTwist.h" @@ -5,18 +7,18 @@ namespace RoboidControl { class LowLevelMessages { public: - static void SendAngle8(char* buffer, unsigned char* ix, const float angle); - static Angle8 ReceiveAngle8(const char* buffer, unsigned char* startIndex); - - static void SendFloat16(char* buffer, unsigned char* ix, float value); - static float ReceiveFloat16(const char* buffer, unsigned char* startIndex); - static void SendSpherical(char* buffer, unsigned char* ix, Spherical s); static Spherical ReceiveSpherical(const char* buffer, unsigned char* startIndex); static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q); static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix); + + static void SendAngle8(char* buffer, unsigned char* ix, const float angle); + static Angle8 ReceiveAngle8(const char* buffer, unsigned char* startIndex); + + static void SendFloat16(char* buffer, unsigned char* ix, float value); + static float ReceiveFloat16(const char* buffer, unsigned char* startIndex); }; } // namespace RoboidControl diff --git a/Messages/Messages.h b/Messages/Messages.h deleted file mode 100644 index df22544..0000000 --- a/Messages/Messages.h +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#include "LinearAlgebra/Spherical.h" -#include "LinearAlgebra/SwingTwist.h" -#include "Thing.h" - -namespace RoboidControl { - -class ParticipantUDP; - -class IMessage { - public: - IMessage(); - virtual unsigned char Serialize(char* buffer); - - static unsigned char* ReceiveMsg(unsigned char packetSize); - - // bool Publish(ParticipantUDP *participant); - // bool SendTo(ParticipantUDP *participant); -}; - -} // namespace RoboidControl diff --git a/Messages/ModelUrlMsg.h b/Messages/ModelUrlMsg.h index 7c493eb..9759aa0 100644 --- a/Messages/ModelUrlMsg.h +++ b/Messages/ModelUrlMsg.h @@ -1,4 +1,7 @@ -#include "Messages.h" +#pragma once + +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { @@ -26,8 +29,6 @@ class ModelUrlMsg : public IMessage { ModelUrlMsg(unsigned char networkId, Thing* thing); /// @copydoc RoboidControl::IMessage::IMessage(char*) ModelUrlMsg(const char* buffer); - // ModelUrlMsg(unsigned char networkId, unsigned char thingId, - // unsigned char urlLegth, const char *url, float scale = 1); /// @brief Destructor for the message virtual ~ModelUrlMsg(); diff --git a/Messages/NameMsg.h b/Messages/NameMsg.h index 0d1bebc..c283a59 100644 --- a/Messages/NameMsg.h +++ b/Messages/NameMsg.h @@ -1,4 +1,7 @@ -#include "Messages.h" +#pragma once + +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { @@ -22,9 +25,6 @@ class NameMsg : public IMessage { /// @param networkId The network ID of the thing /// @param thing The ID of the thing NameMsg(unsigned char networkId, Thing* thing); - // NameMsg(unsigned char networkId, unsigned char thingId, const char *name, - // unsigned char nameLength); - /// @copydoc RoboidControl::IMessage::IMessage(char*) NameMsg(const char* buffer); /// @brief Destructor for the message diff --git a/Messages/NetworkIdMsg.h b/Messages/NetworkIdMsg.h index 083439b..de78906 100644 --- a/Messages/NetworkIdMsg.h +++ b/Messages/NetworkIdMsg.h @@ -1,4 +1,6 @@ -#include "Messages.h" +#pragma once + +#include "IMessage.h" namespace RoboidControl { diff --git a/Messages/ParticipantMsg.h b/Messages/ParticipantMsg.h index 28f9eb5..105881d 100644 --- a/Messages/ParticipantMsg.h +++ b/Messages/ParticipantMsg.h @@ -1,6 +1,6 @@ #pragma once -#include "Messages.h" +#include "IMessage.h" namespace RoboidControl { diff --git a/Messages/PoseMsg.h b/Messages/PoseMsg.h index bc0c336..fe702da 100644 --- a/Messages/PoseMsg.h +++ b/Messages/PoseMsg.h @@ -1,4 +1,6 @@ -#include "Messages.h" +#pragma once +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { @@ -9,7 +11,7 @@ class PoseMsg : public IMessage { public: /// @brief The message ID static const unsigned char id = 0x10; - /// @brief The length of the message + /// @brief The length of the message in bytes unsigned char length = 4 + 4 + 4; /// @brief The network ID of the thing @@ -40,7 +42,8 @@ class PoseMsg : public IMessage { /// @brief Create a new message for sending /// @param networkId he network ID of the thing - /// @param thing The thing for which the pose shouldbe sent + /// @param thing The thing for which the pose should be sent + /// @param force If true, position and orientation are always included, even when they are not updated PoseMsg(unsigned char networkId, Thing* thing, bool force = false); /// @copydoc RoboidControl::IMessage::IMessage(char*) diff --git a/Messages/TextMsg.h b/Messages/TextMsg.h index 5b25fca..a5763e6 100644 --- a/Messages/TextMsg.h +++ b/Messages/TextMsg.h @@ -1,4 +1,4 @@ -#include "Messages.h" +#include "IMessage.h" namespace RoboidControl { @@ -9,10 +9,6 @@ class TextMsg : public IMessage { static const unsigned char id = 0xB0; /// @brief The length of the message without the text itself static const unsigned char length = 2; - /// @brief The network ID of the thing - unsigned char networkId; - /// @brief the ID of the thing - unsigned char thingId; /// @brief The text without the null terminator const char* text; /// @brief The length of the text diff --git a/Messages/ThingMsg.h b/Messages/ThingMsg.h index c40fb97..a26b60d 100644 --- a/Messages/ThingMsg.h +++ b/Messages/ThingMsg.h @@ -1,8 +1,9 @@ -#include "Messages.h" +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { -/// @brief Message providing generic information about a Thing +/// @brief Message providing generic details about a Thing class ThingMsg : public IMessage { public: /// @brief The message ID @@ -13,17 +14,15 @@ class ThingMsg : public IMessage { unsigned char networkId; /// @brief The ID of the thing unsigned char thingId; - /// @brief The Thing.Type of the thing + /// @brief The type of thing unsigned char thingType; - /// @brief The parent of the thing in the hierarachy. This is null for root Things + /// @brief The ID of the parent thing in the hierarchy. This is zero for root things unsigned char parentId; /// @brief Create a message for sending /// @param networkId The network ID of the thing /// @param thing The thing ThingMsg(unsigned char networkId, Thing* thing); - // ThingMsg(unsigned char networkId, unsigned char thingId, - // unsigned char thingType, unsigned char parentId); /// @copydoc RoboidControl::IMessage::IMessage(char*) ThingMsg(const char* buffer); From 81c5d07e39b7faa3157d9530cfcd11c3ca9a9d32 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 1 May 2025 11:06:34 +0200 Subject: [PATCH 15/56] Fix include error --- Messages/DestroyMsg.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Messages/DestroyMsg.h b/Messages/DestroyMsg.h index dde5f81..19e1901 100644 --- a/Messages/DestroyMsg.h +++ b/Messages/DestroyMsg.h @@ -1,6 +1,7 @@ #pragma once #include "IMessage.h" +#include "Thing.h" namespace RoboidControl { From 19310377ff6917b23cb43f8970b8355f725c5e17 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 1 May 2025 14:28:03 +0200 Subject: [PATCH 16/56] Added participant tests --- test/participant_test.cc | 147 ++++++++++++++------------------------- 1 file changed, 54 insertions(+), 93 deletions(-) diff --git a/test/participant_test.cc b/test/participant_test.cc index c826863..42c686b 100644 --- a/test/participant_test.cc +++ b/test/participant_test.cc @@ -4,111 +4,72 @@ // not supported using Visual Studio 2022 compiler... #include -#include -#include -// #include - -#include "Participant.h" #include "Participants/SiteServer.h" #include "Thing.h" using namespace RoboidControl; -// Function to get the current time in milliseconds as unsigned long -unsigned long get_time_ms() { - auto now = std::chrono::steady_clock::now(); - auto ms = std::chrono::duration_cast( - now.time_since_epoch()); - return static_cast(ms.count()); +TEST(Participant, Participant) { + ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7682); + + unsigned long milliseconds = Thing::GetTimeMs(); + unsigned long startTime = milliseconds; + while (milliseconds < startTime + 7000) { + participant->Update(milliseconds); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + milliseconds = Thing::GetTimeMs(); + } + SUCCEED(); } -// class RoboidControlSuite : public ::testing::test { -// TEST_F(RoboidControlSuite, HiddenParticipant) { -// Thing thing = Thing(); - -// unsigned long milliseconds = get_time_ms(); -// unsigned long startTime = milliseconds; -// while (milliseconds < startTime + 1000) { -// Thing.Update(milliseconds); - -// milliseconds = get_time_ms(); -// } -// ASSERT_EQ(1, 1); -// } -// } +TEST(Participant, SiteServer) { + SiteServer* siteServer = new SiteServer(); -class ParticipantSuite : public ::testing::Test { -protected: - // SetUp and TearDown can be used to set up and clean up before/after each - // test - void SetUp() override { - // Initialize test data here + unsigned long milliseconds = Thing::GetTimeMs(); + unsigned long startTime = milliseconds; + while (milliseconds < startTime + 7000) { + siteServer->Update(milliseconds); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + milliseconds = Thing::GetTimeMs(); } + SUCCEED(); +} - void TearDown() override { - // Clean up test data here +TEST(Participant, SiteParticipant) { + SiteServer* siteServer = new SiteServer(7681); + ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682); + + unsigned long milliseconds = Thing::GetTimeMs(); + unsigned long startTime = milliseconds; + while (milliseconds < startTime + 7000) { + siteServer->Update(milliseconds); + participant->Update(milliseconds); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + milliseconds = Thing::GetTimeMs(); } -}; + SUCCEED(); +} +TEST(Participant, ThingMsg) { + SiteServer* siteServer = new SiteServer(7681); + ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682); + Thing* thing = new Thing(participant); + thing->SetName("First Thing"); + thing->SetModel("https://passer.life/extras/ant.jpg"); + + unsigned long milliseconds = Thing::GetTimeMs(); + unsigned long startTime = milliseconds; + while (milliseconds < startTime + 7000) { + siteServer->Update(milliseconds); + participant->Update(milliseconds); - -// TEST_F(ParticipantSuite, ParticipantUDP) { -// ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681); - -// unsigned long milliseconds = get_time_ms(); -// unsigned long startTime = milliseconds; -// while (milliseconds < startTime + 1000) { -// participant->Update(milliseconds); - -// milliseconds = get_time_ms(); -// } -// ASSERT_EQ(1, 1); -// } - -// TEST_F(ParticipantSuite, SiteServer) { -// SiteServer site = SiteServer(7681); - -// unsigned long milliseconds = get_time_ms(); -// unsigned long startTime = milliseconds; -// while (milliseconds < startTime + 1000) { -// site.Update(milliseconds); - -// milliseconds = get_time_ms(); -// } -// ASSERT_EQ(1, 1); -// } - -// TEST_F(ParticipantSuite, SiteParticipant) { -// SiteServer site = SiteServer(7681); -// ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681); - -// unsigned long milliseconds = get_time_ms(); -// unsigned long startTime = milliseconds; -// while (milliseconds < startTime + 1000) { -// site.Update(milliseconds); -// participant.Update(milliseconds); - -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// milliseconds = get_time_ms(); -// } -// ASSERT_EQ(1, 1); -// } - -// TEST_F(ParticipantSuite, Thing) { -// SiteServer site = SiteServer(7681); -// ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681); -// Thing thing = Thing(&participant); - -// unsigned long milliseconds = get_time_ms(); -// unsigned long startTime = milliseconds; -// while (milliseconds < startTime + 1000) { -// site.Update(milliseconds); -// participant.Update(milliseconds); - -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// milliseconds = get_time_ms(); -// } -// ASSERT_EQ(1, 1); -// } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + milliseconds = Thing::GetTimeMs(); + } + SUCCEED(); +} #endif From 2d77aa2461b670813b772de61409a04d26b95d49 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 1 May 2025 14:28:14 +0200 Subject: [PATCH 17/56] Fix issue multiple local participants --- Participant.cpp | 126 +++++++++++++++++++------------- Participant.h | 31 +++++--- Participants/ParticipantUDP.cpp | 37 ++++------ Participants/SiteServer.cpp | 7 +- Thing.h | 1 - 5 files changed, 110 insertions(+), 92 deletions(-) diff --git a/Participant.cpp b/Participant.cpp index e423608..e8d33f8 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -4,7 +4,9 @@ namespace RoboidControl { -std::list Participant::participants; +#pragma region Participant + +ParticipantRegistry Participant::registry; Participant::Participant(const char* ipAddress, int port) { // make a copy of the ip address string @@ -24,6 +26,7 @@ Participant::Participant(const char* ipAddress, int port) { } Participant::~Participant() { + registry.Remove(this); delete[] this->ipAddress; } @@ -34,56 +37,6 @@ void Participant::Update(unsigned long currentTimeMs) { } } -Participant* Participant::GetParticipant(const char* ipAddress, - unsigned int port) { - for (Participant* participant : Participant::participants) { - if (participant == nullptr) - continue; - if (strcmp(participant->ipAddress, ipAddress) == 0 && - participant->port == port) - return participant; - } - // std::cout << "Could not find participant " << ipAddress << ":" << (int)port - // << std::endl; - return nullptr; -} - -Participant* Participant::GetParticipant(unsigned char participantId) { - for (Participant* participant : Participant::participants) { - if (participant == nullptr) - continue; - if (participant->networkId == participantId) - return participant; - } - // std::cout << "Could not find participant " << (int)participantId << std::endl; - return nullptr; -} - -Participant* Participant::AddParticipant(const char* ipAddress, unsigned int port) { - Participant* participant = new Participant(ipAddress, port); - Participant::AddParticipant(participant); - return participant; -} - -void Participant::AddParticipant(Participant* participant) { - Participant* foundParticipant = - Participant::GetParticipant(participant->ipAddress, participant->port); - if (foundParticipant == nullptr) { -#if defined(NO_STD) - this->things[this->thingCount++] = thing; -#else - Participant::participants.push_back(participant); -#endif - std::cout << "Add participant " << participant->ipAddress << ":" - << participant->port << "[" << (int)participant->networkId - << "]\n"; - } else { - std::cout << "Did not add, existing participant " << participant->ipAddress << ":" - << participant->port << "[" << (int)participant->networkId - << "]\n"; - } -} - Thing* Participant::Get(unsigned char thingId) { for (Thing* thing : this->things) { if (thing->id == thingId) @@ -154,4 +107,75 @@ void Participant::Remove(Thing* thing) { #endif } +#pragma endregion + +#pragma region ParticipantRegistry + +Participant* ParticipantRegistry::GetParticipant(const char* ipAddress, + unsigned int port) { + 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; + return nullptr; +} + +Participant* ParticipantRegistry::GetParticipant(unsigned char participantId) { + 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; + return nullptr; +} + +Participant* ParticipantRegistry::AddParticipant(const char* ipAddress, + unsigned int port) { + Participant* participant = new Participant(ipAddress, port); + AddParticipant(participant); + return participant; +} + +void ParticipantRegistry::AddParticipant(Participant* participant) { + Participant* foundParticipant = + GetParticipant(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); +} + +const std::list& ParticipantRegistry::GetAll() const { + return ParticipantRegistry::participants; +} + +#pragma endregion ParticipantRegistry + } // namespace RoboidControl diff --git a/Participant.h b/Participant.h index 2facd6b..4bf393d 100644 --- a/Participant.h +++ b/Participant.h @@ -5,6 +5,24 @@ namespace RoboidControl { constexpr int MAX_THING_COUNT = 256; +class ParticipantRegistry { + public: + Participant* GetParticipant(const char* ipAddress, unsigned int port); + Participant* GetParticipant(unsigned char participantId); + Participant* AddParticipant(const char* ipAddress, unsigned int port); + void AddParticipant(Participant* participant); + + void Remove(Participant* participant); + + const std::list& GetAll() const; + private: + #if defined(NO_STD) + #else + /// @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 @@ -52,17 +70,8 @@ class Participant { /// @param currentTimeMs The current time in milliseconds (optional) virtual void Update(unsigned long currentTimeMs = 0); - public: -#if defined(NO_STD) -#else - /// @brief The list of known participants - static std::list participants; -#endif - static Participant* GetParticipant(const char* ipAddress, unsigned int port); - static Participant* GetParticipant(unsigned char participantId); - static Participant* AddParticipant(const char* ipAddress, unsigned int port); - static void AddParticipant(Participant* participant); - +public: + static ParticipantRegistry registry; }; } // namespace RoboidControl diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index cab013d..a4d5859 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -4,32 +4,18 @@ #include "Arduino/ArduinoParticipant.h" #include "EspIdf/EspIdfParticipant.h" - -#if defined(_WIN32) || defined(_WIN64) -#include -#include #include "Windows/WindowsParticipant.h" -#pragma comment(lib, "ws2_32.lib") - -#elif defined(__unix__) || defined(__APPLE__) -#include -#include // For fcntl -#include -#include -#include -#include #include "Posix/PosixParticipant.h" -#endif #include namespace RoboidControl { -ParticipantUDP::ParticipantUDP(int port) : Participant("0.0.0.0", port) { +ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) { this->remoteSite = nullptr; if (this->port == 0) this->isIsolated = true; - Participant::AddParticipant(this); + Participant::registry.AddParticipant(this); } ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) @@ -38,7 +24,7 @@ ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) this->isIsolated = true; else this->remoteSite = new Participant(ipAddress, port); - Participant::AddParticipant(this); + Participant::registry.AddParticipant(this); } static ParticipantUDP* isolatedParticipant = nullptr; @@ -50,7 +36,7 @@ ParticipantUDP* ParticipantUDP::Isolated() { } void ParticipantUDP::begin() { - if (this->isIsolated) + if (this->isIsolated || this->remoteSite == nullptr) return; SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port); @@ -157,11 +143,14 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { } void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) { - for (Participant* participant : Participant::participants) { + for (Participant* participant : Participant::registry.GetAll()) { if (participant == nullptr || participant == this) continue; - participant->Update(currentTimeMs); + // Call only the Participant version of the Update. + // This is to deal with the function where one of the (remote) + // participants is actually a local participant + participant->Participant::Update(currentTimeMs); if (this->isIsolated) continue; @@ -301,9 +290,9 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize, unsigned int senderPort) { // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort // << std::endl; - Participant* sender = this->GetParticipant(senderIpAddress, senderPort); + Participant* sender = this->registry.GetParticipant(senderIpAddress, senderPort); if (sender == nullptr) { - sender = this->AddParticipant(senderIpAddress, senderPort); + sender = this->registry.AddParticipant(senderIpAddress, senderPort); #if !defined(NO_STD) std::cout << "New remote participant " << sender->ipAddress << ":" << sender->port << std::endl; @@ -456,7 +445,7 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { std::cout << this->name << ": process PoseMsg [" << (int)this->networkId << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; #endif - Participant* owner = Participant::GetParticipant(msg->networkId); + Participant* owner = Participant::registry.GetParticipant(msg->networkId); if (owner == nullptr) return; @@ -480,7 +469,7 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { << "/" << (int)msg->thingId << "]\n"; #endif - Participant* owner = Participant::GetParticipant(msg->networkId); + Participant* owner = Participant::registry.GetParticipant(msg->networkId); if (owner != nullptr) { Thing* thing = owner->Get(msg->thingId); if (thing != nullptr) diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 18b5d6b..fd0fed3 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -11,13 +11,10 @@ namespace RoboidControl { #pragma region Init -SiteServer::SiteServer(int port) { +SiteServer::SiteServer(int port) : ParticipantUDP(port) { this->name = "Site Server"; this->publishInterval = 0; - this->ipAddress = "0.0.0.0"; - this->port = port; - SetupUDP(port, ipAddress, 0); } @@ -34,7 +31,7 @@ void SiteServer::UpdateMyThings(unsigned long currentTimeMs) { if (this->isIsolated == false) { // Send to all other participants - for (Participant* participant : Participant::participants) { + for (Participant* participant : Participant::registry.GetAll()) { if (participant == nullptr || participant == this) continue; diff --git a/Thing.h b/Thing.h index ebc546c..691b6f5 100644 --- a/Thing.h +++ b/Thing.h @@ -66,7 +66,6 @@ class Thing { public: /// @brief Terminated things are no longer updated - void Terminate(); bool terminate = false; #pragma region Properties From 6265daea87513b79249de1b34f3a4e349f38f40a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 1 May 2025 14:36:09 +0200 Subject: [PATCH 18/56] Documented and renamed participant registry --- Arduino/ArduinoParticipant.cpp | 4 +-- Participant.cpp | 12 ++++---- Participant.h | 50 +++++++++++++++++++++++---------- Participants/ParticipantUDP.cpp | 12 ++++---- Posix/PosixParticipant.cpp | 4 +-- Windows/WindowsParticipant.cpp | 4 +-- 6 files changed, 53 insertions(+), 33 deletions(-) diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index b283052..eff8f1b 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -72,9 +72,9 @@ void ParticipantUDP::Receive() { senderAddress.toCharArray(sender_ipAddress, 16); unsigned int sender_port = udp->remotePort(); - // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, + // Participant* remoteParticipant = this->Get(sender_ipAddress, // sender_port); if (remoteParticipant == nullptr) { - // remoteParticipant = this->AddParticipant(sender_ipAddress, + // remoteParticipant = this->Add(sender_ipAddress, // sender_port); // // std::cout << "New sender " << sender_ipAddress << ":" << sender_port // // << "\n"; diff --git a/Participant.cpp b/Participant.cpp index e8d33f8..0cbe739 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -111,7 +111,7 @@ void Participant::Remove(Thing* thing) { #pragma region ParticipantRegistry -Participant* ParticipantRegistry::GetParticipant(const char* ipAddress, +Participant* ParticipantRegistry::Get(const char* ipAddress, unsigned int port) { for (Participant* participant : ParticipantRegistry::participants) { if (participant == nullptr) @@ -128,7 +128,7 @@ Participant* ParticipantRegistry::GetParticipant(const char* ipAddress, return nullptr; } -Participant* ParticipantRegistry::GetParticipant(unsigned char participantId) { +Participant* ParticipantRegistry::Get(unsigned char participantId) { for (Participant* participant : ParticipantRegistry::participants) { if (participant == nullptr) continue; @@ -139,16 +139,16 @@ Participant* ParticipantRegistry::GetParticipant(unsigned char participantId) { return nullptr; } -Participant* ParticipantRegistry::AddParticipant(const char* ipAddress, +Participant* ParticipantRegistry::Add(const char* ipAddress, unsigned int port) { Participant* participant = new Participant(ipAddress, port); - AddParticipant(participant); + Add(participant); return participant; } -void ParticipantRegistry::AddParticipant(Participant* participant) { +void ParticipantRegistry::Add(Participant* participant) { Participant* foundParticipant = - GetParticipant(participant->ipAddress, participant->port); + Get(participant->ipAddress, participant->port); if (foundParticipant == nullptr) { #if defined(NO_STD) diff --git a/Participant.h b/Participant.h index 4bf393d..051589e 100644 --- a/Participant.h +++ b/Participant.h @@ -5,23 +5,43 @@ namespace RoboidControl { constexpr int MAX_THING_COUNT = 256; +/// @brief class which manages all known participants class ParticipantRegistry { - public: - Participant* GetParticipant(const char* ipAddress, unsigned int port); - Participant* GetParticipant(unsigned char participantId); - Participant* AddParticipant(const char* ipAddress, unsigned int port); - void AddParticipant(Participant* participant); + 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); - void Remove(Participant* participant); + /// @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); - const std::list& GetAll() const; - private: - #if defined(NO_STD) - #else - /// @brief The list of known participants - std::list participants; - #endif - }; + /// @brief Remove a participant + /// @param participant The participant to remove + void Remove(Participant* participant); + + /// @brief Get all participants + /// @return All participants + const std::list& GetAll() const; + + private: +#if defined(NO_STD) +#else + /// @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 @@ -70,7 +90,7 @@ class Participant { /// @param currentTimeMs The current time in milliseconds (optional) virtual void Update(unsigned long currentTimeMs = 0); -public: + public: static ParticipantRegistry registry; }; diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index a4d5859..01c2b86 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -15,7 +15,7 @@ ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) { this->remoteSite = nullptr; if (this->port == 0) this->isIsolated = true; - Participant::registry.AddParticipant(this); + Participant::registry.Add(this); } ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) @@ -24,7 +24,7 @@ ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) this->isIsolated = true; else this->remoteSite = new Participant(ipAddress, port); - Participant::registry.AddParticipant(this); + Participant::registry.Add(this); } static ParticipantUDP* isolatedParticipant = nullptr; @@ -290,9 +290,9 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize, unsigned int senderPort) { // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort // << std::endl; - Participant* sender = this->registry.GetParticipant(senderIpAddress, senderPort); + Participant* sender = this->registry.Get(senderIpAddress, senderPort); if (sender == nullptr) { - sender = this->registry.AddParticipant(senderIpAddress, senderPort); + sender = this->registry.Add(senderIpAddress, senderPort); #if !defined(NO_STD) std::cout << "New remote participant " << sender->ipAddress << ":" << sender->port << std::endl; @@ -445,7 +445,7 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { std::cout << this->name << ": process PoseMsg [" << (int)this->networkId << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; #endif - Participant* owner = Participant::registry.GetParticipant(msg->networkId); + Participant* owner = Participant::registry.Get(msg->networkId); if (owner == nullptr) return; @@ -469,7 +469,7 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { << "/" << (int)msg->thingId << "]\n"; #endif - Participant* owner = Participant::registry.GetParticipant(msg->networkId); + Participant* owner = Participant::registry.Get(msg->networkId); if (owner != nullptr) { Thing* thing = owner->Get(msg->thingId); if (thing != nullptr) diff --git a/Posix/PosixParticipant.cpp b/Posix/PosixParticipant.cpp index 88e18af..466035a 100644 --- a/Posix/PosixParticipant.cpp +++ b/Posix/PosixParticipant.cpp @@ -74,9 +74,9 @@ void Receive() { unsigned int sender_port = ntohs(client_addr.sin_port); ReceiveData(packetSize, sender_ipAddress, sender_port); - // RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port); + // RoboidControl::Participant* remoteParticipant = this->Get(sender_ipAddress, sender_port); // if (remoteParticipant == nullptr) { - // remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port); + // remoteParticipant = this->Add(sender_ipAddress, sender_port); // // std::cout << "New sender " << sender_ipAddress << ":" << sender_port // // << "\n"; // // std::cout << "New remote participant " << remoteParticipant->ipAddress diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index d64eb1e..c6e99db 100644 --- a/Windows/WindowsParticipant.cpp +++ b/Windows/WindowsParticipant.cpp @@ -85,9 +85,9 @@ void ParticipantUDP::Receive() { unsigned int sender_port = ntohs(client_addr.sin_port); ReceiveData(packetSize, sender_ipAddress, sender_port); - // RoboidControl::ParticipantUDP* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port); + // RoboidControl::ParticipantUDP* remoteParticipant = this->Get(sender_ipAddress, sender_port); // if (remoteParticipant == nullptr) { - // remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port); + // remoteParticipant = this->Add(sender_ipAddress, sender_port); // // std::cout << "New sender " << sender_ipAddress << ":" // // << sender_port << "\n"; // // std::cout << "New remote participant " << From 40aab4dc7a1715378f26e16b87085e64911ab962 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 13 May 2025 09:38:30 +0200 Subject: [PATCH 19/56] fix NO_STD issues --- Arduino/ArduinoParticipant.cpp | 12 ++++++++++++ Messages/NetworkIdMsg.cpp | 2 ++ Messages/ParticipantMsg.cpp | 6 +++++- Messages/TextMsg.cpp | 6 +++++- Participant.cpp | 34 +++++++++++++++++++++------------ Participant.h | 14 ++++++++++---- Participants/ParticipantUDP.cpp | 8 +++++++- Participants/SiteServer.cpp | 10 ++++++++++ 8 files changed, 73 insertions(+), 19 deletions(-) diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index eff8f1b..dad27fb 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -1,5 +1,9 @@ #include "ArduinoParticipant.h" +#if !defined(NO_STD) +#include +#endif + #if defined(ARDUINO) #if defined(ARDUINO_ARCH_ESP8266) #include @@ -29,7 +33,9 @@ void ParticipantUDP::Setup() { #if defined(UNO_R4) if (WiFi.status() == WL_NO_MODULE) { +#if !defined(NO_STD) std::cout << "No network available!\n"; +#endif return; } #else @@ -42,11 +48,13 @@ void ParticipantUDP::Setup() { udp = new WiFiUDP(); udp->begin(this->port); +#if !defined(NO_STD) std::cout << "Wifi sync started local " << this->port; if (this->remoteSite != nullptr) std::cout << ", remote " << this->remoteSite->ipAddress << ":" << this->remoteSite->port << "\n"; #endif +#endif } void ParticipantUDP::GetBroadcastAddress() { @@ -57,8 +65,10 @@ void ParticipantUDP::GetBroadcastAddress() { this->broadcastIpAddress = new char[broadcastIpString.length() + 1]; broadcastIpString.toCharArray(this->broadcastIpAddress, broadcastIpString.length() + 1); +#if !defined(NO_STD) std::cout << "Broadcast address: " << broadcastIpAddress << "\n"; #endif +#endif } void ParticipantUDP::Receive() { @@ -99,7 +109,9 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { int n = 0; do { if (n > 0) { +#if !defined(NO_STD) std::cout << "Retry sending\n"; +#endif delay(10); } n++; diff --git a/Messages/NetworkIdMsg.cpp b/Messages/NetworkIdMsg.cpp index 40592fc..df9c9cf 100644 --- a/Messages/NetworkIdMsg.cpp +++ b/Messages/NetworkIdMsg.cpp @@ -1,5 +1,7 @@ #include "NetworkIdMsg.h" +#include + namespace RoboidControl { NetworkIdMsg::NetworkIdMsg(const char* buffer) { diff --git a/Messages/ParticipantMsg.cpp b/Messages/ParticipantMsg.cpp index c91fce9..7fb1439 100644 --- a/Messages/ParticipantMsg.cpp +++ b/Messages/ParticipantMsg.cpp @@ -1,5 +1,9 @@ #include "ParticipantMsg.h" +#if !defined(NO_STD) +#include +#endif + namespace RoboidControl { ParticipantMsg::ParticipantMsg(char networkId) { @@ -13,7 +17,7 @@ ParticipantMsg::ParticipantMsg(const char* buffer) { ParticipantMsg::~ParticipantMsg() {} unsigned char ParticipantMsg::Serialize(char* buffer) { -#if defined(DEBUG) +#if defined(DEBUG) && !defined(NO_STD) std::cout << "Send ParticipantMsg [" << (int)this->networkId << "] " << std::endl; #endif diff --git a/Messages/TextMsg.cpp b/Messages/TextMsg.cpp index 119a907..942e112 100644 --- a/Messages/TextMsg.cpp +++ b/Messages/TextMsg.cpp @@ -1,5 +1,9 @@ #include "TextMsg.h" +#if !defined(NO_STD) +#include +#endif + namespace RoboidControl { TextMsg::TextMsg(const char* text, unsigned char textLength) { @@ -24,7 +28,7 @@ unsigned char TextMsg::Serialize(char* buffer) { if (this->textLength == 0 || this->text == nullptr) return 0; -#if defined(DEBUG) +#if defined(DEBUG) && !defined(NO_STD) std::cout << "Send TextMsg " << (int)this->textLength << " " << this->text << std::endl; #endif unsigned char ix = 0; diff --git a/Participant.cpp b/Participant.cpp index 0cbe739..82a3555 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -112,7 +112,8 @@ void Participant::Remove(Thing* thing) { #pragma region ParticipantRegistry Participant* ParticipantRegistry::Get(const char* ipAddress, - unsigned int port) { + unsigned int port) { +#if !defined(NO_STD) for (Participant* participant : ParticipantRegistry::participants) { if (participant == nullptr) continue; @@ -125,10 +126,12 @@ Participant* ParticipantRegistry::Get(const char* ipAddress, } 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; @@ -136,11 +139,12 @@ Participant* ParticipantRegistry::Get(unsigned char 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) { + unsigned int port) { Participant* participant = new Participant(ipAddress, port); Add(participant); return participant; @@ -152,19 +156,19 @@ void ParticipantRegistry::Add(Participant* participant) { if (foundParticipant == nullptr) { #if defined(NO_STD) - this->things[this->thingCount++] = thing; + //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"; + // 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"; } } @@ -172,9 +176,15 @@ 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 diff --git a/Participant.h b/Participant.h index 051589e..358746b 100644 --- a/Participant.h +++ b/Participant.h @@ -31,13 +31,19 @@ class ParticipantRegistry { /// @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: -#if defined(NO_STD) -#else +private: /// @brief The list of known participants std::list participants; #endif diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 01c2b86..c6c4bb1 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -143,7 +143,13 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { } void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) { +#if defined(NO_STD) + Participant** participants = Participant::registry.GetAll(); + for (int ix = 0; ix < Participant::registry.count; ix++) { + Participant* participant = participants[ix]; +#else for (Participant* participant : Participant::registry.GetAll()) { +#endif if (participant == nullptr || participant == this) continue; @@ -441,7 +447,7 @@ void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { } void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { -#if !defined(DEBUG) +#if !defined(DEBUG) && !defined(NO_STD) std::cout << this->name << ": process PoseMsg [" << (int)this->networkId << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; #endif diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index fd0fed3..bfe1c2d 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -31,7 +31,13 @@ void SiteServer::UpdateMyThings(unsigned long currentTimeMs) { if (this->isIsolated == false) { // Send to all other participants +#if defined(NO_STD) + Participant** participants = Participant::registry.GetAll(); + for (int ix = 0; ix < Participant::registry.count; ix++) { + Participant* participant = participants[ix]; +#else for (Participant* participant : Participant::registry.GetAll()) { +#endif if (participant == nullptr || participant == this) continue; @@ -71,8 +77,12 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) { if (msg->parentId != 0) { thing->SetParent(Get(msg->parentId)); if (thing->GetParent() != nullptr) +#if defined(NO_STD) + ; +#else std::cout << "Could not find parent [" << (int)msg->networkId << "/" << (int)msg->parentId << "]\n"; +#endif } else thing->SetParent(nullptr); } From 23ea03fbaf0c69a80e45ab6008075336ab15a80c Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 14 May 2025 12:04:29 +0200 Subject: [PATCH 20/56] Ultrasonic TouchSensor subclass --- Arduino/Things/UltrasonicSensor.cpp | 27 +++++++++++++++------------ Arduino/Things/UltrasonicSensor.h | 24 +++++++++++++++++++----- 2 files changed, 34 insertions(+), 17 deletions(-) diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 646a4f8..c4c960e 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -8,7 +8,7 @@ namespace Arduino { UltrasonicSensor::UltrasonicSensor(Participant* participant, unsigned char pinTrigger, unsigned char pinEcho) - : TouchSensor(participant) { + : Thing(participant) { this->pinTrigger = pinTrigger; this->pinEcho = pinEcho; @@ -51,25 +51,28 @@ float UltrasonicSensor::GetDistance() { // if (distance > 30) // distance = 0; - this->touchedSomething |= (this->distance > 0 && this->distance <= this->touchDistance); - - // std::cout << "Ultrasonic " << this->touchedSomething << " | " << (this->distance > 0) << " " << - // (this->distance <= this->touchDistance) << "\n"; - return distance; } void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) { - this->touchedSomething = false; GetDistance(); Thing::Update(currentTimeMs, recursive); } -// void UltrasonicSensor::ProcessBinary(char* bytes) { -// this->touchedSomething = (bytes[0] == 1); -// if (this->touchedSomething) -// std::cout << "Touching something!\n"; -// } +UltrasonicSensor::TouchSensor::TouchSensor(Thing& parent, + unsigned char pinTrigger, + unsigned char pinEcho) + : RoboidControl::TouchSensor(&parent), + ultrasonic(&parent, pinTrigger, pinEcho) { + this->touchedSomething = false; +} + +void UltrasonicSensor::TouchSensor::Update(unsigned long currentTimeMs, + bool recursive) { + this->ultrasonic.Update(currentTimeMs, recursive); + this->touchedSomething = (this->ultrasonic.distance > 0 && + this->ultrasonic.distance <= this->touchDistance); +} } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index d67d50e..1e49220 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -6,11 +6,8 @@ namespace RoboidControl { namespace Arduino { /// @brief An HC-SR04 ultrasonic distance sensor -class UltrasonicSensor : public TouchSensor { +class UltrasonicSensor : Thing { public: - // Inherit all constructors - //using TouchSensor::TouchSensor; - /// @brief Setup an ultrasonic sensor /// @param participant The participant to use /// @param pinTrigger The pin number of the trigger signal @@ -25,7 +22,7 @@ class UltrasonicSensor : public TouchSensor { // parameters /// @brief The distance at which the object is considered to be touched - float touchDistance = 0.2f; + // float touchDistance = 0.2f; // state @@ -45,6 +42,23 @@ class UltrasonicSensor : public TouchSensor { unsigned char pinTrigger = 0; /// @brief The pin number of the echo signal unsigned char pinEcho = 0; + + public: + class TouchSensor; +}; + +class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { + public: + TouchSensor(Thing& parent, unsigned char pinTrigger, unsigned char pinEcho); + + float touchDistance = 0.2f; + + /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) + virtual void Update(unsigned long currentTimeMs, + bool recursive = false) override; + + protected: + UltrasonicSensor ultrasonic; }; } // namespace Arduino From 38fd8f51c8d6b9f92162140bcc14a50e00b30872 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 14 May 2025 12:36:06 +0200 Subject: [PATCH 21/56] Added readme about the examples folder name --- Examples/README.md | 1 + 1 file changed, 1 insertion(+) create mode 100644 Examples/README.md diff --git a/Examples/README.md b/Examples/README.md new file mode 100644 index 0000000..b694fe9 --- /dev/null +++ b/Examples/README.md @@ -0,0 +1 @@ +Important: this folder has to be names 'examples' exactly to maintain compatibility with Arduino \ No newline at end of file From ada89739642454e5b0f3ba85d4db3e2330dae4ba Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 14 May 2025 12:39:54 +0200 Subject: [PATCH 22/56] step 1 of changing examples map --- {Examples => examples-tmp}/BB2B.cpp | 0 {Examples => examples-tmp}/CMakeLists.txt | 0 {Examples => examples-tmp}/README.md | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename {Examples => examples-tmp}/BB2B.cpp (100%) rename {Examples => examples-tmp}/CMakeLists.txt (100%) rename {Examples => examples-tmp}/README.md (100%) diff --git a/Examples/BB2B.cpp b/examples-tmp/BB2B.cpp similarity index 100% rename from Examples/BB2B.cpp rename to examples-tmp/BB2B.cpp diff --git a/Examples/CMakeLists.txt b/examples-tmp/CMakeLists.txt similarity index 100% rename from Examples/CMakeLists.txt rename to examples-tmp/CMakeLists.txt diff --git a/Examples/README.md b/examples-tmp/README.md similarity index 100% rename from Examples/README.md rename to examples-tmp/README.md From 169d8421439df3129426b30399eca9c33bf07aa2 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 14 May 2025 12:40:23 +0200 Subject: [PATCH 23/56] Step2 of renaming examples map --- {examples-tmp => examples}/BB2B.cpp | 0 {examples-tmp => examples}/CMakeLists.txt | 0 {examples-tmp => examples}/README.md | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename {examples-tmp => examples}/BB2B.cpp (100%) rename {examples-tmp => examples}/CMakeLists.txt (100%) rename {examples-tmp => examples}/README.md (100%) diff --git a/examples-tmp/BB2B.cpp b/examples/BB2B.cpp similarity index 100% rename from examples-tmp/BB2B.cpp rename to examples/BB2B.cpp diff --git a/examples-tmp/CMakeLists.txt b/examples/CMakeLists.txt similarity index 100% rename from examples-tmp/CMakeLists.txt rename to examples/CMakeLists.txt diff --git a/examples-tmp/README.md b/examples/README.md similarity index 100% rename from examples-tmp/README.md rename to examples/README.md From 31c725b0caccf07464b52b34307a5bf6479f8cb0 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 15 May 2025 15:59:43 +0200 Subject: [PATCH 24/56] BB2B with HW works --- Arduino/Things/DRV8833.cpp | 129 +++++++++++++++------------- Arduino/Things/DRV8833.h | 76 +++++++++------- Arduino/Things/UltrasonicSensor.cpp | 49 +++++++---- Arduino/Things/UltrasonicSensor.h | 20 +++-- Thing.cpp | 8 +- Thing.h | 4 +- Things/DifferentialDrive.cpp | 33 +++---- Things/DifferentialDrive.h | 18 ++-- Things/DigitalSensor.cpp | 2 +- Things/DigitalSensor.h | 4 +- Things/Motor.cpp | 9 ++ Things/Motor.h | 23 +++++ Things/TouchSensor.cpp | 2 +- Things/TouchSensor.h | 4 +- 14 files changed, 229 insertions(+), 152 deletions(-) create mode 100644 Things/Motor.cpp create mode 100644 Things/Motor.h diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index dfd659c..5db7504 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -5,6 +5,46 @@ namespace RoboidControl { namespace Arduino { +DRV8833::DRV8833(Configuration config, Participant* participant) + : Thing(participant) { + this->pinStandby = pinStandby; + if (pinStandby != 255) + pinMode(pinStandby, OUTPUT); + + this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2); + this->motorA->SetName("Motor A"); + this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2); + this->motorB->SetName("Motor B"); +} + +DRV8833::DRV8833(Configuration config, Thing* parent) + : DRV8833(config, parent->owner) { + this->SetParent(parent); +} + +#pragma region Differential drive + +DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, + Participant* owner) + : RoboidControl::DifferentialDrive(owner), drv8833(config, owner) { + this->leftWheel = this->drv8833.motorA; + this->rightWheel = this->drv8833.motorB; +} + +DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, + Thing* parent) + : RoboidControl::DifferentialDrive(parent), drv8833(config) {} + +void DRV8833::DifferentialDrive::Update(unsigned long currentTimeMs, + bool recurse) { + RoboidControl::DifferentialDrive::Update(currentTimeMs, recurse); + this->drv8833.Update(currentTimeMs, recurse); +} + +#pragma endregion Differential drive + +#pragma region Motor + #if (ESP32) uint8_t DRV8833Motor::nextAvailablePwmChannel = 0; #endif @@ -13,9 +53,9 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse) - : Thing(driver->owner) { + : Motor(driver->owner) { this->SetParent(driver); - + this->pinIn1 = pinIn1; this->pinIn2 = pinIn2; @@ -33,34 +73,39 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver, pinMode(pinIn2, OUTPUT); // configure the in1 pin to output mode #endif - this->reverse = reverse; + // this->reverse = reverse; } -void DRV8833Motor::SetMaxRPM(unsigned int rpm) { - this->maxRpm = rpm; -} +// void DRV8833Motor::SetMaxRPM(unsigned int rpm) { +// this->maxRpm = rpm; +// } -void DRV8833Motor::SetAngularVelocity(Spherical velocity) { - Thing::SetAngularVelocity(velocity); - // ignoring rotation axis for now. - // Spherical angularVelocity = this->GetAngularVelocity(); - float angularSpeed = velocity.distance; // in degrees/sec +// void DRV8833Motor::SetAngularVelocity(Spherical velocity) { +void DRV8833Motor::SetTargetSpeed(float motorSpeed) { + Motor::SetTargetSpeed(motorSpeed); + // Thing::SetAngularVelocity(velocity); + // ignoring rotation axis for now. + // Spherical angularVelocity = this->GetAngularVelocity(); + // float angularSpeed = velocity.distance; // in degrees/sec - float rpm = angularSpeed / 360 * 60; - float motorSpeed = rpm / this->maxRpm; + // float rpm = angularSpeed / 360 * 60; + // float motorSpeed = rpm / this->maxRpm; - uint8_t motorSignal = (uint8_t)(motorSpeed * 255); - // if (direction == RotationDirection::CounterClockwise) - // speed = -speed; - // Determine the rotation direction - if (velocity.direction.horizontal.InDegrees() < 0) - motorSpeed = -motorSpeed; - if (this->reverse) - motorSpeed = -motorSpeed; + uint8_t motorSignal = + (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255); + // // if (direction == RotationDirection::CounterClockwise) + // // speed = -speed; + // // Determine the rotation direction + // if (velocity.direction.horizontal.InDegrees() < 0) + // motorSpeed = -motorSpeed; + // if (this->reverse) + // motorSpeed = -motorSpeed; // std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm - // " << rpm - // << ", motor signal = " << (int)motorSignal << "\n"; + // " + // << rpm << ", motor signal = " << (int)motorSignal << "\n"; + std::cout << "moto speed " << this->name << " = " << motorSpeed + << ", motor signal = " << (int)motorSignal << "\n"; #if (ESP32) if (motorSpeed == 0) { // stop @@ -109,43 +154,7 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) { #endif } -DRV8833::DRV8833(Participant* participant, - unsigned char pinAIn1, - unsigned char pinAIn2, - unsigned char pinBIn1, - unsigned char pinBIn2, - unsigned char pinStandby, - bool reverseA, - bool reverseB) - : Thing(participant) { - this->pinStandby = pinStandby; - if (pinStandby != 255) - pinMode(pinStandby, OUTPUT); - - this->motorA = new DRV8833Motor(this, pinAIn1, pinAIn2, reverseA); - this->motorA->SetName("Motor A"); - this->motorB = new DRV8833Motor(this, pinBIn1, pinBIn2, reverseB); - this->motorB->SetName("Motor B"); -} - -DRV8833::DRV8833(Thing* parent, - unsigned char pinAIn1, - unsigned char pinAIn2, - unsigned char pinBIn1, - unsigned char pinBIn2, - unsigned char pinStandby, - bool reverseA, - bool reverseB) - : DRV8833(parent->owner, - pinAIn1, - pinAIn2, - pinBIn1, - pinBIn2, - pinStandby, - reverseA, - reverseB) { - this->SetParent(parent); -} +#pragma endregion Motor } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index cbd63d6..838bb9a 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -1,8 +1,10 @@ #pragma once #include +#include "Participants/IsolatedParticipant.h" #include "Thing.h" #include "Things/DifferentialDrive.h" +#include "Things/Motor.h" namespace RoboidControl { namespace Arduino { @@ -11,43 +13,49 @@ class DRV8833Motor; class DRV8833 : public Thing { public: + struct Configuration { + int AIn1; + int AIn2; + int BIn1; + int BIn2; + }; + /// @brief Setup a DRV8833 motor controller - /// @param pinAIn1 The pin number connected to the AIn1 port - /// @param pinAIn2 The pin number connected to the AIn2 port - /// @param pinBIn1 The pin number connected to the BIn1 port - /// @param pinBIn2 The pin number connceted to the BIn2 port - /// @param pinStandby The pin number connected to the standby port, 255 - /// indicated that the port is not connected - /// @param reverseA The forward turning direction of motor A - /// @param reverseB The forward turning direction of motor B - DRV8833(Participant* participant, - unsigned char pinAIn1, - unsigned char pinAIn2, - unsigned char pinBIn1, - unsigned char pinBIn2, - unsigned char pinStandby = 255, - bool reverseA = false, - bool reverseB = false); - DRV8833(Thing* parent, - unsigned char pinAIn1, - unsigned char pinAIn2, - unsigned char pinBIn1, - unsigned char pinBIn2, - unsigned char pinStandby = 255, - bool reverseA = false, - bool reverseB = false); + DRV8833(Configuration config, Participant* owner = nullptr); + DRV8833(Configuration config, Thing* parent); + DRV8833Motor* motorA = nullptr; DRV8833Motor* motorB = nullptr; protected: unsigned char pinStandby = 255; + + public: + class DifferentialDrive; }; -/// @brief Support for a DRV8833 motor controller -class DRV8833Motor : public Thing { +#pragma region Differential drive + +class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { + public: + DifferentialDrive(DRV8833::Configuration config, + Participant* participant = nullptr); + DifferentialDrive(DRV8833::Configuration config, Thing* parent); + + virtual void Update(unsigned long currentTimeMs, + bool recurse = false) override; + + protected: + DRV8833 drv8833; +}; + +#pragma endregion Differential drive + +#pragma region Motor + +/// @brief Support for a DRV8833 motor controller +class DRV8833Motor : public Motor { public: - /// @brief Motor turning direction - enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 }; /// @brief Setup the DC motor /// @param pinIn1 the pin number for the in1 signal @@ -57,16 +65,16 @@ class DRV8833Motor : public Thing { unsigned char pinIn1, unsigned char pinIn2, bool reverse = false); - void SetMaxRPM(unsigned int rpm); + //void SetMaxRPM(unsigned int rpm); - virtual void SetAngularVelocity(Spherical velocity) override; - - bool reverse = false; + //virtual void SetAngularVelocity(Spherical velocity) override; + virtual void SetTargetSpeed(float targetSpeed) override; + //bool reverse = false; protected: unsigned char pinIn1 = 255; unsigned char pinIn2 = 255; - unsigned int maxRpm = 200; + //unsigned int maxRpm = 200; #if (ESP32) uint8_t in1Ch; @@ -75,5 +83,7 @@ class DRV8833Motor : public Thing { #endif }; +#pragma endregion Moto + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index c4c960e..f86a6d1 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -1,39 +1,37 @@ #include "UltrasonicSensor.h" #include +#include namespace RoboidControl { namespace Arduino { -UltrasonicSensor::UltrasonicSensor(Participant* participant, - unsigned char pinTrigger, - unsigned char pinEcho) +UltrasonicSensor::UltrasonicSensor(Configuration config, + Participant* participant) : Thing(participant) { - this->pinTrigger = pinTrigger; - this->pinEcho = pinEcho; + this->pinTrigger = config.triggerPin; + this->pinEcho = config.echoPin; pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode pinMode(pinEcho, INPUT); // configure the echo pin to input mode } -UltrasonicSensor::UltrasonicSensor(Thing* parent, - unsigned char pinTrigger, - unsigned char pinEcho) - : UltrasonicSensor(parent->owner, pinTrigger, pinEcho) { +UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) + : UltrasonicSensor(config, parent->owner) { this->SetParent(parent); } float UltrasonicSensor::GetDistance() { // Start the ultrasonic 'ping' digitalWrite(pinTrigger, LOW); - delayMicroseconds(5); + delayMicroseconds(2); digitalWrite(pinTrigger, HIGH); delayMicroseconds(10); digitalWrite(pinTrigger, LOW); // Measure the duration of the pulse on the echo pin - float duration_us = - pulseIn(pinEcho, HIGH, 100000); // the result is in microseconds + unsigned long duration_us = + pulseIn(pinEcho, HIGH); // the result is in microseconds // Calculate the distance: // * Duration should be divided by 2, because the ping goes to the object @@ -44,14 +42,18 @@ float UltrasonicSensor::GetDistance() { // * Now we calculate the distance based on the speed of sound (340 m/s): // distance = duration_sec * 340; // * The result calculation is therefore: - this->distance = duration_us / 2 / 1000000 * 340; + this->distance = (float)duration_us / 2 / 1000000 * 340; + + // Serial.println(this->distance); + + // std::cout << "US distance " << this->distance << std::endl; // Filter faulty measurements. The sensor can often give values > 30 m which // are not correct // if (distance > 30) // distance = 0; - return distance; + return this->distance; } void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) { @@ -59,11 +61,18 @@ void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) { Thing::Update(currentTimeMs, recursive); } -UltrasonicSensor::TouchSensor::TouchSensor(Thing& parent, - unsigned char pinTrigger, - unsigned char pinEcho) - : RoboidControl::TouchSensor(&parent), - ultrasonic(&parent, pinTrigger, pinEcho) { +#pragma region Touch sensor + +UltrasonicSensor::TouchSensor::TouchSensor( + UltrasonicSensor::Configuration config, + Thing& parent) + : RoboidControl::TouchSensor(&parent), ultrasonic(config, &parent) { + this->touchedSomething = false; +} +UltrasonicSensor::TouchSensor::TouchSensor( + UltrasonicSensor::Configuration config, + Thing* parent) + : RoboidControl::TouchSensor(parent), ultrasonic(config, parent) { this->touchedSomething = false; } @@ -74,5 +83,7 @@ void UltrasonicSensor::TouchSensor::Update(unsigned long currentTimeMs, this->ultrasonic.distance <= this->touchDistance); } +#pragma region Touch sensor + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index 1e49220..d612777 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -8,16 +8,17 @@ namespace Arduino { /// @brief An HC-SR04 ultrasonic distance sensor class UltrasonicSensor : Thing { public: + struct Configuration { + int triggerPin; + int echoPin; + }; + /// @brief Setup an ultrasonic sensor /// @param participant The participant to use /// @param pinTrigger The pin number of the trigger signal /// @param pinEcho The pin number of the echo signal - UltrasonicSensor(Participant* participant, - unsigned char pinTrigger, - unsigned char pinEcho); - UltrasonicSensor(Thing* parent, - unsigned char pinTrigger, - unsigned char pinEcho); + UltrasonicSensor(Configuration config, Participant* participant); + UltrasonicSensor(Configuration config, Thing* parent); // parameters @@ -47,9 +48,12 @@ class UltrasonicSensor : Thing { class TouchSensor; }; +#pragma region Touch sensor + class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { public: - TouchSensor(Thing& parent, unsigned char pinTrigger, unsigned char pinEcho); + TouchSensor(UltrasonicSensor::Configuration config, Thing& parent); + TouchSensor(UltrasonicSensor::Configuration config, Thing* parent); float touchDistance = 0.2f; @@ -61,5 +65,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { UltrasonicSensor ultrasonic; }; +#pragma region Touch sensor + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Thing.cpp b/Thing.cpp index 4fe968a..0807295 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -5,6 +5,7 @@ #include "Participants/IsolatedParticipant.h" #include +// #include #if defined(ARDUINO) #include "Arduino.h" @@ -19,12 +20,14 @@ namespace RoboidControl { #pragma region Init -Thing::Thing(unsigned char thingType) - : Thing(IsolatedParticipant::Isolated(), thingType) {} +// Thing::Thing(unsigned char thingType) +// : Thing(IsolatedParticipant::Isolated(), thingType) {} Thing::Thing(Participant* owner, unsigned char thingType, unsigned char thingId) { + if (owner == nullptr) + owner = IsolatedParticipant::Isolated(); this->owner = owner; this->id = thingId; this->type = thingType; @@ -245,6 +248,7 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) { this->nameChanged = false; if (recursive) { + // std::cout << "# chilren: " << (int)this->childCount << std::endl; for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { Thing* child = this->children[childIx]; if (child == nullptr) diff --git a/Thing.h b/Thing.h index 691b6f5..536d050 100644 --- a/Thing.h +++ b/Thing.h @@ -43,14 +43,14 @@ class Thing { /// @brief Create a new thing without communication abilities /// @param thingType The type of thing (can use Thing::Type) - Thing(unsigned char thingType = Type::Undetermined); + //Thing(unsigned char thingType = Type::Undetermined); /// @brief Create a new thing for a participant /// @param owner The owning participant /// @param thingType The type of thing (can use Thing::Type) /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - Thing(Participant* owner, + Thing(Participant* owner = nullptr, unsigned char thingType = Type::Undetermined, unsigned char thingId = 0); diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index 80567a1..f8fc0ca 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -2,12 +2,15 @@ namespace RoboidControl { - DifferentialDrive::DifferentialDrive() : Thing(Thing::Type::DifferentialDrive) {} +DifferentialDrive::DifferentialDrive(Participant* participant, + unsigned char thingId) + : Thing(participant, Thing::Type::DifferentialDrive, thingId) { + this->leftWheel = new Motor(); + this->rightWheel = new Motor(); +} -DifferentialDrive::DifferentialDrive(Participant* participant, unsigned char thingId) - : Thing(participant, Thing::Type::DifferentialDrive, thingId) {} - -DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) : Thing(parent, Thing::Type::DifferentialDrive, thingId) {} +DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) + : Thing(parent, Thing::Type::DifferentialDrive, thingId) {} void DifferentialDrive::SetDriveDimensions(float wheelDiameter, float wheelSeparation) { @@ -24,17 +27,17 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter, this->rightWheel->SetPosition(Spherical(distance, Direction::right)); } -void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) { - float distance = this->wheelSeparation / 2; - this->leftWheel = leftWheel; - ; - if (leftWheel != nullptr) - this->leftWheel->SetPosition(Spherical(distance, Direction::left)); +// void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) { +// float distance = this->wheelSeparation / 2; +// this->leftWheel = leftWheel; +// ; +// if (leftWheel != nullptr) +// this->leftWheel->SetPosition(Spherical(distance, Direction::left)); - this->rightWheel = rightWheel; - if (rightWheel != nullptr) - this->rightWheel->SetPosition(Spherical(distance, Direction::right)); -} +// this->rightWheel = rightWheel; +// if (rightWheel != nullptr) +// this->rightWheel->SetPosition(Spherical(distance, Direction::right)); +// } void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) { if (this->leftWheel != nullptr) diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index 000d8fd..f352611 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -1,6 +1,7 @@ #pragma once #include "Thing.h" +#include "Motor.h" namespace RoboidControl { @@ -9,13 +10,14 @@ namespace RoboidControl { /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink class DifferentialDrive : public Thing { public: - /// @brief Create a differential drive without networking support - DifferentialDrive(); + // /// @brief Create a differential drive without networking support + // DifferentialDrive(); /// @brief Create a differential drive with networking support /// @param participant The local participant /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DifferentialDrive(Participant* participant, unsigned char thingId = 0); + DifferentialDrive(Participant* participant = nullptr, + unsigned char thingId = 0); /// @brief Create a new child differential drive /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate @@ -45,6 +47,11 @@ class DifferentialDrive : public Thing { /// @copydoc RoboidControl::Thing::Update(unsigned long) virtual void Update(unsigned long currentMs, bool recursive = true) override; + /// @brief The left wheel + Motor* leftWheel = nullptr; + /// @brief The right wheel + Motor* rightWheel = nullptr; + protected: /// @brief The radius of a wheel in meters float wheelRadius = 1.0f; @@ -53,11 +60,6 @@ class DifferentialDrive : public Thing { /// @brief Convert revolutions per second to meters per second float rpsToMs = 1.0f; - - /// @brief The left wheel - Thing* leftWheel = nullptr; - /// @brief The right wheel - Thing* rightWheel = nullptr; }; } // namespace RoboidControl \ No newline at end of file diff --git a/Things/DigitalSensor.cpp b/Things/DigitalSensor.cpp index 528bd7a..5eae131 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -2,7 +2,7 @@ namespace RoboidControl { -DigitalSensor::DigitalSensor() : Thing(Type::Switch) {} +//DigitalSensor::DigitalSensor() : Thing(Type::Switch) {} DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId) : Thing(owner, Type::Switch, thingId) {} diff --git a/Things/DigitalSensor.h b/Things/DigitalSensor.h index 95ab1b3..520ae4c 100644 --- a/Things/DigitalSensor.h +++ b/Things/DigitalSensor.h @@ -8,12 +8,12 @@ namespace RoboidControl { class DigitalSensor : public Thing { public: /// @brief Create a digital sensor without communication abilities - DigitalSensor(); + //DigitalSensor(); /// @brief Create a digital sensor for a participant /// @param owner The owning participant /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DigitalSensor(Participant* owner, unsigned char thingId = 0); + DigitalSensor(Participant* owner = nullptr, unsigned char thingId = 0); /// @brief Create a new child digital sensor /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate diff --git a/Things/Motor.cpp b/Things/Motor.cpp new file mode 100644 index 0000000..7d3abd7 --- /dev/null +++ b/Things/Motor.cpp @@ -0,0 +1,9 @@ +#include "Motor.h" + +RoboidControl::Motor::Motor(Participant* owner) {} + +RoboidControl::Motor::Motor(Thing* parent) {} + +void RoboidControl::Motor::SetTargetSpeed(float targetSpeed) { + this->targetSpeed = targetSpeed; +} diff --git a/Things/Motor.h b/Things/Motor.h new file mode 100644 index 0000000..1ff6716 --- /dev/null +++ b/Things/Motor.h @@ -0,0 +1,23 @@ +#pragma once + +#include "Thing.h" + +namespace RoboidControl { + +class Motor : public Thing { + public: + Motor(Participant* owner = nullptr); + Motor(Thing* parent); + + /// @brief Motor turning direction + enum class Direction { Clockwise = 1, CounterClockwise = -1 }; + /// @brief The forward turning direction of the motor + Direction direction; + + virtual void SetTargetSpeed(float targetSpeed); // -1..0..1 + + protected: + float targetSpeed = 0; +}; + +} // namespace RoboidControl \ No newline at end of file diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index cef0c9e..809fa9d 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -2,7 +2,7 @@ namespace RoboidControl { -TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {} +//TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {} TouchSensor::TouchSensor(Participant* owner, unsigned char thingId) : Thing(owner, Thing::Type::TouchSensor, thingId) {} diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index 42d6a0b..c176145 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -9,12 +9,12 @@ class TouchSensor : public Thing { // Why finishing this release (0.3), I notice that this is equivalent to a digital sensor public: /// @brief Create a touch sensor without communication abilities - TouchSensor(); + //TouchSensor(); /// @brief Create a touch sensor for a participant /// @param owner The owning participant /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - TouchSensor(Participant* owner, unsigned char thingId = 0); + TouchSensor(Participant* owner = nullptr, unsigned char thingId = 0); /// @brief Create a new child touch sensor /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate From 26be4557c5e7c2a4c79250cee3c8062ceba70ffd Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 15 May 2025 19:52:24 +0200 Subject: [PATCH 25/56] Added thing reconstruct --- Participants/SiteServer.cpp | 3 ++- Thing.cpp | 16 +++++++++++----- Thing.h | 7 ++++--- 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index bfe1c2d..5dc6361 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -72,7 +72,8 @@ void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {} void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing* thing = sender->Get(msg->thingId); if (thing == nullptr) - new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); + // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); + Thing::Reconstruct(sender, msg->thingType, msg->thingId); if (msg->parentId != 0) { thing->SetParent(Get(msg->parentId)); diff --git a/Thing.cpp b/Thing.cpp index 0807295..e14a2bc 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -24,12 +24,12 @@ namespace RoboidControl { // : Thing(IsolatedParticipant::Isolated(), thingType) {} Thing::Thing(Participant* owner, - unsigned char thingType, - unsigned char thingId) { + unsigned char thingType) { + // unsigned char thingId) { if (owner == nullptr) owner = IsolatedParticipant::Isolated(); this->owner = owner; - this->id = thingId; + // this->id = thingId; this->type = thingType; this->position = Spherical::zero; @@ -47,11 +47,17 @@ Thing::Thing(Participant* owner, this->owner->Add(this, true); } -Thing::Thing(Thing* parent, unsigned char thingType, unsigned char thingId) - : Thing(parent->owner, thingType, thingId) { +Thing::Thing(Thing* parent, unsigned char thingType) //, unsigned char thingId) + : Thing(parent->owner, thingType) { //}, thingId) { this->SetParent(parent); } +Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, unsigned char thingId) { + Thing thing = Thing(owner, thingType); + thing.id = thingId; + return thing; +} + #pragma endregion Init void Thing::SetName(const char* name) { diff --git a/Thing.h b/Thing.h index 536d050..0827886 100644 --- a/Thing.h +++ b/Thing.h @@ -51,8 +51,8 @@ class Thing { /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID Thing(Participant* owner = nullptr, - unsigned char thingType = Type::Undetermined, - unsigned char thingId = 0); + unsigned char thingType = Type::Undetermined); + //unsigned char thingId = 0); /// @brief Create a new child thing /// @param parent The parent thing @@ -60,8 +60,9 @@ class Thing { /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID /// @note The owner will be the same as the owner of the parent thing - Thing(Thing* parent, unsigned char thingType = 0, unsigned char thingId = 0); + Thing(Thing* parent, unsigned char thingType = 0); //, unsigned char thingId = 0); + static Thing Reconstruct(Participant* owner, unsigned char thingType, unsigned char thingId); #pragma endregion Init public: From 17de9db0be248697baf1b3935cbf25927bac50c5 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 16 May 2025 11:54:40 +0200 Subject: [PATCH 26/56] Added rel. encoder, removed currentTime in Update() --- Arduino/Things/DRV8833.cpp | 29 ++----- Arduino/Things/DRV8833.h | 14 ++-- Arduino/Things/DigitalInput.cpp | 117 ++++++++++++++++++++++++++-- Arduino/Things/DigitalInput.h | 68 +++++++++++++++- Arduino/Things/UltrasonicSensor.cpp | 11 ++- Arduino/Things/UltrasonicSensor.h | 6 +- Participant.cpp | 4 +- Participant.h | 2 +- Participants/ParticipantUDP.cpp | 25 +++--- Participants/ParticipantUDP.h | 6 +- Participants/SiteServer.cpp | 4 +- Participants/SiteServer.h | 2 +- Thing.cpp | 19 ++--- Thing.h | 10 ++- Things/DifferentialDrive.cpp | 25 ++++-- Things/DifferentialDrive.h | 8 +- Things/DigitalSensor.cpp | 12 ++- Things/DigitalSensor.h | 4 +- Things/Motor.cpp | 4 +- Things/RelativeEncoder.cpp | 23 ++++++ Things/RelativeEncoder.h | 38 +++++++++ Things/TemperatureSensor.cpp | 10 ++- Things/TemperatureSensor.h | 3 +- Things/TouchSensor.cpp | 18 +++-- Things/TouchSensor.h | 4 +- 25 files changed, 347 insertions(+), 119 deletions(-) create mode 100644 Things/RelativeEncoder.cpp create mode 100644 Things/RelativeEncoder.h diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 5db7504..80fc402 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -35,10 +35,9 @@ DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, Thing* parent) : RoboidControl::DifferentialDrive(parent), drv8833(config) {} -void DRV8833::DifferentialDrive::Update(unsigned long currentTimeMs, - bool recurse) { - RoboidControl::DifferentialDrive::Update(currentTimeMs, recurse); - this->drv8833.Update(currentTimeMs, recurse); +void DRV8833::DifferentialDrive::Update(bool recurse) { + RoboidControl::DifferentialDrive::Update(recurse); + this->drv8833.Update(recurse); } #pragma endregion Differential drive @@ -80,32 +79,14 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver, // this->maxRpm = rpm; // } -// void DRV8833Motor::SetAngularVelocity(Spherical velocity) { void DRV8833Motor::SetTargetSpeed(float motorSpeed) { Motor::SetTargetSpeed(motorSpeed); - // Thing::SetAngularVelocity(velocity); - // ignoring rotation axis for now. - // Spherical angularVelocity = this->GetAngularVelocity(); - // float angularSpeed = velocity.distance; // in degrees/sec - - // float rpm = angularSpeed / 360 * 60; - // float motorSpeed = rpm / this->maxRpm; uint8_t motorSignal = (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255); - // // if (direction == RotationDirection::CounterClockwise) - // // speed = -speed; - // // Determine the rotation direction - // if (velocity.direction.horizontal.InDegrees() < 0) - // motorSpeed = -motorSpeed; - // if (this->reverse) - // motorSpeed = -motorSpeed; - // std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm - // " - // << rpm << ", motor signal = " << (int)motorSignal << "\n"; - std::cout << "moto speed " << this->name << " = " << motorSpeed - << ", motor signal = " << (int)motorSignal << "\n"; + // std::cout << "moto speed " << this->name << " = " << motorSpeed + // << ", motor signal = " << (int)motorSignal << "\n"; #if (ESP32) if (motorSpeed == 0) { // stop diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index 838bb9a..2da2664 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -42,8 +42,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { Participant* participant = nullptr); DifferentialDrive(DRV8833::Configuration config, Thing* parent); - virtual void Update(unsigned long currentTimeMs, - bool recurse = false) override; + virtual void Update(bool recurse = false) override; protected: DRV8833 drv8833; @@ -56,7 +55,6 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { /// @brief Support for a DRV8833 motor controller class DRV8833Motor : public Motor { public: - /// @brief Setup the DC motor /// @param pinIn1 the pin number for the in1 signal /// @param pinIn2 the pin number for the in2 signal @@ -65,16 +63,16 @@ class DRV8833Motor : public Motor { unsigned char pinIn1, unsigned char pinIn2, bool reverse = false); - //void SetMaxRPM(unsigned int rpm); + // void SetMaxRPM(unsigned int rpm); - //virtual void SetAngularVelocity(Spherical velocity) override; - virtual void SetTargetSpeed(float targetSpeed) override; - //bool reverse = false; + // virtual void SetAngularVelocity(Spherical velocity) override; + virtual void SetTargetSpeed(float targetSpeed) override; + // bool reverse = false; protected: unsigned char pinIn1 = 255; unsigned char pinIn2 = 255; - //unsigned int maxRpm = 200; + // unsigned int maxRpm = 200; #if (ESP32) uint8_t in1Ch; diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index d972ddb..5c3a2c2 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -5,19 +5,122 @@ namespace RoboidControl { namespace Arduino { +#pragma region Digital input + DigitalInput::DigitalInput(Participant* participant, unsigned char pin) - : TouchSensor(participant) { + : Thing(participant) { this->pin = pin; - pinMode(pin, INPUT); + pinMode(this->pin, INPUT); } -void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) { - this->touchedSomething = digitalRead(pin) == LOW; - // std::cout << "DigitalINput pin " << (int)this->pin << ": " << - // this->touchedSomething << "\n"; - Thing::Update(currentTimeMs, recursive); +DigitalInput::DigitalInput(Thing* parent, unsigned char pin) : Thing(parent) { + this->pin = pin; + + pinMode(this->pin, INPUT); } +void DigitalInput::Update(bool recursive) { + this->isHigh = digitalRead(this->pin); + this->isLow = !this->isHigh; + // std::cout << "DigitalINput pin " << (int)this->pin << ": " << + Thing::Update(recursive); +} + +#pragma endregion Digital input + +#pragma region Touch sensor + +DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent) + : RoboidControl::TouchSensor(parent), digitalInput(parent, pin) {} + +void DigitalInput::TouchSensor::Update(bool recursive) { + this->touchedSomething = digitalInput.isLow; +} + +#pragma endregion Touch sensor + +#pragma region Relative encoder + +int DigitalInput::RelativeEncoder::interruptCount = 0; +volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0; +volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0; + +DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, + Thing* parent) + : RoboidControl::RelativeEncoder(parent), + digitalInput(parent, config.pin), + pulsesPerRevolution(config.pulsesPerRevolution) { + + // We support up to 2 pulse counters + if (interruptCount == 0) + attachInterrupt(digitalPinToInterrupt(config.pin), PulseInterrupt0, RISING); + else if (interruptCount == 1) + attachInterrupt(digitalPinToInterrupt(config.pin), PulseInterrupt1, RISING); + else { + // maximum interrupt count reached + std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached" + << std::endl; + return; + } + interruptIx = interruptCount; + interruptCount++; +} + +int DigitalInput::RelativeEncoder::GetPulseCount() { + if (interruptIx == 0) { + int pulseCount = pulseCount0; + pulseCount0 = 0; + return pulseCount; + } else if (interruptIx == 1) { + int pulseCount = pulseCount1; + pulseCount1 = 0; + return pulseCount; + } + return 0; +} + +void DigitalInput::RelativeEncoder::Update(bool recursive) { + unsigned long currentTimeMs = GetTimeMs(); + if (this->lastUpdateTime == 0) { + this->lastUpdateTime = currentTimeMs; + return; + } + + float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f; + int pulseCount = GetPulseCount(); + netPulseCount += pulseCount; + + this->pulseFrequency = pulseCount / timeStep; + this->rotationSpeed = pulseFrequency / pulsesPerRevolution; + + // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency + // << " timestep " << timeStep << std::endl; + + this->lastUpdateTime = currentTimeMs; +} + +#if defined(ESP8266) || defined(ESP32) +void ICACHE_RAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt0() { + pulseCount0++; +} +#else +void DigitalInput::RelativeEncoder::PulseInterrupt0() { + pulseCount0++; +} +#endif + +#if defined(ESP8266) || defined(ESP32) +void IRAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt1() { + pulseCount1++; +} +#else +void DigitalInput::RelativeEncoder::PulseInterrupt1() { + pulseCount1++; +} +#endif + +#pragma endregion Relative encoder + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/DigitalInput.h b/Arduino/Things/DigitalInput.h index 6099918..966cfa8 100644 --- a/Arduino/Things/DigitalInput.h +++ b/Arduino/Things/DigitalInput.h @@ -1,26 +1,88 @@ #pragma once +#include "Things/RelativeEncoder.h" #include "Things/TouchSensor.h" namespace RoboidControl { namespace Arduino { /// @brief A digital input represents the stat of a digital GPIO pin -class DigitalInput : public TouchSensor { +class DigitalInput : public Thing { public: /// @brief Create a new digital input /// @param participant The participant to use /// @param pin The digital pin DigitalInput(Participant* participant, unsigned char pin); + DigitalInput(Thing* parent, unsigned char pin); + + bool isHigh = false; + bool isLow = false; /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) - virtual void Update(unsigned long currentTimeMs, - bool recursive = false) override; + virtual void Update(bool recursive = false) override; protected: /// @brief The pin used for digital input unsigned char pin = 0; + + public: + class TouchSensor; + class RelativeEncoder; }; +#pragma region Touch sensor + +class DigitalInput::TouchSensor : public RoboidControl::TouchSensor { + public: + TouchSensor(unsigned char pin, Thing* parent); + + /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) + virtual void Update(bool recurse = false) override; + + protected: + DigitalInput digitalInput; +}; + +#pragma endregion Touch sensor + +#pragma region Incremental encoder + +class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder { + public: + struct Configuration { + unsigned char pin; + unsigned char pulsesPerRevolution; + }; + + RelativeEncoder(Configuration config, Thing* parent); + + unsigned char pulsesPerRevolution; + + /// @brief The current pulse frequency in Hz + float pulseFrequency = 0; + + /// @copydoc RoboidControl::Thing::Update() + virtual void Update(bool recurse = false) override; + + protected: + DigitalInput digitalInput; + + int interruptIx = 0; + + static int interruptCount; + + volatile static int pulseCount0; + static void PulseInterrupt0(); + + volatile static int pulseCount1; + static void PulseInterrupt1(); + + int GetPulseCount(); + long netPulseCount = 0; + unsigned long lastUpdateTime = 0; +}; + +#pragma endregion Incremental encoder + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index f86a6d1..95d0bb6 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -31,7 +31,7 @@ float UltrasonicSensor::GetDistance() { // Measure the duration of the pulse on the echo pin unsigned long duration_us = - pulseIn(pinEcho, HIGH); // the result is in microseconds + pulseIn(pinEcho, HIGH, 10000); // the result is in microseconds // Calculate the distance: // * Duration should be divided by 2, because the ping goes to the object @@ -56,9 +56,9 @@ float UltrasonicSensor::GetDistance() { return this->distance; } -void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) { +void UltrasonicSensor::Update(bool recursive) { GetDistance(); - Thing::Update(currentTimeMs, recursive); + Thing::Update(recursive); } #pragma region Touch sensor @@ -76,9 +76,8 @@ UltrasonicSensor::TouchSensor::TouchSensor( this->touchedSomething = false; } -void UltrasonicSensor::TouchSensor::Update(unsigned long currentTimeMs, - bool recursive) { - this->ultrasonic.Update(currentTimeMs, recursive); +void UltrasonicSensor::TouchSensor::Update(bool recursive) { + this->ultrasonic.Update(recursive); this->touchedSomething = (this->ultrasonic.distance > 0 && this->ultrasonic.distance <= this->touchDistance); } diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index d612777..563594e 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -35,8 +35,7 @@ class UltrasonicSensor : Thing { float GetDistance(); /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) - virtual void Update(unsigned long currentTimeMs, - bool recursive = false) override; + virtual void Update(bool recursive = false) override; protected: /// @brief The pin number of the trigger signal @@ -58,8 +57,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { float touchDistance = 0.2f; /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) - virtual void Update(unsigned long currentTimeMs, - bool recursive = false) override; + virtual void Update(bool recursive = false) override; protected: UltrasonicSensor ultrasonic; diff --git a/Participant.cpp b/Participant.cpp index 82a3555..03197e6 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -30,10 +30,10 @@ Participant::~Participant() { delete[] this->ipAddress; } -void Participant::Update(unsigned long currentTimeMs) { +void Participant::Update() { for (Thing* thing : this->things) { if (thing != nullptr) - thing->Update(currentTimeMs, true); + thing->Update(true); } } diff --git a/Participant.h b/Participant.h index 358746b..49ee749 100644 --- a/Participant.h +++ b/Participant.h @@ -94,7 +94,7 @@ class Participant { /// @brief Update all things for this participant /// @param currentTimeMs The current time in milliseconds (optional) - virtual void Update(unsigned long currentTimeMs = 0); + virtual void Update(); public: static ParticipantRegistry registry; diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index c6c4bb1..5aa4142 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -4,8 +4,8 @@ #include "Arduino/ArduinoParticipant.h" #include "EspIdf/EspIdfParticipant.h" -#include "Windows/WindowsParticipant.h" #include "Posix/PosixParticipant.h" +#include "Windows/WindowsParticipant.h" #include @@ -66,9 +66,8 @@ void ParticipantUDP::SetupUDP(int localPort, #pragma region Update -void ParticipantUDP::Update(unsigned long currentTimeMs) { - if (currentTimeMs == 0) - currentTimeMs = Thing::GetTimeMs(); +void ParticipantUDP::Update() { + unsigned long currentTimeMs = Thing::GetTimeMs(); if (this->isIsolated == false) { if (this->connected == false) @@ -88,11 +87,11 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { this->ReceiveUDP(); } - UpdateMyThings(currentTimeMs); - UpdateOtherThings(currentTimeMs); + UpdateMyThings(); + UpdateOtherThings(); } -void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { +void ParticipantUDP::UpdateMyThings() { for (Thing* thing : this->things) { if (thing == nullptr) // || thing->GetParent() != nullptr) continue; @@ -102,7 +101,7 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); this->Send(this->remoteSite, thingMsg); delete thingMsg; - + if (thing->nameChanged) { NameMsg* nameMsg = new NameMsg(this->networkId, thing); this->Send(this->remoteSite, nameMsg); @@ -115,7 +114,7 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { // 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! - thing->Update(currentTimeMs, false); + thing->Update(false); if (!(this->isIsolated || this->networkId == 0)) { if (thing->terminate) { @@ -142,21 +141,21 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { } } -void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) { +void ParticipantUDP::UpdateOtherThings() { #if defined(NO_STD) Participant** participants = Participant::registry.GetAll(); for (int ix = 0; ix < Participant::registry.count; ix++) { Participant* participant = participants[ix]; -#else +#else for (Participant* participant : Participant::registry.GetAll()) { -#endif +#endif if (participant == nullptr || participant == this) continue; // Call only the Participant version of the Update. // This is to deal with the function where one of the (remote) // participants is actually a local participant - participant->Participant::Update(currentTimeMs); + participant->Participant::Update(); if (this->isIsolated) continue; diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 8c2efdf..c51131d 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -100,13 +100,13 @@ class ParticipantUDP : public Participant { #pragma region Update public: - virtual void Update(unsigned long currentTimeMs = 0) override; + virtual void Update() override; protected: unsigned long nextPublishMe = 0; - virtual void UpdateMyThings(unsigned long currentTimeMs); - virtual void UpdateOtherThings(unsigned long currentTimeMs); + virtual void UpdateMyThings(); + virtual void UpdateOtherThings(); #pragma endregion Update diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 5dc6361..a24bf09 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -22,12 +22,12 @@ SiteServer::SiteServer(int port) : ParticipantUDP(port) { #pragma region Update -void SiteServer::UpdateMyThings(unsigned long currentTimeMs) { +void SiteServer::UpdateMyThings() { for (Thing* thing : this->things) { if (thing == nullptr) continue; - thing->Update(currentTimeMs, true); + thing->Update( true); if (this->isIsolated == false) { // Send to all other participants diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index a4e6767..67bc5bf 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -24,7 +24,7 @@ class SiteServer : public ParticipantUDP { #pragma region Update - virtual void UpdateMyThings(unsigned long currentTimeMs) override; + virtual void UpdateMyThings() override; #pragma endregion Update diff --git a/Thing.cpp b/Thing.cpp index e14a2bc..bbf82e3 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -230,7 +230,8 @@ Spherical Thing::GetAngularVelocity() { unsigned long Thing::GetTimeMs() { #if defined(ARDUINO) - return millis(); + unsigned long ms = millis(); + return ms; #else auto now = std::chrono::steady_clock::now(); auto ms = std::chrono::duration_cast( @@ -239,11 +240,11 @@ unsigned long Thing::GetTimeMs() { #endif } -void Thing::Update(bool recursive) { - Update(GetTimeMs(), recursive); -} +// void Thing::Update(bool recursive) { +// Update(GetTimeMs(), recursive); +// } -void Thing::Update(unsigned long currentTimeMs, bool recursive) { +void Thing::Update(bool recursive) { // if (this->positionUpdated || this->orientationUpdated) // OnPoseChanged callback this->positionUpdated = false; @@ -254,18 +255,18 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) { this->nameChanged = false; if (recursive) { - // std::cout << "# chilren: " << (int)this->childCount << std::endl; + // std::cout << "# children: " << (int)this->childCount << std::endl; for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { Thing* child = this->children[childIx]; if (child == nullptr) continue; - child->Update(currentTimeMs, recursive); + child->Update(recursive); } } } -void Thing::UpdateThings(unsigned long currentTimeMs) { - IsolatedParticipant::Isolated()->Update(currentTimeMs); +void Thing::UpdateThings() { + IsolatedParticipant::Isolated()->Update(); } #pragma endregion Update diff --git a/Thing.h b/Thing.h index 0827886..9045430 100644 --- a/Thing.h +++ b/Thing.h @@ -32,6 +32,7 @@ class Thing { ControlledMotor, UncontrolledMotor, Servo, + IncrementalEncoder, // Other Roboid, Humanoid, @@ -63,7 +64,8 @@ class Thing { Thing(Thing* parent, unsigned char thingType = 0); //, unsigned char thingId = 0); static Thing Reconstruct(Participant* owner, unsigned char thingType, unsigned char thingId); -#pragma endregion Init + + #pragma endregion Init public: /// @brief Terminated things are no longer updated @@ -214,15 +216,15 @@ class Thing { static unsigned long GetTimeMs(); /// @brief Updates the state of the thing - void Update(bool recursive = false); + //void Update(bool recursive = false); /// @brief Updates the state of the thing /// @param currentTimeMs The current clock time in milliseconds; if this is /// zero, the current time is retrieved automatically /// @param recurse When true, this will Update the descendants recursively - virtual void Update(unsigned long currentTimeMs, bool recurse = false); + virtual void Update(bool recurse = false); - static void UpdateThings(unsigned long currentTimeMs); + static void UpdateThings(); #pragma endregion Update diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index f8fc0ca..30b3c0b 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -2,15 +2,24 @@ namespace RoboidControl { -DifferentialDrive::DifferentialDrive(Participant* participant, - unsigned char thingId) - : Thing(participant, Thing::Type::DifferentialDrive, thingId) { +// DifferentialDrive::DifferentialDrive(Participant* participant, +// unsigned char thingId) +// : Thing(participant, Thing::Type::DifferentialDrive, thingId) { +// this->leftWheel = new Motor(); +// this->rightWheel = new Motor(); +// } + +// DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) +// : Thing(parent, Thing::Type::DifferentialDrive, thingId) {} + +DifferentialDrive::DifferentialDrive(Participant* owner) : Thing(owner, Type::DifferentialDrive) { + this->leftWheel = new Motor(); + this->rightWheel = new Motor(); +} +DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent, Type::DifferentialDrive) { this->leftWheel = new Motor(); this->rightWheel = new Motor(); } - -DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) - : Thing(parent, Thing::Type::DifferentialDrive, thingId) {} void DifferentialDrive::SetDriveDimensions(float wheelDiameter, float wheelSeparation) { @@ -47,7 +56,7 @@ void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) { Spherical(speedRight, Direction::right)); } -void DifferentialDrive::Update(unsigned long currentMs, bool recursive) { +void DifferentialDrive::Update(bool recursive) { if (this->linearVelocityUpdated) { // this assumes forward velocity only.... float linearVelocity = this->GetLinearVelocity().distance; @@ -68,7 +77,7 @@ void DifferentialDrive::Update(unsigned long currentMs, bool recursive) { this->SetWheelVelocity(speedLeft, speedRight); } - Thing::Update(currentMs, recursive); + Thing::Update(recursive); } } // namespace RoboidControl \ No newline at end of file diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index f352611..2647a12 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -16,13 +16,13 @@ class DifferentialDrive : public Thing { /// @param participant The local participant /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DifferentialDrive(Participant* participant = nullptr, - unsigned char thingId = 0); + DifferentialDrive(Participant* participant = nullptr); //, + //unsigned char thingId = 0); /// @brief Create a new child differential drive /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DifferentialDrive(Thing* parent, unsigned char thingId = 0); + DifferentialDrive(Thing* parent); //, unsigned char thingId = 0); /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters @@ -45,7 +45,7 @@ class DifferentialDrive : public Thing { void SetWheelVelocity(float speedLeft, float speedRight); /// @copydoc RoboidControl::Thing::Update(unsigned long) - virtual void Update(unsigned long currentMs, bool recursive = true) override; + virtual void Update(bool recursive = true) override; /// @brief The left wheel Motor* leftWheel = nullptr; diff --git a/Things/DigitalSensor.cpp b/Things/DigitalSensor.cpp index 5eae131..08a959e 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -4,11 +4,15 @@ namespace RoboidControl { //DigitalSensor::DigitalSensor() : Thing(Type::Switch) {} -DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId) - : Thing(owner, Type::Switch, thingId) {} +// DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId) +// : Thing(owner, Type::Switch, thingId) {} -DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId) - : Thing(parent, Type::Switch) {} +// DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId) +// : Thing(parent, Type::Switch) {} + +DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {} + +DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {} int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) { bytes[(*ix)++] = state ? 1 : 0; diff --git a/Things/DigitalSensor.h b/Things/DigitalSensor.h index 520ae4c..a9c0571 100644 --- a/Things/DigitalSensor.h +++ b/Things/DigitalSensor.h @@ -13,12 +13,12 @@ class DigitalSensor : public Thing { /// @param owner The owning participant /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DigitalSensor(Participant* owner = nullptr, unsigned char thingId = 0); + DigitalSensor(Participant* owner = nullptr); //, unsigned char thingId = 0); /// @brief Create a new child digital sensor /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DigitalSensor(Thing* parent, unsigned char thingId = 0); + DigitalSensor(Thing* parent); //, unsigned char thingId = 0); /// @brief The sigital state bool state = 0; diff --git a/Things/Motor.cpp b/Things/Motor.cpp index 7d3abd7..96b50a6 100644 --- a/Things/Motor.cpp +++ b/Things/Motor.cpp @@ -1,8 +1,8 @@ #include "Motor.h" -RoboidControl::Motor::Motor(Participant* owner) {} +RoboidControl::Motor::Motor(Participant* owner) : Thing(owner, Type::UncontrolledMotor) {} -RoboidControl::Motor::Motor(Thing* parent) {} +RoboidControl::Motor::Motor(Thing* parent) : Thing(parent, Type::UncontrolledMotor) {} void RoboidControl::Motor::SetTargetSpeed(float targetSpeed) { this->targetSpeed = targetSpeed; diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp new file mode 100644 index 0000000..dd10cdd --- /dev/null +++ b/Things/RelativeEncoder.cpp @@ -0,0 +1,23 @@ +#include "RelativeEncoder.h" + +namespace RoboidControl { + +RelativeEncoder::RelativeEncoder(Participant* owner) + : Thing(owner, Type::IncrementalEncoder) {} +RelativeEncoder::RelativeEncoder(Thing* parent) + : Thing(parent, Type::IncrementalEncoder) {} + +float RelativeEncoder::GetRotationSpeed() { + return rotationSpeed; +} + +int RelativeEncoder::GenerateBinary(char* data, unsigned char* ix) { + data[(*ix)++] = (unsigned char)(this->rotationSpeed * 127); + return 1; +} + +void RelativeEncoder::ProcessBinary(char* data) { + this->rotationSpeed = (float)data[0] / 127; +} + +} // namespace RoboidControl diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h new file mode 100644 index 0000000..fec8c2b --- /dev/null +++ b/Things/RelativeEncoder.h @@ -0,0 +1,38 @@ +#pragma once + +#include "Thing.h" + +namespace RoboidControl { + +/// @brief An Incremental Encoder measures the rotations of an axle using a +/// rotary sensor. Some encoders are able to detect direction, while others can +/// not. +class RelativeEncoder : public Thing { + public: + /// @brief Creates a sensor which measures distance from pulses + /// @param pulsesPerRevolution The number of pulse edges which make a + /// full rotation + /// @param distancePerRevolution The distance a wheel travels per full + /// rotation + RelativeEncoder(Participant* owner); + RelativeEncoder(Thing* parent); + + /// @brief Get the rotation speed + /// @return The speed in revolutions per second + virtual float GetRotationSpeed(); + float rotationSpeed; + + /// @brief Function used to generate binary data for this touch sensor + /// @param buffer The byte array for thw binary data + /// @param ix The starting position for writing the binary data + int GenerateBinary(char* bytes, unsigned char* ix) override; + /// @brief Function used to process binary data received for this touch sensor + /// @param bytes The binary data to process + virtual void ProcessBinary(char* bytes) override; + +protected: + /// @brief rotation speed in revolutions per second + //float _rotationSpeed; +}; + +} // namespace RoboidControl diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index 45718a5..632b3c0 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -4,9 +4,13 @@ namespace RoboidControl { -TemperatureSensor::TemperatureSensor(Participant* participant, - unsigned char thingId) - : Thing(participant, Type::TemperatureSensor, thingId) {} +// TemperatureSensor::TemperatureSensor(Participant* participant, +// unsigned char thingId) +// : Thing(participant, Type::TemperatureSensor, thingId) {} + +TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {} + +TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {} void TemperatureSensor::SetTemperature(float temp) { this->temperature = temp; diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index a447385..629c178 100644 --- a/Things/TemperatureSensor.h +++ b/Things/TemperatureSensor.h @@ -12,7 +12,8 @@ class TemperatureSensor : public Thing { /// @brief Create a temperature sensor with the given ID /// @param networkId The network ID of the sensor /// @param thingId The ID of the thing - TemperatureSensor(Participant* participant, unsigned char thingId); + TemperatureSensor(Participant* participant); //, unsigned char thingId); + TemperatureSensor(Thing* parent); /// @brief The measured temperature float temperature = 0; diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index 809fa9d..7657072 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -2,14 +2,20 @@ namespace RoboidControl { -//TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {} +// TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {} -TouchSensor::TouchSensor(Participant* owner, unsigned char thingId) - : Thing(owner, Thing::Type::TouchSensor, thingId) {} +// TouchSensor::TouchSensor(Participant* owner, unsigned char thingId) +// : Thing(owner, Thing::Type::TouchSensor, thingId) {} -TouchSensor::TouchSensor(Thing* parent, unsigned char thingId) : Thing(parent->owner, Thing::Type::TouchSensor, thingId) { - this->SetParent(parent); -} +// TouchSensor::TouchSensor(Thing* parent, unsigned char thingId) : +// Thing(parent->owner, Thing::Type::TouchSensor, thingId) { +// this->SetParent(parent); +// } + +TouchSensor::TouchSensor(Participant* owner) + : Thing(owner, Type::TouchSensor) {} + +TouchSensor::TouchSensor(Thing* parent) : Thing(parent, Type::TouchSensor) {} int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { bytes[(*ix)++] = touchedSomething ? 1 : 0; diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index c176145..e857f90 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -14,12 +14,12 @@ class TouchSensor : public Thing { /// @param owner The owning participant /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - TouchSensor(Participant* owner = nullptr, unsigned char thingId = 0); + TouchSensor(Participant* owner = nullptr); //, unsigned char thingId = 0); /// @brief Create a new child touch sensor /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - TouchSensor(Thing* parent, unsigned char thingId = 0); + TouchSensor(Thing* parent); //, unsigned char thingId = 0); /// @brief Value which is true when the sensor is touching something, false /// otherwise From 3f8eadf8ccaf39fbdeaca51b60a0320e2feec434 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Sat, 17 May 2025 18:09:38 +0200 Subject: [PATCH 27/56] Added Root & new Thing ref. constructor --- Thing.cpp | 26 +++++++++++++++++++++----- Thing.h | 26 +++++++++++++++----------- 2 files changed, 36 insertions(+), 16 deletions(-) diff --git a/Thing.cpp b/Thing.cpp index bbf82e3..67a704e 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -20,12 +20,28 @@ namespace RoboidControl { #pragma region Init -// Thing::Thing(unsigned char thingType) -// : Thing(IsolatedParticipant::Isolated(), thingType) {} +Thing Root = Thing(Thing::Type::Undetermined, Root); -Thing::Thing(Participant* owner, - unsigned char thingType) { - // unsigned char thingId) { +Thing::Thing(unsigned char thingType, Thing& parent) { + this->type = thingType; + + this->position = Spherical::zero; + this->positionUpdated = true; + this->orientation = SwingTwist::identity; + this->orientationUpdated = true; + this->hierarchyChanged = true; + + this->linearVelocity = Spherical::zero; + this->angularVelocity = Spherical::zero; + + // std::cout << "add thing [" << (int)this->id << "] to owner " + // << this->owner->ipAddress << ":" << this->owner->port << + // std::endl; + this->owner->Add(this, true); +} + +Thing::Thing(Participant* owner, unsigned char thingType) { + // unsigned char thingId) { if (owner == nullptr) owner = IsolatedParticipant::Isolated(); this->owner = owner; diff --git a/Thing.h b/Thing.h index 9045430..8247bc5 100644 --- a/Thing.h +++ b/Thing.h @@ -41,10 +41,9 @@ class Thing { }; #pragma region Init + static Thing Root; // = Thing(Type::Undetermined, Root); - /// @brief Create a new thing without communication abilities - /// @param thingType The type of thing (can use Thing::Type) - //Thing(unsigned char thingType = Type::Undetermined); + Thing(unsigned char thingType = Type::Undetermined, Thing& parent = Root); /// @brief Create a new thing for a participant /// @param owner The owning participant @@ -52,8 +51,8 @@ class Thing { /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID Thing(Participant* owner = nullptr, - unsigned char thingType = Type::Undetermined); - //unsigned char thingId = 0); + unsigned char thingType = Type::Undetermined); + // unsigned char thingId = 0); /// @brief Create a new child thing /// @param parent The parent thing @@ -61,11 +60,14 @@ class Thing { /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID /// @note The owner will be the same as the owner of the parent thing - Thing(Thing* parent, unsigned char thingType = 0); //, unsigned char thingId = 0); + Thing(Thing* parent, + unsigned char thingType = 0); //, unsigned char thingId = 0); - static Thing Reconstruct(Participant* owner, unsigned char thingType, unsigned char thingId); + static Thing Reconstruct(Participant* owner, + unsigned char thingType, + unsigned char thingId); - #pragma endregion Init +#pragma endregion Init public: /// @brief Terminated things are no longer updated @@ -201,9 +203,11 @@ class Thing { /// parent's orientation SwingTwist orientation; - /// @brief The linear velocity of the thing in local space, in meters per second + /// @brief The linear velocity of the thing in local space, in meters per + /// second Spherical linearVelocity; - /// @brief The angular velocity of the thing in local space, in degrees per second + /// @brief The angular velocity of the thing in local space, in degrees per + /// second Spherical angularVelocity; #pragma endregion Pose @@ -216,7 +220,7 @@ class Thing { static unsigned long GetTimeMs(); /// @brief Updates the state of the thing - //void Update(bool recursive = false); + // void Update(bool recursive = false); /// @brief Updates the state of the thing /// @param currentTimeMs The current clock time in milliseconds; if this is From 5b5513971c6690df32f99d56c4b25469df9aeb02 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Sun, 18 May 2025 17:34:52 +0200 Subject: [PATCH 28/56] Reduced the use of raw pointers --- CMakeLists.txt | 6 +++--- LinearAlgebra/Direction.cpp | 6 ++++-- LinearAlgebra/Direction.h | 2 -- LinearAlgebra/Polar.cpp | 4 ++-- LinearAlgebra/Spherical.cpp | 4 ++++ LinearAlgebra/Spherical.h | 1 - LinearAlgebra/SwingTwist.cpp | 6 +++++- LinearAlgebra/test/Direction_test.cc | 2 ++ Participant.cpp | 2 ++ Participant.h | 14 ++++++++------ Posix/PosixParticipant.cpp | 8 ++++---- Posix/PosixParticipant.h | 5 +++++ Thing.cpp | 11 ++++++----- Thing.h | 11 ++++------- Things/DifferentialDrive.cpp | 6 +++++- Things/DifferentialDrive.h | 5 +++-- Things/DigitalSensor.cpp | 3 ++- Things/DigitalSensor.h | 3 ++- Things/Motor.cpp | 14 +++++++++++--- Things/Motor.h | 5 +++-- Things/RelativeEncoder.cpp | 4 ++-- Things/RelativeEncoder.h | 3 ++- Things/TemperatureSensor.cpp | 4 +++- Things/TemperatureSensor.h | 3 ++- Things/TouchSensor.cpp | 3 ++- Things/TouchSensor.h | 5 +++-- examples/BB2B.cpp | 14 +++++++------- test/participant_test.cc | 15 +++++++++------ test/thing_test.cc | 4 ++-- 29 files changed, 107 insertions(+), 66 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 09c1b91..e4a9a1b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,13 +19,13 @@ if(ESP_PLATFORM) REQUIRES esp_netif esp_wifi ) else() + set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + project(RoboidControl) add_subdirectory(LinearAlgebra) add_subdirectory(Examples) - set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard - set(CMAKE_POSITION_INDEPENDENT_CODE ON) - add_compile_definitions(GTEST) include(FetchContent) FetchContent_Declare( diff --git a/LinearAlgebra/Direction.cpp b/LinearAlgebra/Direction.cpp index f9c5e40..86fa77c 100644 --- a/LinearAlgebra/Direction.cpp +++ b/LinearAlgebra/Direction.cpp @@ -9,6 +9,7 @@ #include +namespace LinearAlgebra { template DirectionOf::DirectionOf() { this->horizontal = AngleOf(); @@ -98,5 +99,6 @@ void DirectionOf::Normalize() { } } -template class DirectionOf; -template class DirectionOf; +template class LinearAlgebra::DirectionOf; +template class LinearAlgebra::DirectionOf; +} \ No newline at end of file diff --git a/LinearAlgebra/Direction.h b/LinearAlgebra/Direction.h index 57f6647..8c58b6c 100644 --- a/LinearAlgebra/Direction.h +++ b/LinearAlgebra/Direction.h @@ -99,6 +99,4 @@ using Direction = DirectionSingle; } // namespace LinearAlgebra -using namespace LinearAlgebra; - #endif \ No newline at end of file diff --git a/LinearAlgebra/Polar.cpp b/LinearAlgebra/Polar.cpp index 9dea130..15ac40f 100644 --- a/LinearAlgebra/Polar.cpp +++ b/LinearAlgebra/Polar.cpp @@ -175,5 +175,5 @@ PolarOf PolarOf::Rotate(const PolarOf& v, AngleOf angle) { return r; } -template class PolarOf; -template class PolarOf; \ No newline at end of file +template class LinearAlgebra::PolarOf; +template class LinearAlgebra::PolarOf; \ No newline at end of file diff --git a/LinearAlgebra/Spherical.cpp b/LinearAlgebra/Spherical.cpp index 1a50357..0371f92 100644 --- a/LinearAlgebra/Spherical.cpp +++ b/LinearAlgebra/Spherical.cpp @@ -5,6 +5,8 @@ #include +namespace LinearAlgebra { + template SphericalOf::SphericalOf() { this->distance = 0.0f; @@ -301,3 +303,5 @@ SphericalOf SphericalOf::RotateVertical(const SphericalOf& v, template class SphericalOf; template class SphericalOf; + +} // namespace LinearAlgebra \ No newline at end of file diff --git a/LinearAlgebra/Spherical.h b/LinearAlgebra/Spherical.h index 5cad52c..309db03 100644 --- a/LinearAlgebra/Spherical.h +++ b/LinearAlgebra/Spherical.h @@ -186,7 +186,6 @@ using Spherical = SphericalSingle; #endif } // namespace LinearAlgebra -using namespace LinearAlgebra; #include "Polar.h" #include "Vector3.h" diff --git a/LinearAlgebra/SwingTwist.cpp b/LinearAlgebra/SwingTwist.cpp index 58905c7..deca73c 100644 --- a/LinearAlgebra/SwingTwist.cpp +++ b/LinearAlgebra/SwingTwist.cpp @@ -4,6 +4,8 @@ #include "SwingTwist.h" +namespace LinearAlgebra { + template SwingTwistOf::SwingTwistOf() { this->swing = DirectionOf(AngleOf(), AngleOf()); @@ -165,4 +167,6 @@ void SwingTwistOf::Normalize() { } template class SwingTwistOf; -template class SwingTwistOf; \ No newline at end of file +template class SwingTwistOf; + +} \ No newline at end of file diff --git a/LinearAlgebra/test/Direction_test.cc b/LinearAlgebra/test/Direction_test.cc index 461140c..6489caa 100644 --- a/LinearAlgebra/test/Direction_test.cc +++ b/LinearAlgebra/test/Direction_test.cc @@ -6,6 +6,8 @@ #include "Direction.h" +using namespace LinearAlgebra; + #define FLOAT_INFINITY std::numeric_limits::infinity() TEST(Direction16, Compare) { diff --git a/Participant.cpp b/Participant.cpp index 03197e6..51dfabd 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -8,6 +8,8 @@ namespace RoboidControl { ParticipantRegistry Participant::registry; +Participant LocalParticipant = Participant("0.0.0.0", 0); + Participant::Participant(const char* ipAddress, int port) { // make a copy of the ip address string int addressLength = (int)strlen(ipAddress); diff --git a/Participant.h b/Participant.h index 49ee749..35c5ce4 100644 --- a/Participant.h +++ b/Participant.h @@ -10,7 +10,7 @@ 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 + /// @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 @@ -20,7 +20,7 @@ class ParticipantRegistry { /// @brief Add a participant with the given details /// @param ipAddress The IP address of the participant - /// @param port The port number 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 @@ -33,17 +33,17 @@ class ParticipantRegistry { private: #if defined(NO_STD) -public: + public: Participant** GetAll() const; int count = 0; -private: + private: Participant** participants; #else -public: + public: /// @brief Get all participants /// @return All participants const std::list& GetAll() const; -private: + private: /// @brief The list of known participants std::list participants; #endif @@ -72,6 +72,8 @@ class Participant { /// @brief Destructor for the participant ~Participant(); + static Participant LocalParticipant; + public: #if defined(NO_STD) unsigned char thingCount = 0; diff --git a/Posix/PosixParticipant.cpp b/Posix/PosixParticipant.cpp index 466035a..a01a059 100644 --- a/Posix/PosixParticipant.cpp +++ b/Posix/PosixParticipant.cpp @@ -11,7 +11,7 @@ namespace RoboidControl { namespace Posix { -void Setup(int localPort, const char* remoteIpAddress, int remotePort) { +void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(__unix__) || defined(__APPLE__) // Create a UDP socket @@ -63,7 +63,7 @@ void Setup(int localPort, const char* remoteIpAddress, int remotePort) { #endif } -void Receive() { +void ParticipantUDP::Receive() { #if defined(__unix__) || defined(__APPLE__) sockaddr_in client_addr; socklen_t len = sizeof(client_addr); @@ -90,7 +90,7 @@ void Receive() { #endif } -bool Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { #if defined(__unix__) || defined(__APPLE__) // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port) // << "\n"; @@ -113,7 +113,7 @@ bool Send(Participant* remoteParticipant, int bufferSize) { return true; } -bool Publish(IMessage* msg) { +bool ParticipantUDP::Publish(IMessage* msg) { #if defined(__unix__) || defined(__APPLE__) int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) diff --git a/Posix/PosixParticipant.h b/Posix/PosixParticipant.h index ec16d32..a83569f 100644 --- a/Posix/PosixParticipant.h +++ b/Posix/PosixParticipant.h @@ -11,6 +11,11 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { void Receive(); bool Send(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); + + protected: + sockaddr_in remote_addr; + sockaddr_in server_addr; + sockaddr_in broadcast_addr; }; } // namespace Posix diff --git a/Thing.cpp b/Thing.cpp index 67a704e..3f8168c 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -20,7 +20,7 @@ namespace RoboidControl { #pragma region Init -Thing Root = Thing(Thing::Type::Undetermined, Root); +Thing Thing::Root = Thing(Thing::Type::Undetermined, Root); Thing::Thing(unsigned char thingType, Thing& parent) { this->type = thingType; @@ -38,6 +38,7 @@ Thing::Thing(unsigned char thingType, Thing& parent) { // << this->owner->ipAddress << ":" << this->owner->port << // std::endl; this->owner->Add(this, true); + this->SetParent(&parent); } Thing::Thing(Participant* owner, unsigned char thingType) { @@ -63,10 +64,10 @@ Thing::Thing(Participant* owner, unsigned char thingType) { this->owner->Add(this, true); } -Thing::Thing(Thing* parent, unsigned char thingType) //, unsigned char thingId) - : Thing(parent->owner, thingType) { //}, thingId) { - this->SetParent(parent); -} +// Thing::Thing(Thing* parent, unsigned char thingType) //, unsigned char thingId) +// : Thing(parent->owner, thingType) { //}, thingId) { +// this->SetParent(parent); +// } Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, unsigned char thingId) { Thing thing = Thing(owner, thingType); diff --git a/Thing.h b/Thing.h index 8247bc5..cc97ab8 100644 --- a/Thing.h +++ b/Thing.h @@ -41,18 +41,17 @@ class Thing { }; #pragma region Init - static Thing Root; // = Thing(Type::Undetermined, Root); + static Thing Root; - Thing(unsigned char thingType = Type::Undetermined, Thing& parent = Root); + Thing(unsigned char thingType = Thing::Type::Undetermined, Thing& parent = Root); /// @brief Create a new thing for a participant /// @param owner The owning participant /// @param thingType The type of thing (can use Thing::Type) /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - Thing(Participant* owner = nullptr, - unsigned char thingType = Type::Undetermined); - // unsigned char thingId = 0); + Thing(Participant* owner, + unsigned char thingType = Thing::Type::Undetermined); /// @brief Create a new child thing /// @param parent The parent thing @@ -60,8 +59,6 @@ class Thing { /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID /// @note The owner will be the same as the owner of the parent thing - Thing(Thing* parent, - unsigned char thingType = 0); //, unsigned char thingId = 0); static Thing Reconstruct(Participant* owner, unsigned char thingType, diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index 30b3c0b..29c9e66 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -16,7 +16,11 @@ DifferentialDrive::DifferentialDrive(Participant* owner) : Thing(owner, Type::Di this->leftWheel = new Motor(); this->rightWheel = new Motor(); } -DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent, Type::DifferentialDrive) { +// DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent, Type::DifferentialDrive) { +// this->leftWheel = new Motor(); +// this->rightWheel = new Motor(); +// } +DifferentialDrive::DifferentialDrive(Thing& parent) : Thing(Type::DifferentialDrive, parent) { this->leftWheel = new Motor(); this->rightWheel = new Motor(); } diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index 2647a12..65cd5a7 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -16,13 +16,14 @@ class DifferentialDrive : public Thing { /// @param participant The local participant /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DifferentialDrive(Participant* participant = nullptr); //, + DifferentialDrive(Participant* participant); //, //unsigned char thingId = 0); /// @brief Create a new child differential drive /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DifferentialDrive(Thing* parent); //, unsigned char thingId = 0); + // DifferentialDrive(Thing* parent); //, unsigned char thingId = 0); + DifferentialDrive(Thing& parent = Thing::Root); /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters diff --git a/Things/DigitalSensor.cpp b/Things/DigitalSensor.cpp index 08a959e..0ac2414 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -12,7 +12,8 @@ namespace RoboidControl { DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {} -DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {} +// DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {} +DigitalSensor::DigitalSensor(Thing& parent) : Thing(Type::Switch, parent) {} int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) { bytes[(*ix)++] = state ? 1 : 0; diff --git a/Things/DigitalSensor.h b/Things/DigitalSensor.h index a9c0571..9b77f6b 100644 --- a/Things/DigitalSensor.h +++ b/Things/DigitalSensor.h @@ -18,7 +18,8 @@ class DigitalSensor : public Thing { /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DigitalSensor(Thing* parent); //, unsigned char thingId = 0); + // DigitalSensor(Thing* parent); //, unsigned char thingId = 0); + DigitalSensor(Thing& parent = Thing::Root); /// @brief The sigital state bool state = 0; diff --git a/Things/Motor.cpp b/Things/Motor.cpp index 96b50a6..f9796b1 100644 --- a/Things/Motor.cpp +++ b/Things/Motor.cpp @@ -1,9 +1,17 @@ #include "Motor.h" -RoboidControl::Motor::Motor(Participant* owner) : Thing(owner, Type::UncontrolledMotor) {} +namespace RoboidControl { -RoboidControl::Motor::Motor(Thing* parent) : Thing(parent, Type::UncontrolledMotor) {} +RoboidControl::Motor::Motor(Participant* owner) + : Thing(owner, Type::UncontrolledMotor) {} + +// RoboidControl::Motor::Motor(Thing* parent) +// : Thing(parent, Type::UncontrolledMotor) {} + +Motor::Motor(Thing& parent) : Thing(Type::UncontrolledMotor, parent) {} void RoboidControl::Motor::SetTargetSpeed(float targetSpeed) { - this->targetSpeed = targetSpeed; + this->targetSpeed = targetSpeed; } + +} // namespace RoboidControl \ No newline at end of file diff --git a/Things/Motor.h b/Things/Motor.h index 1ff6716..d10d15b 100644 --- a/Things/Motor.h +++ b/Things/Motor.h @@ -6,8 +6,9 @@ namespace RoboidControl { class Motor : public Thing { public: - Motor(Participant* owner = nullptr); - Motor(Thing* parent); + Motor(Participant* owner); + // Motor(Thing* parent); + Motor(Thing& parent = Thing::Root); /// @brief Motor turning direction enum class Direction { Clockwise = 1, CounterClockwise = -1 }; diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp index dd10cdd..e13891f 100644 --- a/Things/RelativeEncoder.cpp +++ b/Things/RelativeEncoder.cpp @@ -4,8 +4,8 @@ namespace RoboidControl { RelativeEncoder::RelativeEncoder(Participant* owner) : Thing(owner, Type::IncrementalEncoder) {} -RelativeEncoder::RelativeEncoder(Thing* parent) - : Thing(parent, Type::IncrementalEncoder) {} +// RelativeEncoder::RelativeEncoder(Thing* parent) +// : Thing(parent, Type::IncrementalEncoder) {} float RelativeEncoder::GetRotationSpeed() { return rotationSpeed; diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h index fec8c2b..573e7c5 100644 --- a/Things/RelativeEncoder.h +++ b/Things/RelativeEncoder.h @@ -15,7 +15,8 @@ class RelativeEncoder : public Thing { /// @param distancePerRevolution The distance a wheel travels per full /// rotation RelativeEncoder(Participant* owner); - RelativeEncoder(Thing* parent); + // RelativeEncoder(Thing* parent); + RelativeEncoder(Thing& parent); /// @brief Get the rotation speed /// @return The speed in revolutions per second diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index 632b3c0..d9c9113 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -10,7 +10,9 @@ namespace RoboidControl { TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {} -TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {} +TemperatureSensor::TemperatureSensor(Thing& parent) : Thing(Type::TemperatureSensor, parent) {} + +// TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {} void TemperatureSensor::SetTemperature(float temp) { this->temperature = temp; diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index 629c178..23f56fa 100644 --- a/Things/TemperatureSensor.h +++ b/Things/TemperatureSensor.h @@ -13,7 +13,8 @@ class TemperatureSensor : public Thing { /// @param networkId The network ID of the sensor /// @param thingId The ID of the thing TemperatureSensor(Participant* participant); //, unsigned char thingId); - TemperatureSensor(Thing* parent); + // TemperatureSensor(Thing* parent); + TemperatureSensor(Thing& parent = Thing::Root); /// @brief The measured temperature float temperature = 0; diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index 7657072..d3d2a91 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -15,7 +15,8 @@ namespace RoboidControl { TouchSensor::TouchSensor(Participant* owner) : Thing(owner, Type::TouchSensor) {} -TouchSensor::TouchSensor(Thing* parent) : Thing(parent, Type::TouchSensor) {} +// TouchSensor::TouchSensor(Thing* parent) : Thing(parent, Type::TouchSensor) {} +TouchSensor::TouchSensor(Thing& parent) : Thing(Type::TouchSensor, parent) {} int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { bytes[(*ix)++] = touchedSomething ? 1 : 0; diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index e857f90..aca28a4 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -14,12 +14,13 @@ class TouchSensor : public Thing { /// @param owner The owning participant /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - TouchSensor(Participant* owner = nullptr); //, unsigned char thingId = 0); + TouchSensor(Participant* owner); //, unsigned char thingId = 0); /// @brief Create a new child touch sensor /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - TouchSensor(Thing* parent); //, unsigned char thingId = 0); + // TouchSensor(Thing* parent); //, unsigned char thingId = 0); + TouchSensor(Thing& parent = Thing::Root); /// @brief Value which is true when the sensor is touching something, false /// otherwise diff --git a/examples/BB2B.cpp b/examples/BB2B.cpp index 1dad561..a07aa9f 100644 --- a/examples/BB2B.cpp +++ b/examples/BB2B.cpp @@ -17,26 +17,26 @@ using namespace RoboidControl; int main() { // The robot's propulsion is a differential drive - DifferentialDrive* bb2b = new DifferentialDrive(); + DifferentialDrive bb2b = DifferentialDrive(); // Is has a touch sensor at the front left of the roboid - TouchSensor* touchLeft = new TouchSensor(bb2b); + TouchSensor touchLeft = TouchSensor(bb2b); // and other one on the right - TouchSensor* touchRight = new TouchSensor(bb2b); + TouchSensor touchRight = TouchSensor(bb2b); // Do forever: while (true) { // The left wheel turns forward when nothing is touched on the right side // and turn backward when the roboid hits something on the right - float leftWheelSpeed = (touchRight->touchedSomething) ? -600.0f : 600.0f; + float leftWheelSpeed = (touchRight.touchedSomething) ? -600.0f : 600.0f; // The right wheel does the same, but instead is controlled by // touches on the left side - float rightWheelSpeed = (touchLeft->touchedSomething) ? -600.0f : 600.0f; + float rightWheelSpeed = (touchLeft.touchedSomething) ? -600.0f : 600.0f; // When both sides are touching something, both wheels will turn backward // and the roboid will move backwards - bb2b->SetWheelVelocity(leftWheelSpeed, rightWheelSpeed); + bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed); // Update the roboid state - bb2b->Update(true); + bb2b.Update(true); // and sleep for 100ms #if defined(ARDUINO) diff --git a/test/participant_test.cc b/test/participant_test.cc index 42c686b..0b4e147 100644 --- a/test/participant_test.cc +++ b/test/participant_test.cc @@ -7,6 +7,9 @@ #include "Participants/SiteServer.h" #include "Thing.h" +#include +#include + using namespace RoboidControl; TEST(Participant, Participant) { @@ -15,7 +18,7 @@ TEST(Participant, Participant) { unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds; while (milliseconds < startTime + 7000) { - participant->Update(milliseconds); + participant->Update(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); milliseconds = Thing::GetTimeMs(); @@ -29,7 +32,7 @@ TEST(Participant, SiteServer) { unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds; while (milliseconds < startTime + 7000) { - siteServer->Update(milliseconds); + siteServer->Update(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); milliseconds = Thing::GetTimeMs(); @@ -44,8 +47,8 @@ TEST(Participant, SiteParticipant) { unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds; while (milliseconds < startTime + 7000) { - siteServer->Update(milliseconds); - participant->Update(milliseconds); + siteServer->Update(); + participant->Update(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); milliseconds = Thing::GetTimeMs(); @@ -63,8 +66,8 @@ TEST(Participant, ThingMsg) { unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds; while (milliseconds < startTime + 7000) { - siteServer->Update(milliseconds); - participant->Update(milliseconds); + siteServer->Update(); + participant->Update(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); milliseconds = Thing::GetTimeMs(); diff --git a/test/thing_test.cc b/test/thing_test.cc index b8ceea1..74ff48c 100644 --- a/test/thing_test.cc +++ b/test/thing_test.cc @@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) { unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds; while (milliseconds < startTime + 1000) { - Thing::UpdateThings(milliseconds); + Thing::UpdateThings(); milliseconds = Thing::GetTimeMs(); } @@ -29,7 +29,7 @@ TEST(RoboidControlSuite, IsolatedParticipant) { unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds; while (milliseconds < startTime + 1000) { - participant->Update(milliseconds); + participant->Update(); milliseconds = Thing::GetTimeMs(); } From da8831a9685c7b0c14968f21beb3c0ac058796a9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 19 May 2025 08:55:00 +0200 Subject: [PATCH 29/56] Added controlledMotor, ptr to ref for hierarchy --- Arduino/Things/DRV8833.cpp | 10 ++++-- Arduino/Things/DRV8833.h | 4 +-- Arduino/Things/DigitalInput.cpp | 21 +++++++----- Arduino/Things/DigitalInput.h | 7 ++-- Arduino/Things/UltrasonicSensor.cpp | 22 +++++++----- Arduino/Things/UltrasonicSensor.h | 4 +-- Posix/PosixParticipant.h | 2 ++ Things/ControlledMotor.cpp | 50 +++++++++++++++++++++++++++ Things/ControlledMotor.h | 52 +++++++++++++++++++++++++++++ Things/DifferentialDrive.cpp | 12 ++++--- Things/Motor.cpp | 4 +-- Things/Motor.h | 4 +-- Things/RelativeEncoder.cpp | 4 +-- Things/RelativeEncoder.h | 2 +- 14 files changed, 160 insertions(+), 38 deletions(-) create mode 100644 Things/ControlledMotor.cpp create mode 100644 Things/ControlledMotor.h diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 80fc402..373bce1 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -5,6 +5,8 @@ namespace RoboidControl { namespace Arduino { +#pragma region DRV8833 + DRV8833::DRV8833(Configuration config, Participant* participant) : Thing(participant) { this->pinStandby = pinStandby; @@ -22,6 +24,8 @@ DRV8833::DRV8833(Configuration config, Thing* parent) this->SetParent(parent); } +#pragma endregion DRV8833 + #pragma region Differential drive DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, @@ -32,7 +36,7 @@ DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, } DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, - Thing* parent) + Thing& parent) : RoboidControl::DifferentialDrive(parent), drv8833(config) {} void DRV8833::DifferentialDrive::Update(bool recurse) { @@ -79,8 +83,8 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver, // this->maxRpm = rpm; // } -void DRV8833Motor::SetTargetSpeed(float motorSpeed) { - Motor::SetTargetSpeed(motorSpeed); +void DRV8833Motor::SetTargetVelocity(float motorSpeed) { + Motor::SetTargetVelocity(motorSpeed); uint8_t motorSignal = (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255); diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index 2da2664..c13a333 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -40,7 +40,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { public: DifferentialDrive(DRV8833::Configuration config, Participant* participant = nullptr); - DifferentialDrive(DRV8833::Configuration config, Thing* parent); + DifferentialDrive(DRV8833::Configuration config, Thing& parent); virtual void Update(bool recurse = false) override; @@ -66,7 +66,7 @@ class DRV8833Motor : public Motor { // void SetMaxRPM(unsigned int rpm); // virtual void SetAngularVelocity(Spherical velocity) override; - virtual void SetTargetSpeed(float targetSpeed) override; + virtual void SetTargetVelocity(float targetSpeed) override; // bool reverse = false; protected: diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index 5c3a2c2..64486cc 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -14,12 +14,17 @@ DigitalInput::DigitalInput(Participant* participant, unsigned char pin) pinMode(this->pin, INPUT); } -DigitalInput::DigitalInput(Thing* parent, unsigned char pin) : Thing(parent) { +DigitalInput::DigitalInput(unsigned char pin, Thing& parent) : Thing(parent) { this->pin = pin; - pinMode(this->pin, INPUT); } +// DigitalInput::DigitalInput(Thing* parent, unsigned char pin) : Thing(parent) { +// this->pin = pin; + +// pinMode(this->pin, INPUT); +// } + void DigitalInput::Update(bool recursive) { this->isHigh = digitalRead(this->pin); this->isLow = !this->isHigh; @@ -31,8 +36,8 @@ void DigitalInput::Update(bool recursive) { #pragma region Touch sensor -DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent) - : RoboidControl::TouchSensor(parent), digitalInput(parent, pin) {} +DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing& parent) + : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {} void DigitalInput::TouchSensor::Update(bool recursive) { this->touchedSomething = digitalInput.isLow; @@ -47,9 +52,9 @@ volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0; volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0; DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, - Thing* parent) + Thing& parent) : RoboidControl::RelativeEncoder(parent), - digitalInput(parent, config.pin), + digitalInput(config.pin, parent), pulsesPerRevolution(config.pulsesPerRevolution) { // We support up to 2 pulse counters @@ -94,8 +99,8 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) { this->pulseFrequency = pulseCount / timeStep; this->rotationSpeed = pulseFrequency / pulsesPerRevolution; - // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency - // << " timestep " << timeStep << std::endl; + std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency + << " timestep " << timeStep << std::endl; this->lastUpdateTime = currentTimeMs; } diff --git a/Arduino/Things/DigitalInput.h b/Arduino/Things/DigitalInput.h index 966cfa8..ef3d25c 100644 --- a/Arduino/Things/DigitalInput.h +++ b/Arduino/Things/DigitalInput.h @@ -13,7 +13,8 @@ class DigitalInput : public Thing { /// @param participant The participant to use /// @param pin The digital pin DigitalInput(Participant* participant, unsigned char pin); - DigitalInput(Thing* parent, unsigned char pin); + //DigitalInput(Thing* parent, unsigned char pin); + DigitalInput(unsigned char pin, Thing& parent); bool isHigh = false; bool isLow = false; @@ -34,7 +35,7 @@ class DigitalInput : public Thing { class DigitalInput::TouchSensor : public RoboidControl::TouchSensor { public: - TouchSensor(unsigned char pin, Thing* parent); + TouchSensor(unsigned char pin, Thing& parent); /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) virtual void Update(bool recurse = false) override; @@ -54,7 +55,7 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder { unsigned char pulsesPerRevolution; }; - RelativeEncoder(Configuration config, Thing* parent); + RelativeEncoder(Configuration config, Thing& parent); unsigned char pulsesPerRevolution; diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 95d0bb6..7abd9a8 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -16,9 +16,13 @@ UltrasonicSensor::UltrasonicSensor(Configuration config, pinMode(pinEcho, INPUT); // configure the echo pin to input mode } -UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) - : UltrasonicSensor(config, parent->owner) { - this->SetParent(parent); +UltrasonicSensor::UltrasonicSensor(Configuration config, Thing& parent) + : Thing(parent) { + this->pinTrigger = config.triggerPin; + this->pinEcho = config.echoPin; + + pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode + pinMode(pinEcho, INPUT); // configure the echo pin to input mode } float UltrasonicSensor::GetDistance() { @@ -63,15 +67,15 @@ void UltrasonicSensor::Update(bool recursive) { #pragma region Touch sensor +// UltrasonicSensor::TouchSensor::TouchSensor( +// UltrasonicSensor::Configuration config, +// Thing& parent) +// : RoboidControl::TouchSensor(parent), ultrasonic(config, parent) { +// this->touchedSomething = false; +// } UltrasonicSensor::TouchSensor::TouchSensor( UltrasonicSensor::Configuration config, Thing& parent) - : RoboidControl::TouchSensor(&parent), ultrasonic(config, &parent) { - this->touchedSomething = false; -} -UltrasonicSensor::TouchSensor::TouchSensor( - UltrasonicSensor::Configuration config, - Thing* parent) : RoboidControl::TouchSensor(parent), ultrasonic(config, parent) { this->touchedSomething = false; } diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index 563594e..0ae252e 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -18,7 +18,7 @@ class UltrasonicSensor : Thing { /// @param pinTrigger The pin number of the trigger signal /// @param pinEcho The pin number of the echo signal UltrasonicSensor(Configuration config, Participant* participant); - UltrasonicSensor(Configuration config, Thing* parent); + UltrasonicSensor(Configuration config, Thing& parent); // parameters @@ -52,7 +52,7 @@ class UltrasonicSensor : Thing { class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { public: TouchSensor(UltrasonicSensor::Configuration config, Thing& parent); - TouchSensor(UltrasonicSensor::Configuration config, Thing* parent); + //TouchSensor(UltrasonicSensor::Configuration config, Thing* parent); float touchDistance = 0.2f; diff --git a/Posix/PosixParticipant.h b/Posix/PosixParticipant.h index a83569f..98302f3 100644 --- a/Posix/PosixParticipant.h +++ b/Posix/PosixParticipant.h @@ -13,9 +13,11 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { bool Publish(IMessage* msg); protected: +#if defined(__unix__) || defined(__APPLE__) sockaddr_in remote_addr; sockaddr_in server_addr; sockaddr_in broadcast_addr; +#endif }; } // namespace Posix diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp new file mode 100644 index 0000000..bff3b80 --- /dev/null +++ b/Things/ControlledMotor.cpp @@ -0,0 +1,50 @@ +#include "ControlledMotor.h" + +namespace RoboidControl { + +ControlledMotor::ControlledMotor(Motor* motor, RelativeEncoder* encoder) + : Motor(), motor(motor), encoder(encoder) { + this->type = Type::ControlledMotor; + Thing* parent = motor->GetParent(); + this->SetParent(parent); +} + +void ControlledMotor::SetTargetVelocity(float velocity) { + this->targetVelocity = velocity; + this->rotationDirection = + (targetVelocity < 0) ? Direction::Reverse : Direction::Forward; +} + +void ControlledMotor::Update(bool recurse) { + encoder->Update(recurse); + + this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed; + + unsigned long currentTimeMs = GetTimeMs(); + float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f; + this->lastUpdateTime = currentTimeMs; + + float error = this->targetVelocity - this->actualVelocity; + float acceleration = + error * timeStep * pidP; // Just P is used at this moment + motor->SetTargetVelocity(targetVelocity + + acceleration); // or something like that + + motor->Update(recurse); +} + +// float ControlledMotor::GetActualVelocity() { +// return (int)rotationDirection * encoder->rotationSpeed; +// } + +// bool ControlledMotor::Drive(float distance) { +// if (!driving) { +// targetDistance = distance; +// startDistance = encoder->GetDistance(); +// driving = true; +// } +// float totalDistance = encoder->GetDistance() - startDistance; +// bool completed = totalDistance > targetDistance; +// return completed; +// } +} // namespace RoboidControl \ No newline at end of file diff --git a/Things/ControlledMotor.h b/Things/ControlledMotor.h new file mode 100644 index 0000000..1742289 --- /dev/null +++ b/Things/ControlledMotor.h @@ -0,0 +1,52 @@ +#pragma once + +#include "Motor.h" +#include "RelativeEncoder.h" + +namespace RoboidControl { + +/// @brief A motor with speed control +/// It uses a feedback loop from an encoder to regulate the speed +/// The speed is measured in revolutions per second. +class ControlledMotor : public Motor { + public: + ControlledMotor(Motor* motor, RelativeEncoder* encoder); + + float pidP = 1; + float pidD = 0; + float pidI = 0; + + float actualVelocity; + + enum Direction { Forward = 1, Reverse = -1 }; + Direction rotationDirection; + + virtual void Update(bool recurse = false) override; + + /// @brief Set the target verlocity for the motor controller + /// @param speed the target velocity in revolutions per second. + virtual void SetTargetVelocity(float velocity) override; + + /// @brief Get the actual velocity from the encoder + /// @return The velocity in revolutions per second + // float GetActualVelocity(); + + // bool Drive(float distance); + + Motor* motor; + RelativeEncoder* encoder; + + protected: + float lastUpdateTime; + + + //float targetVelocity; + +// float netDistance = 0; +// float startDistance = 0; +// bool driving = false; +// float targetDistance = 0; +// float lastEncoderPosition = 0; +}; + +} // namespace RoboidControl diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index 29c9e66..0b472a7 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -52,12 +52,16 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter, // this->rightWheel->SetPosition(Spherical(distance, Direction::right)); // } -void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) { +void DifferentialDrive::SetWheelVelocity(float velocityLeft, float velocityRight) { + // if (this->leftWheel != nullptr) + // this->leftWheel->SetAngularVelocity(Spherical(velocityLeft, Direction::left)); + // if (this->rightWheel != nullptr) + // this->rightWheel->SetAngularVelocity( + // Spherical(velocityRight, Direction::right)); if (this->leftWheel != nullptr) - this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left)); + this->leftWheel->SetTargetVelocity(velocityLeft); if (this->rightWheel != nullptr) - this->rightWheel->SetAngularVelocity( - Spherical(speedRight, Direction::right)); + this->rightWheel->SetTargetVelocity(velocityRight); } void DifferentialDrive::Update(bool recursive) { diff --git a/Things/Motor.cpp b/Things/Motor.cpp index f9796b1..bbcd532 100644 --- a/Things/Motor.cpp +++ b/Things/Motor.cpp @@ -10,8 +10,8 @@ RoboidControl::Motor::Motor(Participant* owner) Motor::Motor(Thing& parent) : Thing(Type::UncontrolledMotor, parent) {} -void RoboidControl::Motor::SetTargetSpeed(float targetSpeed) { - this->targetSpeed = targetSpeed; +void RoboidControl::Motor::SetTargetVelocity(float targetSpeed) { + this->targetVelocity = targetSpeed; } } // namespace RoboidControl \ No newline at end of file diff --git a/Things/Motor.h b/Things/Motor.h index d10d15b..530941b 100644 --- a/Things/Motor.h +++ b/Things/Motor.h @@ -15,10 +15,10 @@ class Motor : public Thing { /// @brief The forward turning direction of the motor Direction direction; - virtual void SetTargetSpeed(float targetSpeed); // -1..0..1 + virtual void SetTargetVelocity(float velocity); // -1..0..1 protected: - float targetSpeed = 0; + float targetVelocity = 0; }; } // namespace RoboidControl \ No newline at end of file diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp index e13891f..b7f8a8c 100644 --- a/Things/RelativeEncoder.cpp +++ b/Things/RelativeEncoder.cpp @@ -4,8 +4,8 @@ namespace RoboidControl { RelativeEncoder::RelativeEncoder(Participant* owner) : Thing(owner, Type::IncrementalEncoder) {} -// RelativeEncoder::RelativeEncoder(Thing* parent) -// : Thing(parent, Type::IncrementalEncoder) {} +RelativeEncoder::RelativeEncoder(Thing& parent) + : Thing(Type::IncrementalEncoder, parent) {} float RelativeEncoder::GetRotationSpeed() { return rotationSpeed; diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h index 573e7c5..ff951b8 100644 --- a/Things/RelativeEncoder.h +++ b/Things/RelativeEncoder.h @@ -16,7 +16,7 @@ class RelativeEncoder : public Thing { /// rotation RelativeEncoder(Participant* owner); // RelativeEncoder(Thing* parent); - RelativeEncoder(Thing& parent); + RelativeEncoder(Thing& parent = Thing::Root); /// @brief Get the rotation speed /// @return The speed in revolutions per second From 1b5fef15e6d31446813e2fcc66c4a62a2cfb8dba Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 19 May 2025 15:58:35 +0200 Subject: [PATCH 30/56] More refactoring to get rid of pointers --- Arduino/Things/DRV8833.cpp | 32 ++++------ Arduino/Things/DRV8833.h | 15 ++--- Arduino/Things/DigitalInput.cpp | 30 ++++----- Arduino/Things/DigitalInput.h | 7 ++- Arduino/Things/UltrasonicSensor.cpp | 23 +------ Arduino/Things/UltrasonicSensor.h | 11 +--- Messages/PoseMsg.cpp | 4 +- Messages/ThingMsg.cpp | 14 +++-- Participant.cpp | 11 +++- Participant.h | 2 +- Participants/SiteServer.cpp | 13 ++-- Thing.cpp | 95 +++++++++++++---------------- Thing.h | 25 ++++---- Things/ControlledMotor.cpp | 22 ++++--- Things/ControlledMotor.h | 24 ++------ Things/DifferentialDrive.cpp | 52 +++++++--------- Things/DifferentialDrive.h | 25 +++----- Things/DigitalSensor.cpp | 2 +- Things/DigitalSensor.h | 2 +- Things/Motor.cpp | 4 +- Things/Motor.h | 2 +- Things/RelativeEncoder.cpp | 2 - Things/RelativeEncoder.h | 2 +- Things/TemperatureSensor.cpp | 2 +- Things/TemperatureSensor.h | 2 +- Things/TouchSensor.cpp | 14 ----- Things/TouchSensor.h | 10 +-- 27 files changed, 181 insertions(+), 266 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 373bce1..e95d94a 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -7,41 +7,31 @@ namespace Arduino { #pragma region DRV8833 -DRV8833::DRV8833(Configuration config, Participant* participant) - : Thing(participant) { - this->pinStandby = pinStandby; +DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) { + this->pinStandby = config.standby; if (pinStandby != 255) pinMode(pinStandby, OUTPUT); - this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2); + this->motorA = new DRV8833Motor(*this, config.AIn1, config.AIn2); this->motorA->SetName("Motor A"); - this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2); + this->motorB = new DRV8833Motor(*this, config.BIn1, config.BIn2); this->motorB->SetName("Motor B"); } -DRV8833::DRV8833(Configuration config, Thing* parent) - : DRV8833(config, parent->owner) { - this->SetParent(parent); -} - #pragma endregion DRV8833 #pragma region Differential drive -DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, - Participant* owner) - : RoboidControl::DifferentialDrive(owner), drv8833(config, owner) { - this->leftWheel = this->drv8833.motorA; - this->rightWheel = this->drv8833.motorB; -} - DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, Thing& parent) - : RoboidControl::DifferentialDrive(parent), drv8833(config) {} + : RoboidControl::DifferentialDrive(parent), drv8833(config) { + this->leftWheel = this->drv8833.motorA; + this->rightWheel = this->drv8833.motorB; +} void DRV8833::DifferentialDrive::Update(bool recurse) { RoboidControl::DifferentialDrive::Update(recurse); - this->drv8833.Update(recurse); + this->drv8833.Update(false); } #pragma endregion Differential drive @@ -52,11 +42,11 @@ void DRV8833::DifferentialDrive::Update(bool recurse) { uint8_t DRV8833Motor::nextAvailablePwmChannel = 0; #endif -DRV8833Motor::DRV8833Motor(DRV8833* driver, +DRV8833Motor::DRV8833Motor(DRV8833& driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse) - : Motor(driver->owner) { + : Motor() { this->SetParent(driver); this->pinIn1 = pinIn1; diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index c13a333..1f87b85 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -18,11 +18,12 @@ class DRV8833 : public Thing { int AIn2; int BIn1; int BIn2; + int standby = 255; }; /// @brief Setup a DRV8833 motor controller - DRV8833(Configuration config, Participant* owner = nullptr); - DRV8833(Configuration config, Thing* parent); + // DRV8833(Configuration config, Participant* owner = nullptr); + DRV8833(Configuration config, Thing& parent = Thing::LocalRoot()); DRV8833Motor* motorA = nullptr; DRV8833Motor* motorB = nullptr; @@ -38,9 +39,9 @@ class DRV8833 : public Thing { class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { public: - DifferentialDrive(DRV8833::Configuration config, - Participant* participant = nullptr); - DifferentialDrive(DRV8833::Configuration config, Thing& parent); + // DifferentialDrive(DRV8833::Configuration config, + // Participant* participant = nullptr); + DifferentialDrive(DRV8833::Configuration config, Thing& parent = Thing::LocalRoot()); virtual void Update(bool recurse = false) override; @@ -59,7 +60,7 @@ class DRV8833Motor : public Motor { /// @param pinIn1 the pin number for the in1 signal /// @param pinIn2 the pin number for the in2 signal /// @param direction the forward turning direction of the motor - DRV8833Motor(DRV8833* driver, + DRV8833Motor(DRV8833& driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false); @@ -81,7 +82,7 @@ class DRV8833Motor : public Motor { #endif }; -#pragma endregion Moto +#pragma endregion Motor } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index 64486cc..e6435db 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -7,28 +7,15 @@ namespace Arduino { #pragma region Digital input -DigitalInput::DigitalInput(Participant* participant, unsigned char pin) - : Thing(participant) { - this->pin = pin; - - pinMode(this->pin, INPUT); -} - DigitalInput::DigitalInput(unsigned char pin, Thing& parent) : Thing(parent) { this->pin = pin; pinMode(this->pin, INPUT); + std::cout << "digital input start\n"; } -// DigitalInput::DigitalInput(Thing* parent, unsigned char pin) : Thing(parent) { -// this->pin = pin; - -// pinMode(this->pin, INPUT); -// } - void DigitalInput::Update(bool recursive) { this->isHigh = digitalRead(this->pin); this->isLow = !this->isHigh; - // std::cout << "DigitalINput pin " << (int)this->pin << ": " << Thing::Update(recursive); } @@ -55,13 +42,16 @@ DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, Thing& parent) : RoboidControl::RelativeEncoder(parent), digitalInput(config.pin, parent), - pulsesPerRevolution(config.pulsesPerRevolution) { + pulsesPerRevolution(config.pulsesPerRevolution) {} +void DigitalInput::RelativeEncoder::Start() { // We support up to 2 pulse counters if (interruptCount == 0) - attachInterrupt(digitalPinToInterrupt(config.pin), PulseInterrupt0, RISING); + attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0, + RISING); else if (interruptCount == 1) - attachInterrupt(digitalPinToInterrupt(config.pin), PulseInterrupt1, RISING); + attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1, + RISING); else { // maximum interrupt count reached std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached" @@ -70,6 +60,7 @@ DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, } interruptIx = interruptCount; interruptCount++; + std::cout << "pin ints. " << interruptCount << std::endl; } int DigitalInput::RelativeEncoder::GetPulseCount() { @@ -88,6 +79,7 @@ int DigitalInput::RelativeEncoder::GetPulseCount() { void DigitalInput::RelativeEncoder::Update(bool recursive) { unsigned long currentTimeMs = GetTimeMs(); if (this->lastUpdateTime == 0) { + this->Start(); this->lastUpdateTime = currentTimeMs; return; } @@ -99,8 +91,8 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) { this->pulseFrequency = pulseCount / timeStep; this->rotationSpeed = pulseFrequency / pulsesPerRevolution; - std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency - << " timestep " << timeStep << std::endl; + // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency + // << " timestep " << timeStep << std::endl; this->lastUpdateTime = currentTimeMs; } diff --git a/Arduino/Things/DigitalInput.h b/Arduino/Things/DigitalInput.h index ef3d25c..81a203e 100644 --- a/Arduino/Things/DigitalInput.h +++ b/Arduino/Things/DigitalInput.h @@ -13,7 +13,7 @@ class DigitalInput : public Thing { /// @param participant The participant to use /// @param pin The digital pin DigitalInput(Participant* participant, unsigned char pin); - //DigitalInput(Thing* parent, unsigned char pin); + // DigitalInput(Thing* parent, unsigned char pin); DigitalInput(unsigned char pin, Thing& parent); bool isHigh = false; @@ -55,7 +55,7 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder { unsigned char pulsesPerRevolution; }; - RelativeEncoder(Configuration config, Thing& parent); + RelativeEncoder(Configuration config, Thing& parent = Thing::LocalRoot()); unsigned char pulsesPerRevolution; @@ -81,6 +81,9 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder { int GetPulseCount(); long netPulseCount = 0; unsigned long lastUpdateTime = 0; + + private: + void Start(); }; #pragma endregion Incremental encoder diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 7abd9a8..81d581d 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -6,16 +6,6 @@ namespace RoboidControl { namespace Arduino { -UltrasonicSensor::UltrasonicSensor(Configuration config, - Participant* participant) - : Thing(participant) { - this->pinTrigger = config.triggerPin; - this->pinEcho = config.echoPin; - - pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode - pinMode(pinEcho, INPUT); // configure the echo pin to input mode -} - UltrasonicSensor::UltrasonicSensor(Configuration config, Thing& parent) : Thing(parent) { this->pinTrigger = config.triggerPin; @@ -67,23 +57,16 @@ void UltrasonicSensor::Update(bool recursive) { #pragma region Touch sensor -// UltrasonicSensor::TouchSensor::TouchSensor( -// UltrasonicSensor::Configuration config, -// Thing& parent) -// : RoboidControl::TouchSensor(parent), ultrasonic(config, parent) { -// this->touchedSomething = false; -// } UltrasonicSensor::TouchSensor::TouchSensor( UltrasonicSensor::Configuration config, Thing& parent) - : RoboidControl::TouchSensor(parent), ultrasonic(config, parent) { - this->touchedSomething = false; -} + : RoboidControl::TouchSensor(parent), ultrasonic(config) {} void UltrasonicSensor::TouchSensor::Update(bool recursive) { - this->ultrasonic.Update(recursive); + this->ultrasonic.Update(false); this->touchedSomething = (this->ultrasonic.distance > 0 && this->ultrasonic.distance <= this->touchDistance); + RoboidControl::TouchSensor::Update(recursive); } #pragma region Touch sensor diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index 0ae252e..e4554e2 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -13,12 +13,7 @@ class UltrasonicSensor : Thing { int echoPin; }; - /// @brief Setup an ultrasonic sensor - /// @param participant The participant to use - /// @param pinTrigger The pin number of the trigger signal - /// @param pinEcho The pin number of the echo signal - UltrasonicSensor(Configuration config, Participant* participant); - UltrasonicSensor(Configuration config, Thing& parent); + UltrasonicSensor(Configuration config, Thing& parent = Thing::LocalRoot()); // parameters @@ -51,8 +46,8 @@ class UltrasonicSensor : Thing { class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { public: - TouchSensor(UltrasonicSensor::Configuration config, Thing& parent); - //TouchSensor(UltrasonicSensor::Configuration config, Thing* parent); + TouchSensor(UltrasonicSensor::Configuration config, + Thing& parent = Thing::LocalRoot()); float touchDistance = 0.2f; diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index c476388..79d2e4a 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { this->thingId = thing->id; this->poseType = 0; - if (thing->positionUpdated || (force && thing->GetParent() != nullptr)) { + if (thing->positionUpdated || (force && thing->IsRoot())) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; } - if (thing->orientationUpdated || (force && thing->GetParent() != nullptr)) { + if (thing->orientationUpdated || (force && thing->IsRoot())) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; } diff --git a/Messages/ThingMsg.cpp b/Messages/ThingMsg.cpp index 932530f..2350dd2 100644 --- a/Messages/ThingMsg.cpp +++ b/Messages/ThingMsg.cpp @@ -14,11 +14,12 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; this->thingType = thing->type; - Thing* parent = thing->GetParent(); - if (parent != nullptr) - this->parentId = parent->id; - else + if (thing->IsRoot()) this->parentId = 0; + else { + Thing parent = thing->GetParent(); + this->parentId = parent.id; + } } // ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId, @@ -33,8 +34,9 @@ ThingMsg::~ThingMsg() {} unsigned char ThingMsg::Serialize(char* buffer) { #if defined(DEBUG) - std::cout << "Send ThingMsg [" << (int)this->networkId << "/" << (int)this->thingId - << "] " << (int)this->thingType << " " << (int)this->parentId << std::endl; + std::cout << "Send ThingMsg [" << (int)this->networkId << "/" + << (int)this->thingId << "] " << (int)this->thingType << " " + << (int)this->parentId << std::endl; #endif unsigned char ix = 0; buffer[ix++] = this->id; diff --git a/Participant.cpp b/Participant.cpp index 51dfabd..eea7563 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -8,7 +8,12 @@ namespace RoboidControl { ParticipantRegistry Participant::registry; -Participant LocalParticipant = Participant("0.0.0.0", 0); +//Participant Participant::LocalParticipant = Participant("0.0.0.0", 0); + +Participant& Participant::LocalParticipant() { + static Participant localParticipant("0.0.0.0", 0); + return localParticipant; + } Participant::Participant(const char* ipAddress, int port) { // make a copy of the ip address string @@ -51,7 +56,9 @@ Thing* Participant::Get(unsigned char thingId) { } void Participant::Add(Thing* thing, bool checkId) { + if (checkId && thing->id == 0) { + // allocate a new thing ID #if defined(NO_STD) thing->id = this->thingCount + 1; @@ -69,7 +76,7 @@ void Participant::Add(Thing* thing, bool checkId) { this->things.push_back(thing); #endif // std::cout << "Add thing with generated ID " << this->ipAddress << ":" - // << this->port << "[" << (int)thing->id << "]\n"; + // << this->port << "[" << (int)thing->id << "]\n"; } else { Thing* foundThing = Get(thing->id); if (foundThing == nullptr) { diff --git a/Participant.h b/Participant.h index 35c5ce4..2be1323 100644 --- a/Participant.h +++ b/Participant.h @@ -72,7 +72,7 @@ class Participant { /// @brief Destructor for the participant ~Participant(); - static Participant LocalParticipant; + static Participant& LocalParticipant(); public: #if defined(NO_STD) diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index a24bf09..394f328 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -27,7 +27,7 @@ void SiteServer::UpdateMyThings() { if (thing == nullptr) continue; - thing->Update( true); + thing->Update(true); if (this->isIsolated == false) { // Send to all other participants @@ -76,16 +76,17 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing::Reconstruct(sender, msg->thingType, msg->thingId); if (msg->parentId != 0) { - thing->SetParent(Get(msg->parentId)); - if (thing->GetParent() != nullptr) + thing->SetParent(*Get(msg->parentId)); + if (thing->IsRoot()) + // if (thing->GetParent() != nullptr) #if defined(NO_STD) ; -#else +#else std::cout << "Could not find parent [" << (int)msg->networkId << "/" << (int)msg->parentId << "]\n"; -#endif +#endif } else - thing->SetParent(nullptr); + thing->SetParent(Thing::LocalRoot()); } #pragma endregion Receive diff --git a/Thing.cpp b/Thing.cpp index 3f8168c..2ec0708 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -20,7 +20,10 @@ namespace RoboidControl { #pragma region Init -Thing Thing::Root = Thing(Thing::Type::Undetermined, Root); +Thing& Thing::LocalRoot() { + static Thing localRoot(Thing::Type::Undetermined, localRoot); + return localRoot; +} Thing::Thing(unsigned char thingType, Thing& parent) { this->type = thingType; @@ -34,47 +37,18 @@ Thing::Thing(unsigned char thingType, Thing& parent) { this->linearVelocity = Spherical::zero; this->angularVelocity = Spherical::zero; - // std::cout << "add thing [" << (int)this->id << "] to owner " - // << this->owner->ipAddress << ":" << this->owner->port << - // std::endl; + this->owner = &Participant::LocalParticipant(); this->owner->Add(this, true); - this->SetParent(&parent); + this->SetParent(parent); } -Thing::Thing(Participant* owner, unsigned char thingType) { - // unsigned char thingId) { - if (owner == nullptr) - owner = IsolatedParticipant::Isolated(); - this->owner = owner; - // this->id = thingId; - this->type = thingType; - - this->position = Spherical::zero; - this->positionUpdated = true; - this->orientation = SwingTwist::identity; - this->orientationUpdated = true; - this->hierarchyChanged = true; - - this->linearVelocity = Spherical::zero; - this->angularVelocity = Spherical::zero; - - // std::cout << "add thing [" << (int)this->id << "] to owner " - // << this->owner->ipAddress << ":" << this->owner->port << - // std::endl; - this->owner->Add(this, true); -} - -// Thing::Thing(Thing* parent, unsigned char thingType) //, unsigned char thingId) -// : Thing(parent->owner, thingType) { //}, thingId) { -// this->SetParent(parent); +// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, +// unsigned char thingId) { +// Thing thing = Thing(owner, thingType); +// thing.id = thingId; +// return thing; // } -Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, unsigned char thingId) { - Thing thing = Thing(owner, thingType); - thing.id = thingId; - return thing; -} - #pragma endregion Init void Thing::SetName(const char* name) { @@ -92,26 +66,39 @@ void Thing::SetModel(const char* url) { #pragma region Hierarchy -void Thing::SetParent(Thing* parent) { - if (parent == nullptr) { - Thing* parentThing = this->parent; - if (parentThing != nullptr) - parentThing->RemoveChild(this); - this->parent = nullptr; - } else - parent->AddChild(this); +// void Thing::SetParent(Thing* parent) { +// if (parent == nullptr) { +// Thing* parentThing = this->parent; +// if (parentThing != nullptr) +// parentThing->RemoveChild(this); +// this->parent = nullptr; +// } else +// parent->AddChild(this); +// this->hierarchyChanged = true; +// } + +void Thing::SetParent(Thing& parent) { + parent.AddChild(this); this->hierarchyChanged = true; } +const Thing& Thing::GetParent() { + return *this->parent; +} + +bool Thing::IsRoot() const { + return this == &LocalRoot(); //&Thing::Root; +} + // void Thing::SetParent(Thing* root, const char* name) { // Thing* thing = root->FindChild(name); // if (thing != nullptr) // this->SetParent(thing); // } -Thing* Thing::GetParent() { - return this->parent; -} +// Thing* Thing::GetParent() { +// return this->parent; +// } Thing* Thing::GetChildByIndex(unsigned char ix) { return this->children[ix]; @@ -123,11 +110,11 @@ void Thing::AddChild(Thing* child) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { newChildren[childIx] = this->children[childIx]; - if (this->children[childIx] == child) { - // child is already present, stop copying do not update the children - delete[] newChildren; - return; - } + // if (this->children[childIx] == child) { + // // child is already present, stop copying do not update the children + // delete[] newChildren; + // return; + // } } newChildren[this->childCount] = child; @@ -156,7 +143,7 @@ Thing* Thing::RemoveChild(Thing* child) { } } - child->parent = nullptr; + child->parent = &Thing::LocalRoot(); delete[] this->children; this->children = newChildren; diff --git a/Thing.h b/Thing.h index cc97ab8..2fca59c 100644 --- a/Thing.h +++ b/Thing.h @@ -41,17 +41,16 @@ class Thing { }; #pragma region Init - static Thing Root; + static Thing& LocalRoot(); - Thing(unsigned char thingType = Thing::Type::Undetermined, Thing& parent = Root); - - /// @brief Create a new thing for a participant - /// @param owner The owning participant + /// @brief Create a new thing /// @param thingType The type of thing (can use Thing::Type) - /// @param thingId The ID of the thing, leave out or set to zero to generate - /// an ID - Thing(Participant* owner, - unsigned char thingType = Thing::Type::Undetermined); + /// @param parent (optional) The parent thing + /// The owner will be the same as the owner of the parent thing, it will + /// be Participant::LocalParticipant if the parent is not specified. A thing + /// without a parent will be a root thing. + Thing(unsigned char thingType = Thing::Type::Undetermined, + Thing& parent = LocalRoot()); /// @brief Create a new child thing /// @param parent The parent thing @@ -108,10 +107,14 @@ class Thing { /// @brief Sets the parent of this Thing /// @param parent The Thing which should become the parent - virtual void SetParent(Thing* parent); + // virtual void SetParent(Thing* parent); + void SetParent(Thing& parent); /// @brief Gets the parent of this Thing /// @return The parent Thing - Thing* GetParent(); + // Thing* GetParent(); + const Thing& GetParent(); + + bool IsRoot() const; /// @brief The number of children unsigned char childCount = 0; diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp index bff3b80..9cb2107 100644 --- a/Things/ControlledMotor.cpp +++ b/Things/ControlledMotor.cpp @@ -2,11 +2,14 @@ namespace RoboidControl { -ControlledMotor::ControlledMotor(Motor* motor, RelativeEncoder* encoder) - : Motor(), motor(motor), encoder(encoder) { +ControlledMotor::ControlledMotor(Motor& motor, + RelativeEncoder& encoder, + Thing& parent) + : Motor(parent), motor(motor), encoder(encoder) { this->type = Type::ControlledMotor; - Thing* parent = motor->GetParent(); - this->SetParent(parent); + // encoder.SetParent(*this); + // Thing parent = motor.GetParent(); + // this->SetParent(parent); } void ControlledMotor::SetTargetVelocity(float velocity) { @@ -16,9 +19,9 @@ void ControlledMotor::SetTargetVelocity(float velocity) { } void ControlledMotor::Update(bool recurse) { - encoder->Update(recurse); + encoder.Update(false); - this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed; + this->actualVelocity = (int)rotationDirection * encoder.rotationSpeed; unsigned long currentTimeMs = GetTimeMs(); float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f; @@ -27,10 +30,11 @@ void ControlledMotor::Update(bool recurse) { float error = this->targetVelocity - this->actualVelocity; float acceleration = error * timeStep * pidP; // Just P is used at this moment - motor->SetTargetVelocity(targetVelocity + - acceleration); // or something like that + std::cout << "motor acc. " << acceleration << std::endl; + motor.SetTargetVelocity(targetVelocity + + acceleration); // or something like that - motor->Update(recurse); + motor.Update(false); } // float ControlledMotor::GetActualVelocity() { diff --git a/Things/ControlledMotor.h b/Things/ControlledMotor.h index 1742289..1f46aa2 100644 --- a/Things/ControlledMotor.h +++ b/Things/ControlledMotor.h @@ -10,12 +10,13 @@ namespace RoboidControl { /// The speed is measured in revolutions per second. class ControlledMotor : public Motor { public: - ControlledMotor(Motor* motor, RelativeEncoder* encoder); + ControlledMotor(Motor& motor, RelativeEncoder& encoder, Thing& parent = Thing::LocalRoot()); float pidP = 1; float pidD = 0; float pidI = 0; + /// @brief The actual velocity in revolutions per second float actualVelocity; enum Direction { Forward = 1, Reverse = -1 }; @@ -24,29 +25,14 @@ class ControlledMotor : public Motor { virtual void Update(bool recurse = false) override; /// @brief Set the target verlocity for the motor controller - /// @param speed the target velocity in revolutions per second. + /// @param speed the target velocity in revolutions per second virtual void SetTargetVelocity(float velocity) override; - /// @brief Get the actual velocity from the encoder - /// @return The velocity in revolutions per second - // float GetActualVelocity(); - - // bool Drive(float distance); - - Motor* motor; - RelativeEncoder* encoder; + Motor& motor; + RelativeEncoder& encoder; protected: float lastUpdateTime; - - - //float targetVelocity; - -// float netDistance = 0; -// float startDistance = 0; -// bool driving = false; -// float targetDistance = 0; -// float lastEncoderPosition = 0; }; } // namespace RoboidControl diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index 0b472a7..bde68c7 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -2,25 +2,8 @@ namespace RoboidControl { -// DifferentialDrive::DifferentialDrive(Participant* participant, -// unsigned char thingId) -// : Thing(participant, Thing::Type::DifferentialDrive, thingId) { -// this->leftWheel = new Motor(); -// this->rightWheel = new Motor(); -// } - -// DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) -// : Thing(parent, Thing::Type::DifferentialDrive, thingId) {} - -DifferentialDrive::DifferentialDrive(Participant* owner) : Thing(owner, Type::DifferentialDrive) { - this->leftWheel = new Motor(); - this->rightWheel = new Motor(); -} -// DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent, Type::DifferentialDrive) { -// this->leftWheel = new Motor(); -// this->rightWheel = new Motor(); -// } -DifferentialDrive::DifferentialDrive(Thing& parent) : Thing(Type::DifferentialDrive, parent) { +DifferentialDrive::DifferentialDrive(Thing& parent) + : Thing(Type::DifferentialDrive, parent) { this->leftWheel = new Motor(); this->rightWheel = new Motor(); } @@ -40,21 +23,28 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter, this->rightWheel->SetPosition(Spherical(distance, Direction::right)); } -// void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) { -// float distance = this->wheelSeparation / 2; -// this->leftWheel = leftWheel; -// ; -// if (leftWheel != nullptr) -// this->leftWheel->SetPosition(Spherical(distance, Direction::left)); +Motor& DifferentialDrive::GetMotorLeft() { + return *this->leftWheel; +} -// this->rightWheel = rightWheel; -// if (rightWheel != nullptr) -// this->rightWheel->SetPosition(Spherical(distance, Direction::right)); -// } +Motor& DifferentialDrive::GetMotorRight() { + return *this->rightWheel; +} -void DifferentialDrive::SetWheelVelocity(float velocityLeft, float velocityRight) { +void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) { + float distance = this->wheelSeparation / 2; + this->leftWheel = &leftMotor; + this->leftWheel->SetPosition(Spherical(distance, Direction::left)); + + this->rightWheel = &rightMotor; + this->rightWheel->SetPosition(Spherical(distance, Direction::right)); +} + +void DifferentialDrive::SetWheelVelocity(float velocityLeft, + float velocityRight) { // if (this->leftWheel != nullptr) - // this->leftWheel->SetAngularVelocity(Spherical(velocityLeft, Direction::left)); + // this->leftWheel->SetAngularVelocity(Spherical(velocityLeft, + // Direction::left)); // if (this->rightWheel != nullptr) // this->rightWheel->SetAngularVelocity( // Spherical(velocityRight, Direction::right)); diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index 65cd5a7..c7685fe 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -10,20 +10,11 @@ namespace RoboidControl { /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink class DifferentialDrive : public Thing { public: - // /// @brief Create a differential drive without networking support - // DifferentialDrive(); - /// @brief Create a differential drive with networking support - /// @param participant The local participant - /// @param thingId The ID of the thing, leave out or set to zero to generate - /// an ID - DifferentialDrive(Participant* participant); //, - //unsigned char thingId = 0); /// @brief Create a new child differential drive /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - // DifferentialDrive(Thing* parent); //, unsigned char thingId = 0); - DifferentialDrive(Thing& parent = Thing::Root); + DifferentialDrive(Thing& parent = Thing::LocalRoot()); /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters @@ -33,10 +24,13 @@ class DifferentialDrive : public Thing { /// linear and angular velocity. /// @sa SetLinearVelocity SetAngularVelocity void SetDriveDimensions(float wheelDiameter, float wheelSeparation); + + Motor& GetMotorLeft(); + Motor& GetMotorRight(); /// @brief Congures the motors for the wheels /// @param leftWheel The motor for the left wheel /// @param rightWheel The motor for the right wheel - void SetMotors(Thing* leftWheel, Thing* rightWheel); + void SetMotors(Motor& leftMotor, Motor& rightMotor); /// @brief Directly specify the speeds of the motors /// @param speedLeft The speed of the left wheel in degrees per second. @@ -48,10 +42,6 @@ class DifferentialDrive : public Thing { /// @copydoc RoboidControl::Thing::Update(unsigned long) virtual void Update(bool recursive = true) override; - /// @brief The left wheel - Motor* leftWheel = nullptr; - /// @brief The right wheel - Motor* rightWheel = nullptr; protected: /// @brief The radius of a wheel in meters @@ -61,6 +51,11 @@ class DifferentialDrive : public Thing { /// @brief Convert revolutions per second to meters per second float rpsToMs = 1.0f; + + /// @brief The left wheel + Motor* leftWheel = nullptr; + /// @brief The right wheel + Motor* rightWheel = nullptr; }; } // namespace RoboidControl \ No newline at end of file diff --git a/Things/DigitalSensor.cpp b/Things/DigitalSensor.cpp index 0ac2414..0c1e213 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -10,7 +10,7 @@ namespace RoboidControl { // DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId) // : Thing(parent, Type::Switch) {} -DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {} +// DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {} // DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {} DigitalSensor::DigitalSensor(Thing& parent) : Thing(Type::Switch, parent) {} diff --git a/Things/DigitalSensor.h b/Things/DigitalSensor.h index 9b77f6b..b6579f2 100644 --- a/Things/DigitalSensor.h +++ b/Things/DigitalSensor.h @@ -19,7 +19,7 @@ class DigitalSensor : public Thing { /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID // DigitalSensor(Thing* parent); //, unsigned char thingId = 0); - DigitalSensor(Thing& parent = Thing::Root); + DigitalSensor(Thing& parent = Thing::LocalRoot()); /// @brief The sigital state bool state = 0; diff --git a/Things/Motor.cpp b/Things/Motor.cpp index bbcd532..354c7bd 100644 --- a/Things/Motor.cpp +++ b/Things/Motor.cpp @@ -2,8 +2,8 @@ namespace RoboidControl { -RoboidControl::Motor::Motor(Participant* owner) - : Thing(owner, Type::UncontrolledMotor) {} +// RoboidControl::Motor::Motor(Participant* owner) +// : Thing(owner, Type::UncontrolledMotor) {} // RoboidControl::Motor::Motor(Thing* parent) // : Thing(parent, Type::UncontrolledMotor) {} diff --git a/Things/Motor.h b/Things/Motor.h index 530941b..a625bb3 100644 --- a/Things/Motor.h +++ b/Things/Motor.h @@ -8,7 +8,7 @@ class Motor : public Thing { public: Motor(Participant* owner); // Motor(Thing* parent); - Motor(Thing& parent = Thing::Root); + Motor(Thing& parent = Thing::LocalRoot()); /// @brief Motor turning direction enum class Direction { Clockwise = 1, CounterClockwise = -1 }; diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp index b7f8a8c..5cc9464 100644 --- a/Things/RelativeEncoder.cpp +++ b/Things/RelativeEncoder.cpp @@ -2,8 +2,6 @@ namespace RoboidControl { -RelativeEncoder::RelativeEncoder(Participant* owner) - : Thing(owner, Type::IncrementalEncoder) {} RelativeEncoder::RelativeEncoder(Thing& parent) : Thing(Type::IncrementalEncoder, parent) {} diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h index ff951b8..e783c7d 100644 --- a/Things/RelativeEncoder.h +++ b/Things/RelativeEncoder.h @@ -16,7 +16,7 @@ class RelativeEncoder : public Thing { /// rotation RelativeEncoder(Participant* owner); // RelativeEncoder(Thing* parent); - RelativeEncoder(Thing& parent = Thing::Root); + RelativeEncoder(Thing& parent = Thing::LocalRoot()); /// @brief Get the rotation speed /// @return The speed in revolutions per second diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index d9c9113..a3fb13d 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -8,7 +8,7 @@ namespace RoboidControl { // unsigned char thingId) // : Thing(participant, Type::TemperatureSensor, thingId) {} -TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {} +// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {} TemperatureSensor::TemperatureSensor(Thing& parent) : Thing(Type::TemperatureSensor, parent) {} diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index 23f56fa..b1feac3 100644 --- a/Things/TemperatureSensor.h +++ b/Things/TemperatureSensor.h @@ -14,7 +14,7 @@ class TemperatureSensor : public Thing { /// @param thingId The ID of the thing TemperatureSensor(Participant* participant); //, unsigned char thingId); // TemperatureSensor(Thing* parent); - TemperatureSensor(Thing& parent = Thing::Root); + TemperatureSensor(Thing& parent = Thing::LocalRoot()); /// @brief The measured temperature float temperature = 0; diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index d3d2a91..6960b45 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -2,20 +2,6 @@ namespace RoboidControl { -// TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {} - -// TouchSensor::TouchSensor(Participant* owner, unsigned char thingId) -// : Thing(owner, Thing::Type::TouchSensor, thingId) {} - -// TouchSensor::TouchSensor(Thing* parent, unsigned char thingId) : -// Thing(parent->owner, Thing::Type::TouchSensor, thingId) { -// this->SetParent(parent); -// } - -TouchSensor::TouchSensor(Participant* owner) - : Thing(owner, Type::TouchSensor) {} - -// TouchSensor::TouchSensor(Thing* parent) : Thing(parent, Type::TouchSensor) {} TouchSensor::TouchSensor(Thing& parent) : Thing(Type::TouchSensor, parent) {} int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index aca28a4..0768f66 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -8,19 +8,11 @@ namespace RoboidControl { class TouchSensor : public Thing { // Why finishing this release (0.3), I notice that this is equivalent to a digital sensor public: - /// @brief Create a touch sensor without communication abilities - //TouchSensor(); - /// @brief Create a touch sensor for a participant - /// @param owner The owning participant - /// @param thingId The ID of the thing, leave out or set to zero to generate - /// an ID - TouchSensor(Participant* owner); //, unsigned char thingId = 0); /// @brief Create a new child touch sensor /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - // TouchSensor(Thing* parent); //, unsigned char thingId = 0); - TouchSensor(Thing& parent = Thing::Root); + TouchSensor(Thing& parent = Thing::LocalRoot()); /// @brief Value which is true when the sensor is touching something, false /// otherwise From 140138977a7690c553aef185f9ec05d80f6e822f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 21 May 2025 15:59:11 +0200 Subject: [PATCH 31/56] AR mode works, but reaction time is slow --- Arduino/ArduinoParticipant.cpp | 20 ++----- Arduino/Things/DRV8833.cpp | 18 +++--- Arduino/Things/DRV8833.h | 11 ++-- Arduino/Things/DigitalInput.cpp | 6 +- Arduino/Things/DigitalInput.h | 8 +-- Arduino/Things/UltrasonicSensor.cpp | 17 +++--- Arduino/Things/UltrasonicSensor.h | 4 +- Messages/ThingMsg.cpp | 12 +--- Participant.cpp | 59 +++++++++++-------- Participant.h | 12 +++- Participants/ParticipantUDP.cpp | 50 ++++++++++++++-- Participants/ParticipantUDP.h | 5 +- Participants/SiteServer.cpp | 7 ++- Thing.cpp | 88 +++++++++++++++++++---------- Thing.h | 29 ++++++---- Things/ControlledMotor.cpp | 14 ++--- Things/ControlledMotor.h | 6 +- Things/DifferentialDrive.cpp | 41 +++++++++++--- Things/DifferentialDrive.h | 23 +++++--- Things/DigitalSensor.cpp | 2 +- Things/DigitalSensor.h | 2 +- Things/Motor.cpp | 15 +++-- Things/Motor.h | 11 ++-- Things/RelativeEncoder.cpp | 2 +- Things/RelativeEncoder.h | 2 +- Things/TemperatureSensor.cpp | 2 +- Things/TemperatureSensor.h | 2 +- Things/TouchSensor.cpp | 20 ++++++- Things/TouchSensor.h | 7 ++- 29 files changed, 313 insertions(+), 182 deletions(-) diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index dad27fb..a787d5a 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -82,19 +82,6 @@ void ParticipantUDP::Receive() { senderAddress.toCharArray(sender_ipAddress, 16); unsigned int sender_port = udp->remotePort(); - // Participant* remoteParticipant = this->Get(sender_ipAddress, - // sender_port); if (remoteParticipant == nullptr) { - // remoteParticipant = this->Add(sender_ipAddress, - // sender_port); - // // std::cout << "New sender " << sender_ipAddress << ":" << sender_port - // // << "\n"; - // // std::cout << "New remote participant " << - // remoteParticipant->ipAddress - // // << ":" << remoteParticipant->port << " " - // // << (int)remoteParticipant->networkId << "\n"; - // } - - // ReceiveData(packetSize, remoteParticipant); ReceiveData(packetSize, sender_ipAddress, sender_port); packetSize = udp->parsePacket(); } @@ -107,6 +94,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { // << remoteParticipant->port << "\n"; int n = 0; + int r = 0; do { if (n > 0) { #if !defined(NO_STD) @@ -115,9 +103,13 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { delay(10); } n++; + udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port); udp->write((unsigned char*)buffer, bufferSize); - } while (udp->endPacket() == 0 && n < 10); + r = udp->endPacket(); + // On an Uno R4 WiFi, endPacket blocks for 10 seconds the first time + // It is not cleary yet why + } while (r == 0 && n < 10); #endif return true; diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index e95d94a..59a657d 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -7,14 +7,14 @@ namespace Arduino { #pragma region DRV8833 -DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) { +DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) { this->pinStandby = config.standby; if (pinStandby != 255) pinMode(pinStandby, OUTPUT); - this->motorA = new DRV8833Motor(*this, config.AIn1, config.AIn2); + this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2); this->motorA->SetName("Motor A"); - this->motorB = new DRV8833Motor(*this, config.BIn1, config.BIn2); + this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2); this->motorB->SetName("Motor B"); } @@ -23,11 +23,11 @@ DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) { #pragma region Differential drive DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, - Thing& parent) - : RoboidControl::DifferentialDrive(parent), drv8833(config) { - this->leftWheel = this->drv8833.motorA; - this->rightWheel = this->drv8833.motorB; -} + Thing* parent) + : RoboidControl::DifferentialDrive(this->drv8833.motorA, + this->drv8833.motorB, + parent), + drv8833(config, this) {} void DRV8833::DifferentialDrive::Update(bool recurse) { RoboidControl::DifferentialDrive::Update(recurse); @@ -42,7 +42,7 @@ void DRV8833::DifferentialDrive::Update(bool recurse) { uint8_t DRV8833Motor::nextAvailablePwmChannel = 0; #endif -DRV8833Motor::DRV8833Motor(DRV8833& driver, +DRV8833Motor::DRV8833Motor(DRV8833* driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse) diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index 1f87b85..f2c2d3f 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -18,12 +18,11 @@ class DRV8833 : public Thing { int AIn2; int BIn1; int BIn2; - int standby = 255; + int standby; }; /// @brief Setup a DRV8833 motor controller - // DRV8833(Configuration config, Participant* owner = nullptr); - DRV8833(Configuration config, Thing& parent = Thing::LocalRoot()); + DRV8833(Configuration config, Thing* parent = Thing::LocalRoot()); DRV8833Motor* motorA = nullptr; DRV8833Motor* motorB = nullptr; @@ -39,9 +38,7 @@ class DRV8833 : public Thing { class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { public: - // DifferentialDrive(DRV8833::Configuration config, - // Participant* participant = nullptr); - DifferentialDrive(DRV8833::Configuration config, Thing& parent = Thing::LocalRoot()); + DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot()); virtual void Update(bool recurse = false) override; @@ -60,7 +57,7 @@ class DRV8833Motor : public Motor { /// @param pinIn1 the pin number for the in1 signal /// @param pinIn2 the pin number for the in2 signal /// @param direction the forward turning direction of the motor - DRV8833Motor(DRV8833& driver, + DRV8833Motor(DRV8833* driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false); diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index e6435db..12f3b12 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -7,7 +7,7 @@ namespace Arduino { #pragma region Digital input -DigitalInput::DigitalInput(unsigned char pin, Thing& parent) : Thing(parent) { +DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(Type::Undetermined, parent) { this->pin = pin; pinMode(this->pin, INPUT); std::cout << "digital input start\n"; @@ -23,7 +23,7 @@ void DigitalInput::Update(bool recursive) { #pragma region Touch sensor -DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing& parent) +DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent) : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {} void DigitalInput::TouchSensor::Update(bool recursive) { @@ -39,7 +39,7 @@ volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0; volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0; DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, - Thing& parent) + Thing* parent) : RoboidControl::RelativeEncoder(parent), digitalInput(config.pin, parent), pulsesPerRevolution(config.pulsesPerRevolution) {} diff --git a/Arduino/Things/DigitalInput.h b/Arduino/Things/DigitalInput.h index 81a203e..e529dbc 100644 --- a/Arduino/Things/DigitalInput.h +++ b/Arduino/Things/DigitalInput.h @@ -12,9 +12,9 @@ class DigitalInput : public Thing { /// @brief Create a new digital input /// @param participant The participant to use /// @param pin The digital pin - DigitalInput(Participant* participant, unsigned char pin); + //DigitalInput(Participant* participant, unsigned char pin); // DigitalInput(Thing* parent, unsigned char pin); - DigitalInput(unsigned char pin, Thing& parent); + DigitalInput(unsigned char pin, Thing* parent); bool isHigh = false; bool isLow = false; @@ -35,7 +35,7 @@ class DigitalInput : public Thing { class DigitalInput::TouchSensor : public RoboidControl::TouchSensor { public: - TouchSensor(unsigned char pin, Thing& parent); + TouchSensor(unsigned char pin, Thing* parent); /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) virtual void Update(bool recurse = false) override; @@ -55,7 +55,7 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder { unsigned char pulsesPerRevolution; }; - RelativeEncoder(Configuration config, Thing& parent = Thing::LocalRoot()); + RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot()); unsigned char pulsesPerRevolution; diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 81d581d..d92da44 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -6,8 +6,9 @@ namespace RoboidControl { namespace Arduino { -UltrasonicSensor::UltrasonicSensor(Configuration config, Thing& parent) - : Thing(parent) { +UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) + : Thing(Type::Undetermined, parent) { + this->name = "Ultrasonic sensor"; this->pinTrigger = config.triggerPin; this->pinEcho = config.echoPin; @@ -57,16 +58,14 @@ void UltrasonicSensor::Update(bool recursive) { #pragma region Touch sensor -UltrasonicSensor::TouchSensor::TouchSensor( - UltrasonicSensor::Configuration config, - Thing& parent) - : RoboidControl::TouchSensor(parent), ultrasonic(config) {} +UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent) + : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {} void UltrasonicSensor::TouchSensor::Update(bool recursive) { - this->ultrasonic.Update(false); - this->touchedSomething = (this->ultrasonic.distance > 0 && - this->ultrasonic.distance <= this->touchDistance); RoboidControl::TouchSensor::Update(recursive); + this->ultrasonic.Update(false); + this->touchedSomething |= (this->ultrasonic.distance > 0 && + this->ultrasonic.distance <= this->touchDistance); } #pragma region Touch sensor diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index e4554e2..7c02778 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -13,7 +13,7 @@ class UltrasonicSensor : Thing { int echoPin; }; - UltrasonicSensor(Configuration config, Thing& parent = Thing::LocalRoot()); + UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot()); // parameters @@ -47,7 +47,7 @@ class UltrasonicSensor : Thing { class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { public: TouchSensor(UltrasonicSensor::Configuration config, - Thing& parent = Thing::LocalRoot()); + Thing* parent = Thing::LocalRoot()); float touchDistance = 0.2f; diff --git a/Messages/ThingMsg.cpp b/Messages/ThingMsg.cpp index 2350dd2..be0c41c 100644 --- a/Messages/ThingMsg.cpp +++ b/Messages/ThingMsg.cpp @@ -17,19 +17,11 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) { if (thing->IsRoot()) this->parentId = 0; else { - Thing parent = thing->GetParent(); - this->parentId = parent.id; + Thing* parent = thing->GetParent(); + this->parentId = parent->id; } } -// ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId, -// unsigned char thingType, unsigned char parentId) { -// this->networkId = networkId; -// this->thingId = thingId; -// this->thingType = thingType; -// this->parentId = parentId; -// } - ThingMsg::~ThingMsg() {} unsigned char ThingMsg::Serialize(char* buffer) { diff --git a/Participant.cpp b/Participant.cpp index eea7563..3f11839 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -8,14 +8,26 @@ namespace RoboidControl { ParticipantRegistry Participant::registry; -//Participant Participant::LocalParticipant = Participant("0.0.0.0", 0); +Participant* Participant::LocalParticipant = new Participant(); -Participant& Participant::LocalParticipant() { - static Participant localParticipant("0.0.0.0", 0); - return localParticipant; - } +void Participant::ReplaceLocalParticipant(Participant& newParticipant) { + LocalParticipant = &newParticipant; + std::cout << "Replaced local participant" << std::endl; +} + +Participant::Participant() { + std::cout << "P\n"; + //this->root.name = "Isolated"; + this->root = new Thing(this); + this->root->name = "Root"; + this->Add(this->root); +} Participant::Participant(const char* ipAddress, int port) { + // Add the root thing to the list of things, because we could not do that + // earlier (I think) + this->Add(this->root); + // make a copy of the ip address string int addressLength = (int)strlen(ipAddress); int stringLength = addressLength + 1; @@ -33,7 +45,7 @@ Participant::Participant(const char* ipAddress, int port) { } Participant::~Participant() { - registry.Remove(this); + // registry.Remove(this); delete[] this->ipAddress; } @@ -56,9 +68,7 @@ Thing* Participant::Get(unsigned char thingId) { } void Participant::Add(Thing* thing, bool checkId) { - if (checkId && thing->id == 0) { - // allocate a new thing ID #if defined(NO_STD) thing->id = this->thingCount + 1; @@ -76,7 +86,7 @@ void Participant::Add(Thing* thing, bool checkId) { this->things.push_back(thing); #endif // std::cout << "Add thing with generated ID " << this->ipAddress << ":" - // << this->port << "[" << (int)thing->id << "]\n"; + // << this->port << "[" << (int)thing->id << "]\n"; } else { Thing* foundThing = Get(thing->id); if (foundThing == nullptr) { @@ -128,8 +138,8 @@ Participant* ParticipantRegistry::Get(const char* ipAddress, continue; if (strcmp(participant->ipAddress, ipAddress) == 0 && participant->port == port) { - std::cout << "found participant " << participant->ipAddress << ":" - << (int)participant->port << std::endl; + // std::cout << "found participant " << participant->ipAddress << ":" + // << (int)participant->port << std::endl; return participant; } } @@ -140,7 +150,7 @@ Participant* ParticipantRegistry::Get(const char* ipAddress, } Participant* ParticipantRegistry::Get(unsigned char participantId) { -#if !defined(NO_STD) +#if !defined(NO_STD) for (Participant* participant : ParticipantRegistry::participants) { if (participant == nullptr) continue; @@ -148,7 +158,7 @@ Participant* ParticipantRegistry::Get(unsigned char participantId) { return participant; } std::cout << "Could not find participant " << (int)participantId << std::endl; -#endif +#endif return nullptr; } @@ -165,19 +175,22 @@ void ParticipantRegistry::Add(Participant* participant) { if (foundParticipant == nullptr) { #if defined(NO_STD) - //this->things[this->thingCount++] = thing; + // 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"; + // 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"; } } diff --git a/Participant.h b/Participant.h index 2be1323..ea1cdcf 100644 --- a/Participant.h +++ b/Participant.h @@ -1,4 +1,5 @@ #pragma once + #include "Thing.h" namespace RoboidControl { @@ -36,6 +37,7 @@ class ParticipantRegistry { public: Participant** GetAll() const; int count = 0; + private: Participant** participants; #else @@ -43,6 +45,7 @@ class ParticipantRegistry { /// @brief Get all participants /// @return All participants const std::list& GetAll() const; + private: /// @brief The list of known participants std::list participants; @@ -57,6 +60,9 @@ class ParticipantRegistry { /// reference to remote participants. class Participant { public: + /// @brief The name of the participant + const char* name = "Participant"; + /// @brief The Ip Address of a participant. const char* ipAddress = "0.0.0.0"; /// @brief The port number for UDP communication with the participant. @@ -65,6 +71,7 @@ class Participant { /// @brief The network Id to identify the participant unsigned char networkId = 0; + Participant(); /// @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 @@ -72,7 +79,10 @@ class Participant { /// @brief Destructor for the participant ~Participant(); - static Participant& LocalParticipant(); + static Participant* LocalParticipant; + static void ReplaceLocalParticipant(Participant& newParticipant); + + Thing* root = new Thing(this); public: #if defined(NO_STD) diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 5aa4142..2ec2f88 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -1,5 +1,6 @@ #include "ParticipantUDP.h" +#include "Participant.h" #include "Thing.h" #include "Arduino/ArduinoParticipant.h" @@ -11,20 +12,38 @@ namespace RoboidControl { +#pragma region Init + ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) { + this->name = "ParticipantUDP"; this->remoteSite = nullptr; if (this->port == 0) this->isIsolated = true; Participant::registry.Add(this); + + this->root = Thing::LocalRoot(); //::LocalParticipant->root; + this->root->owner = this; + this->root->name = "UDP Root"; + this->Add(this->root); + + Participant::ReplaceLocalParticipant(*this); } ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) : Participant("127.0.0.1", localPort) { + this->name = "ParticipantUDP"; if (this->port == 0) this->isIsolated = true; else this->remoteSite = new Participant(ipAddress, port); Participant::registry.Add(this); + + this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; + this->root->owner = this; + this->root->name = "UDP Root"; + this->Add(this->root); + + Participant::ReplaceLocalParticipant(*this); } static ParticipantUDP* isolatedParticipant = nullptr; @@ -64,11 +83,19 @@ void ParticipantUDP::SetupUDP(int localPort, this->connected = true; } +#pragma endregion Init + #pragma region Update +// The update order +// 1. receive external messages +// 2. update the state +// 3. send out the updated messages void ParticipantUDP::Update() { unsigned long currentTimeMs = Thing::GetTimeMs(); + PrepMyThings(); + if (this->isIsolated == false) { if (this->connected == false) begin(); @@ -91,11 +118,22 @@ void ParticipantUDP::Update() { UpdateOtherThings(); } +void ParticipantUDP::PrepMyThings() { + for (Thing* thing : this->things) { + if (thing == nullptr) + continue; + + thing->PrepareForUpdate(); + } +} + void ParticipantUDP::UpdateMyThings() { + // std::cout << this->things.size() << std::endl; for (Thing* thing : this->things) { if (thing == nullptr) // || thing->GetParent() != nullptr) continue; + // std::cout << thing->name << "\n"; if (thing->hierarchyChanged) { if (!(this->isIsolated || this->networkId == 0)) { ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); @@ -110,12 +148,14 @@ void ParticipantUDP::UpdateMyThings() { } } + // std::cout << "B\n"; // 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! thing->Update(false); + // std::cout << "C\n"; if (!(this->isIsolated || this->networkId == 0)) { if (thing->terminate) { DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); @@ -136,8 +176,10 @@ void ParticipantUDP::UpdateMyThings() { delete binaryMsg; } } + // std::cout << "D\n"; if (thing->terminate) this->Remove(thing); + // std::cout << "E\n"; } } @@ -200,8 +242,8 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { if (bufferSize <= 0) return true; - // std::cout << "send msg " << (int)this->buffer[0] << " to " - // << remoteParticipant->ipAddress << std::endl; + // std::cout << "send msg " << (static_cast(this->buffer[0]) & 0xff) + // << " to " << remoteParticipant->ipAddress << std::endl; #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = @@ -378,14 +420,14 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) { #if defined(DEBUG) - std::cout << this->name << ": process SiteMsg " << (int)this->networkId + std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId << " -> " << (int)msg->networkId << "\n"; #endif if (this->networkId != msg->networkId) { this->networkId = msg->networkId; - // std::cout << this->things.size() << " things\n"; + std::cout << this->things.size() << " things\n"; for (Thing* thing : this->things) this->SendThingInfo(sender, thing); } diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index c51131d..bf5e1a2 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -78,9 +78,6 @@ class ParticipantUDP : public Participant { /// local network long publishInterval = 3000; // 3 seconds - /// @brief The name of the participant - const char* name = "ParticipantUDP"; - protected: char buffer[1024]; @@ -105,6 +102,8 @@ class ParticipantUDP : public Participant { protected: unsigned long nextPublishMe = 0; + /// @brief Prepare the local things for the next update + virtual void PrepMyThings(); virtual void UpdateMyThings(); virtual void UpdateOtherThings(); diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 394f328..9eb7ce9 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -73,10 +73,13 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing* thing = sender->Get(msg->thingId); if (thing == nullptr) // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); - Thing::Reconstruct(sender, msg->thingType, msg->thingId); + // Thing::Reconstruct(sender, msg->thingType, msg->thingId); + //thing = new Thing(msg->thingType, sender->root); + ; + thing->id = msg->thingId; if (msg->parentId != 0) { - thing->SetParent(*Get(msg->parentId)); + thing->SetParent(Get(msg->parentId)); if (thing->IsRoot()) // if (thing->GetParent() != nullptr) #if defined(NO_STD) diff --git a/Thing.cpp b/Thing.cpp index 2ec0708..1ec24b3 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -20,12 +20,31 @@ namespace RoboidControl { #pragma region Init -Thing& Thing::LocalRoot() { - static Thing localRoot(Thing::Type::Undetermined, localRoot); +Thing* Thing::LocalRoot() { + Participant* p = Participant::LocalParticipant; + Thing* localRoot = p->root; return localRoot; } -Thing::Thing(unsigned char thingType, Thing& parent) { +// Only use this for root things +Thing::Thing(Participant* owner) { + this->type = Type::Roboid; // should become root + + this->position = Spherical::zero; + this->positionUpdated = true; + this->orientation = SwingTwist::identity; + this->orientationUpdated = true; + this->hierarchyChanged = true; + + this->linearVelocity = Spherical::zero; + this->angularVelocity = Spherical::zero; + + this->owner = owner; + //this->owner->Add(this, true); + std::cout << this->owner->name << ": New root thing " << std::endl; +} + +Thing::Thing(unsigned char thingType, Thing* parent) { this->type = thingType; this->position = Spherical::zero; @@ -37,9 +56,16 @@ Thing::Thing(unsigned char thingType, Thing& parent) { this->linearVelocity = Spherical::zero; this->angularVelocity = Spherical::zero; - this->owner = &Participant::LocalParticipant(); + this->owner = parent->owner; this->owner->Add(this, true); this->SetParent(parent); + + std::cout << this->owner->name << ": New thing for " << parent->name + << std::endl; +} + +Thing::~Thing() { + std::cout << "Destroy thing " << this->name << std::endl; } // Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, @@ -66,28 +92,28 @@ void Thing::SetModel(const char* url) { #pragma region Hierarchy -// void Thing::SetParent(Thing* parent) { -// if (parent == nullptr) { -// Thing* parentThing = this->parent; -// if (parentThing != nullptr) -// parentThing->RemoveChild(this); -// this->parent = nullptr; -// } else -// parent->AddChild(this); -// this->hierarchyChanged = true; -// } - -void Thing::SetParent(Thing& parent) { - parent.AddChild(this); +void Thing::SetParent(Thing* parent) { + if (parent == nullptr) { + Thing* parentThing = this->parent; + if (parentThing != nullptr) + parentThing->RemoveChild(this); + this->parent = nullptr; + } else + parent->AddChild(this); this->hierarchyChanged = true; } -const Thing& Thing::GetParent() { - return *this->parent; -} +// void Thing::SetParent(Thing* parent) { +// parent->AddChild(this); +// this->hierarchyChanged = true; +// } + +// const Thing& Thing::GetParent() { +// return *this->parent; +// } bool Thing::IsRoot() const { - return this == &LocalRoot(); //&Thing::Root; + return this == LocalRoot() || this->parent == nullptr; //&Thing::Root; } // void Thing::SetParent(Thing* root, const char* name) { @@ -96,9 +122,9 @@ bool Thing::IsRoot() const { // this->SetParent(thing); // } -// Thing* Thing::GetParent() { -// return this->parent; -// } +Thing* Thing::GetParent() { + return this->parent; +} Thing* Thing::GetChildByIndex(unsigned char ix) { return this->children[ix]; @@ -110,11 +136,11 @@ void Thing::AddChild(Thing* child) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { newChildren[childIx] = this->children[childIx]; - // if (this->children[childIx] == child) { - // // child is already present, stop copying do not update the children - // delete[] newChildren; - // return; - // } + if (this->children[childIx] == child) { + // child is already present, stop copying do not update the children + delete[] newChildren; + return; + } } newChildren[this->childCount] = child; @@ -143,7 +169,7 @@ Thing* Thing::RemoveChild(Thing* child) { } } - child->parent = &Thing::LocalRoot(); + child->parent = Thing::LocalRoot(); delete[] this->children; this->children = newChildren; @@ -248,6 +274,8 @@ unsigned long Thing::GetTimeMs() { // Update(GetTimeMs(), recursive); // } +void Thing::PrepareForUpdate() {} + void Thing::Update(bool recursive) { // if (this->positionUpdated || this->orientationUpdated) // OnPoseChanged callback diff --git a/Thing.h b/Thing.h index 2fca59c..439606c 100644 --- a/Thing.h +++ b/Thing.h @@ -41,8 +41,15 @@ class Thing { }; #pragma region Init - static Thing& LocalRoot(); + static Thing* LocalRoot(); + private: + // Special constructor to create a root thing + Thing(Participant* parent); + // Which can only be used by the Participant + friend class Participant; + + public: /// @brief Create a new thing /// @param thingType The type of thing (can use Thing::Type) /// @param parent (optional) The parent thing @@ -50,7 +57,7 @@ class Thing { /// be Participant::LocalParticipant if the parent is not specified. A thing /// without a parent will be a root thing. Thing(unsigned char thingType = Thing::Type::Undetermined, - Thing& parent = LocalRoot()); + Thing* parent = LocalRoot()); /// @brief Create a new child thing /// @param parent The parent thing @@ -59,6 +66,8 @@ class Thing { /// an ID /// @note The owner will be the same as the owner of the parent thing + ~Thing(); + static Thing Reconstruct(Participant* owner, unsigned char thingType, unsigned char thingId); @@ -73,6 +82,7 @@ class Thing { /// @brief The participant managing this thing Participant* owner = nullptr; + /// @brief The ID of the thing unsigned char id = 0; @@ -108,11 +118,11 @@ class Thing { /// @brief Sets the parent of this Thing /// @param parent The Thing which should become the parent // virtual void SetParent(Thing* parent); - void SetParent(Thing& parent); + void SetParent(Thing* parent); /// @brief Gets the parent of this Thing /// @return The parent Thing // Thing* GetParent(); - const Thing& GetParent(); + Thing* GetParent(); bool IsRoot() const; @@ -215,12 +225,7 @@ class Thing { #pragma region Update public: - /// @brief Get the current time in milliseconds - /// @return The current time in milliseconds - static unsigned long GetTimeMs(); - - /// @brief Updates the state of the thing - // void Update(bool recursive = false); + virtual void PrepareForUpdate(); /// @brief Updates the state of the thing /// @param currentTimeMs The current clock time in milliseconds; if this is @@ -230,6 +235,10 @@ class Thing { static void UpdateThings(); + /// @brief Get the current time in milliseconds + /// @return The current time in milliseconds + static unsigned long GetTimeMs(); + #pragma endregion Update public: diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp index 9cb2107..1eb1417 100644 --- a/Things/ControlledMotor.cpp +++ b/Things/ControlledMotor.cpp @@ -2,9 +2,9 @@ namespace RoboidControl { -ControlledMotor::ControlledMotor(Motor& motor, - RelativeEncoder& encoder, - Thing& parent) +ControlledMotor::ControlledMotor(Motor* motor, + RelativeEncoder* encoder, + Thing* parent) : Motor(parent), motor(motor), encoder(encoder) { this->type = Type::ControlledMotor; // encoder.SetParent(*this); @@ -19,9 +19,9 @@ void ControlledMotor::SetTargetVelocity(float velocity) { } void ControlledMotor::Update(bool recurse) { - encoder.Update(false); + encoder->Update(false); - this->actualVelocity = (int)rotationDirection * encoder.rotationSpeed; + this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed; unsigned long currentTimeMs = GetTimeMs(); float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f; @@ -31,10 +31,10 @@ void ControlledMotor::Update(bool recurse) { float acceleration = error * timeStep * pidP; // Just P is used at this moment std::cout << "motor acc. " << acceleration << std::endl; - motor.SetTargetVelocity(targetVelocity + + motor->SetTargetVelocity(targetVelocity + acceleration); // or something like that - motor.Update(false); + motor->Update(false); } // float ControlledMotor::GetActualVelocity() { diff --git a/Things/ControlledMotor.h b/Things/ControlledMotor.h index 1f46aa2..462f206 100644 --- a/Things/ControlledMotor.h +++ b/Things/ControlledMotor.h @@ -10,7 +10,7 @@ namespace RoboidControl { /// The speed is measured in revolutions per second. class ControlledMotor : public Motor { public: - ControlledMotor(Motor& motor, RelativeEncoder& encoder, Thing& parent = Thing::LocalRoot()); + ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot()); float pidP = 1; float pidD = 0; @@ -28,8 +28,8 @@ class ControlledMotor : public Motor { /// @param speed the target velocity in revolutions per second virtual void SetTargetVelocity(float velocity) override; - Motor& motor; - RelativeEncoder& encoder; + Motor* motor; + RelativeEncoder* encoder; protected: float lastUpdateTime; diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index bde68c7..14967ae 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -1,11 +1,27 @@ #include "DifferentialDrive.h" +#include "Messages/LowLevelMessages.h" + namespace RoboidControl { -DifferentialDrive::DifferentialDrive(Thing& parent) +DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(Type::DifferentialDrive, parent) { - this->leftWheel = new Motor(); - this->rightWheel = new Motor(); + this->name = "Differential drive"; + + this->leftWheel = new Motor(this); + this->leftWheel->name = "Left motor"; + + this->rightWheel = new Motor(this); + this->rightWheel->name = "Right motor"; +} + +DifferentialDrive::DifferentialDrive(Motor* leftMotor, + Motor* rightMotor, + Thing* parent) + : Thing(Type::DifferentialDrive, parent) { + this->name = "Differential drive"; + this->leftWheel = leftMotor; + this->rightWheel = rightMotor; } void DifferentialDrive::SetDriveDimensions(float wheelDiameter, @@ -23,13 +39,13 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter, this->rightWheel->SetPosition(Spherical(distance, Direction::right)); } -Motor& DifferentialDrive::GetMotorLeft() { - return *this->leftWheel; -} +// Motor& DifferentialDrive::GetMotorLeft() { +// return *this->leftWheel; +// } -Motor& DifferentialDrive::GetMotorRight() { - return *this->rightWheel; -} +// Motor& DifferentialDrive::GetMotorRight() { +// return *this->rightWheel; +// } void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) { float distance = this->wheelSeparation / 2; @@ -78,4 +94,11 @@ void DifferentialDrive::Update(bool recursive) { Thing::Update(recursive); } +int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) { + data[(*ix)++] = this->leftWheel->id; + data[(*ix)++] = this->rightWheel->id; + LowLevelMessages::SendFloat16(data, ix, this->wheelRadius); + return 4; +} + } // namespace RoboidControl \ No newline at end of file diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index c7685fe..cb45977 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -14,7 +14,9 @@ class DifferentialDrive : public Thing { /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DifferentialDrive(Thing& parent = Thing::LocalRoot()); + DifferentialDrive(Thing* parent = Thing::LocalRoot()); + + DifferentialDrive(Motor* leftMotor, Motor* rightMotor, Thing* parent = Thing::LocalRoot()); /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters @@ -25,8 +27,8 @@ class DifferentialDrive : public Thing { /// @sa SetLinearVelocity SetAngularVelocity void SetDriveDimensions(float wheelDiameter, float wheelSeparation); - Motor& GetMotorLeft(); - Motor& GetMotorRight(); + // Motor& GetMotorLeft(); + // Motor& GetMotorRight(); /// @brief Congures the motors for the wheels /// @param leftWheel The motor for the left wheel /// @param rightWheel The motor for the right wheel @@ -42,20 +44,23 @@ class DifferentialDrive : public Thing { /// @copydoc RoboidControl::Thing::Update(unsigned long) virtual void Update(bool recursive = true) override; + int GenerateBinary(char* bytes, unsigned char* ix) override; + // virtual void ProcessBinary(char* bytes) override; + + /// @brief The left wheel + Motor* leftWheel = nullptr; + /// @brief The right wheel + Motor* rightWheel = nullptr; protected: /// @brief The radius of a wheel in meters - float wheelRadius = 1.0f; + float wheelRadius = 0.0f; /// @brief The distance between the wheels in meters - float wheelSeparation = 1.0f; + float wheelSeparation = 0.0f; /// @brief Convert revolutions per second to meters per second float rpsToMs = 1.0f; - /// @brief The left wheel - Motor* leftWheel = nullptr; - /// @brief The right wheel - Motor* rightWheel = nullptr; }; } // namespace RoboidControl \ No newline at end of file diff --git a/Things/DigitalSensor.cpp b/Things/DigitalSensor.cpp index 0c1e213..ba0d859 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -13,7 +13,7 @@ namespace RoboidControl { // DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {} // DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {} -DigitalSensor::DigitalSensor(Thing& parent) : Thing(Type::Switch, parent) {} +DigitalSensor::DigitalSensor(Thing* parent) : Thing(Type::Switch, parent) {} int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) { bytes[(*ix)++] = state ? 1 : 0; diff --git a/Things/DigitalSensor.h b/Things/DigitalSensor.h index b6579f2..e31e87d 100644 --- a/Things/DigitalSensor.h +++ b/Things/DigitalSensor.h @@ -19,7 +19,7 @@ class DigitalSensor : public Thing { /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID // DigitalSensor(Thing* parent); //, unsigned char thingId = 0); - DigitalSensor(Thing& parent = Thing::LocalRoot()); + DigitalSensor(Thing* parent = Thing::LocalRoot()); /// @brief The sigital state bool state = 0; diff --git a/Things/Motor.cpp b/Things/Motor.cpp index 354c7bd..12ab226 100644 --- a/Things/Motor.cpp +++ b/Things/Motor.cpp @@ -2,16 +2,15 @@ namespace RoboidControl { -// RoboidControl::Motor::Motor(Participant* owner) -// : Thing(owner, Type::UncontrolledMotor) {} +Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {} -// RoboidControl::Motor::Motor(Thing* parent) -// : Thing(parent, Type::UncontrolledMotor) {} - -Motor::Motor(Thing& parent) : Thing(Type::UncontrolledMotor, parent) {} - -void RoboidControl::Motor::SetTargetVelocity(float targetSpeed) { +void Motor::SetTargetVelocity(float targetSpeed) { this->targetVelocity = targetSpeed; } +int Motor::GenerateBinary(char* data, unsigned char* ix) { + data[(*ix)++] = this->targetVelocity * 127.0f; + return 1; +} + } // namespace RoboidControl \ No newline at end of file diff --git a/Things/Motor.h b/Things/Motor.h index a625bb3..38ac149 100644 --- a/Things/Motor.h +++ b/Things/Motor.h @@ -6,18 +6,19 @@ namespace RoboidControl { class Motor : public Thing { public: - Motor(Participant* owner); - // Motor(Thing* parent); - Motor(Thing& parent = Thing::LocalRoot()); + Motor(Thing* parent = Thing::LocalRoot()); /// @brief Motor turning direction enum class Direction { Clockwise = 1, CounterClockwise = -1 }; /// @brief The forward turning direction of the motor Direction direction; - virtual void SetTargetVelocity(float velocity); // -1..0..1 + virtual void SetTargetVelocity(float velocity); // -1..0..1 - protected: + int GenerateBinary(char* bytes, unsigned char* ix) override; + // virtual void ProcessBinary(char* bytes) override; + + //protected: float targetVelocity = 0; }; diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp index 5cc9464..6e6a19c 100644 --- a/Things/RelativeEncoder.cpp +++ b/Things/RelativeEncoder.cpp @@ -2,7 +2,7 @@ namespace RoboidControl { -RelativeEncoder::RelativeEncoder(Thing& parent) +RelativeEncoder::RelativeEncoder(Thing* parent) : Thing(Type::IncrementalEncoder, parent) {} float RelativeEncoder::GetRotationSpeed() { diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h index e783c7d..b2aeda4 100644 --- a/Things/RelativeEncoder.h +++ b/Things/RelativeEncoder.h @@ -16,7 +16,7 @@ class RelativeEncoder : public Thing { /// rotation RelativeEncoder(Participant* owner); // RelativeEncoder(Thing* parent); - RelativeEncoder(Thing& parent = Thing::LocalRoot()); + RelativeEncoder(Thing* parent = Thing::LocalRoot()); /// @brief Get the rotation speed /// @return The speed in revolutions per second diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index a3fb13d..d67800b 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -10,7 +10,7 @@ namespace RoboidControl { // TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {} -TemperatureSensor::TemperatureSensor(Thing& parent) : Thing(Type::TemperatureSensor, parent) {} +TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {} // TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {} diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index b1feac3..0da4210 100644 --- a/Things/TemperatureSensor.h +++ b/Things/TemperatureSensor.h @@ -14,7 +14,7 @@ class TemperatureSensor : public Thing { /// @param thingId The ID of the thing TemperatureSensor(Participant* participant); //, unsigned char thingId); // TemperatureSensor(Thing* parent); - TemperatureSensor(Thing& parent = Thing::LocalRoot()); + TemperatureSensor(Thing* parent = Thing::LocalRoot()); /// @brief The measured temperature float temperature = 0; diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index 6960b45..d1b9b41 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -2,15 +2,29 @@ namespace RoboidControl { -TouchSensor::TouchSensor(Thing& parent) : Thing(Type::TouchSensor, parent) {} +TouchSensor::TouchSensor(Thing* parent) : Thing(Type::TouchSensor, parent) { + this->name = "Touch sensor"; +} + +void TouchSensor::PrepareForUpdate() { + this->touchedSomething = this->externalTouch; +} + +void TouchSensor::Update(bool recursive) { + Thing::Update(recursive); +} int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { - bytes[(*ix)++] = touchedSomething ? 1 : 0; + bytes[(*ix)++] = this->touchedSomething ? 1 : 0; return 1; } void TouchSensor::ProcessBinary(char* bytes) { - this->touchedSomething |= (bytes[0] == 1); + this->externalTouch = (bytes[0] == 1); + if (this->externalTouch) + std::cout << "touching!\n"; + else + std::cout << "not touching\n"; } } // namespace RoboidControl \ No newline at end of file diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index 0768f66..cd7bd73 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -12,12 +12,15 @@ class TouchSensor : public Thing { /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - TouchSensor(Thing& parent = Thing::LocalRoot()); + TouchSensor(Thing* parent = Thing::LocalRoot()); /// @brief Value which is true when the sensor is touching something, false /// otherwise bool touchedSomething = false; + virtual void PrepareForUpdate() override; + virtual void Update(bool recursive) override; + /// @brief Function used to generate binary data for this touch sensor /// @param buffer The byte array for thw binary data /// @param ix The starting position for writing the binary data @@ -25,6 +28,8 @@ class TouchSensor : public Thing { /// @brief Function used to process binary data received for this touch sensor /// @param bytes The binary data to process virtual void ProcessBinary(char* bytes) override; +protected: + bool externalTouch = false; }; } // namespace RoboidControl From 86200232774ddcbd8d6f2af7843ca3962bfca922 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 23 May 2025 17:57:14 +0200 Subject: [PATCH 32/56] Working controlled motor --- Arduino/Things/DRV8833.h | 2 +- Arduino/Things/DigitalInput.cpp | 18 ++++++++++----- Arduino/Things/UltrasonicSensor.cpp | 4 ++-- Arduino/Things/UltrasonicSensor.h | 4 ++-- Things/ControlledMotor.cpp | 36 ++++++++++++++++++++--------- Things/ControlledMotor.h | 6 +++-- Things/RelativeEncoder.h | 4 ++-- 7 files changed, 48 insertions(+), 26 deletions(-) diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index f2c2d3f..9762980 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -18,7 +18,7 @@ class DRV8833 : public Thing { int AIn2; int BIn1; int BIn2; - int standby; + int standby = 255; }; /// @brief Setup a DRV8833 motor controller diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index 12f3b12..ef41ad9 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -7,7 +7,8 @@ namespace Arduino { #pragma region Digital input -DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(Type::Undetermined, parent) { +DigitalInput::DigitalInput(unsigned char pin, Thing* parent) + : Thing(Type::Undetermined, parent) { this->pin = pin; pinMode(this->pin, INPUT); std::cout << "digital input start\n"; @@ -46,13 +47,15 @@ DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, void DigitalInput::RelativeEncoder::Start() { // We support up to 2 pulse counters - if (interruptCount == 0) + if (interruptCount == 0) { + std::cout << "pin interrupt 1 activated" << std::endl; attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0, RISING); - else if (interruptCount == 1) + } else if (interruptCount == 1) { + std::cout << "pin interrupt 2 activated" << std::endl; attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1, RISING); - else { + } else { // maximum interrupt count reached std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached" << std::endl; @@ -85,14 +88,17 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) { } float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f; + if (timeStep <= 0) + return; + int pulseCount = GetPulseCount(); netPulseCount += pulseCount; this->pulseFrequency = pulseCount / timeStep; this->rotationSpeed = pulseFrequency / pulsesPerRevolution; - // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency - // << " timestep " << timeStep << std::endl; + std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency + << " timestep " << timeStep << std::endl; this->lastUpdateTime = currentTimeMs; } diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index d92da44..2568070 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -9,8 +9,8 @@ namespace Arduino { UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) { this->name = "Ultrasonic sensor"; - this->pinTrigger = config.triggerPin; - this->pinEcho = config.echoPin; + this->pinTrigger = config.trigger; + this->pinEcho = config.echo; pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode pinMode(pinEcho, INPUT); // configure the echo pin to input mode diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index 7c02778..8e72887 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -9,8 +9,8 @@ namespace Arduino { class UltrasonicSensor : Thing { public: struct Configuration { - int triggerPin; - int echoPin; + int trigger; + int echo; }; UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot()); diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp index 1eb1417..26b16bc 100644 --- a/Things/ControlledMotor.cpp +++ b/Things/ControlledMotor.cpp @@ -1,4 +1,5 @@ #include "ControlledMotor.h" +#include "LinearAlgebra/FloatSingle.h" namespace RoboidControl { @@ -7,9 +8,10 @@ ControlledMotor::ControlledMotor(Motor* motor, Thing* parent) : Motor(parent), motor(motor), encoder(encoder) { this->type = Type::ControlledMotor; - // encoder.SetParent(*this); + //encoder->SetParent(null); // Thing parent = motor.GetParent(); // this->SetParent(parent); + this->integral = 0; } void ControlledMotor::SetTargetVelocity(float velocity) { @@ -19,22 +21,34 @@ void ControlledMotor::SetTargetVelocity(float velocity) { } void ControlledMotor::Update(bool recurse) { - encoder->Update(false); - - this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed; - unsigned long currentTimeMs = GetTimeMs(); float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f; this->lastUpdateTime = currentTimeMs; + if (timeStep <= 0) + return; + + // encoder->Update(false); + + this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed; float error = this->targetVelocity - this->actualVelocity; - float acceleration = - error * timeStep * pidP; // Just P is used at this moment - std::cout << "motor acc. " << acceleration << std::endl; - motor->SetTargetVelocity(targetVelocity + - acceleration); // or something like that + float p_term = error * pidP; + this->integral += error * timeStep; + float i_term = pidI * this->integral; + float d_term = pidD * (error - this->lastError) / timeStep; + this->lastError = error; - motor->Update(false); + float output = p_term + i_term + d_term; + std::cout << "target " << this->targetVelocity << " actual " + << this->actualVelocity << " output = " << output << std::endl; + // float acceleration = + // error * timeStep * pidP; // Just P is used at this moment + // std::cout << "motor acc. " << acceleration << std::endl; + + // float newTargetVelocity = motor->targetVelocity + acceleration; + output = LinearAlgebra::Float::Clamp(output, -1, 1); + motor->SetTargetVelocity(output); // or something like that + //motor->Update(false); } // float ControlledMotor::GetActualVelocity() { diff --git a/Things/ControlledMotor.h b/Things/ControlledMotor.h index 462f206..cfb0229 100644 --- a/Things/ControlledMotor.h +++ b/Things/ControlledMotor.h @@ -12,9 +12,9 @@ class ControlledMotor : public Motor { public: ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot()); - float pidP = 1; + float pidP = 0.5; float pidD = 0; - float pidI = 0; + float pidI = 0.2; /// @brief The actual velocity in revolutions per second float actualVelocity; @@ -32,6 +32,8 @@ class ControlledMotor : public Motor { RelativeEncoder* encoder; protected: + float integral = 0; + float lastError = 0; float lastUpdateTime; }; diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h index b2aeda4..350d8a5 100644 --- a/Things/RelativeEncoder.h +++ b/Things/RelativeEncoder.h @@ -14,14 +14,14 @@ class RelativeEncoder : public Thing { /// full rotation /// @param distancePerRevolution The distance a wheel travels per full /// rotation - RelativeEncoder(Participant* owner); + //RelativeEncoder(Participant* owner); // RelativeEncoder(Thing* parent); RelativeEncoder(Thing* parent = Thing::LocalRoot()); /// @brief Get the rotation speed /// @return The speed in revolutions per second virtual float GetRotationSpeed(); - float rotationSpeed; + float rotationSpeed = 0; /// @brief Function used to generate binary data for this touch sensor /// @param buffer The byte array for thw binary data From 6e860be9135ba6a19ef2b6b723cafd353e80f6a4 Mon Sep 17 00:00:00 2001 From: pascal Date: Mon, 26 May 2025 09:22:59 +0200 Subject: [PATCH 33/56] Updated BB2B example --- examples/BB2B.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/BB2B.cpp b/examples/BB2B.cpp index a07aa9f..b1ab231 100644 --- a/examples/BB2B.cpp +++ b/examples/BB2B.cpp @@ -19,9 +19,9 @@ int main() { // The robot's propulsion is a differential drive DifferentialDrive bb2b = DifferentialDrive(); // Is has a touch sensor at the front left of the roboid - TouchSensor touchLeft = TouchSensor(bb2b); + TouchSensor touchLeft = TouchSensor(&bb2b); // and other one on the right - TouchSensor touchRight = TouchSensor(bb2b); + TouchSensor touchRight = TouchSensor(&bb2b); // Do forever: while (true) { From 0920f99c1ee3e464ef39aa29c47a59a1d5f58fc1 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 26 May 2025 12:27:09 +0200 Subject: [PATCH 34/56] Fix tests --- LinearAlgebra/test/Polar_test.cc | 1 + LinearAlgebra/test/Spherical16_test.cc | 1 + LinearAlgebra/test/SphericalSingle_test.cc | 1 + test/participant_test.cc | 2 +- test/thing_test.cc | 2 +- 5 files changed, 5 insertions(+), 2 deletions(-) diff --git a/LinearAlgebra/test/Polar_test.cc b/LinearAlgebra/test/Polar_test.cc index 9e244f2..ba65946 100644 --- a/LinearAlgebra/test/Polar_test.cc +++ b/LinearAlgebra/test/Polar_test.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include "Polar.h" #include "Spherical.h" diff --git a/LinearAlgebra/test/Spherical16_test.cc b/LinearAlgebra/test/Spherical16_test.cc index e9fbc40..8eea700 100644 --- a/LinearAlgebra/test/Spherical16_test.cc +++ b/LinearAlgebra/test/Spherical16_test.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include "Spherical.h" #include "Vector3.h" diff --git a/LinearAlgebra/test/SphericalSingle_test.cc b/LinearAlgebra/test/SphericalSingle_test.cc index 7840d77..3dedb31 100644 --- a/LinearAlgebra/test/SphericalSingle_test.cc +++ b/LinearAlgebra/test/SphericalSingle_test.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include "Spherical.h" diff --git a/test/participant_test.cc b/test/participant_test.cc index 0b4e147..13831bd 100644 --- a/test/participant_test.cc +++ b/test/participant_test.cc @@ -59,7 +59,7 @@ TEST(Participant, SiteParticipant) { TEST(Participant, ThingMsg) { SiteServer* siteServer = new SiteServer(7681); ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682); - Thing* thing = new Thing(participant); + Thing* thing = new Thing(); thing->SetName("First Thing"); thing->SetModel("https://passer.life/extras/ant.jpg"); diff --git a/test/thing_test.cc b/test/thing_test.cc index 74ff48c..ff651c3 100644 --- a/test/thing_test.cc +++ b/test/thing_test.cc @@ -24,7 +24,7 @@ TEST(RoboidControlSuite, HiddenParticipant) { TEST(RoboidControlSuite, IsolatedParticipant) { ParticipantUDP* participant = ParticipantUDP::Isolated(); - Thing* thing = new Thing(participant); + Thing* thing = new Thing(); unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds; From e50c8eb9c89c7fc801ed2c543a1d3970011efb2c Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 26 May 2025 15:26:44 +0200 Subject: [PATCH 35/56] Squashed 'LinearAlgebra/' changes from bfa1123..8b6f628 8b6f628 Fix Matrix unit test 28a3064 Fix tests 91f4280 Reduced the use of raw pointers 83539df Fixes to make it work againg, but failing at udp.begin() 3cca327 Optimizations 361807c Performance improvements 7bd83fa Processomg Acc data * propagating 0eb2851 Linking works 06ff5e9 Added ESP-IDF Wifi/UDP support (but it still give linker errors) 7461a1f Implemented integrate function b7f102a Fix matrix2 tests 7d2dd08 First steps implementation Matrix operations f05b411 Cleanup 05a3b09 Update namespace, Windows compatibility 95a248a First udp communcation is working fea15c3 removed namespace Passer 753de2d Fixes 9674f68 Disabled test failing on MacOS 5b89234 Made it work on MacOS d0ba29e Updated LinearAlgebra ebfb4fb Merge commit '0b63a044eb84175cc922f3f1fe0ce039d101bf7b' 0b63a04 Updated documentation, cleanup a0082bd Add direction documentation 770ce32 Merge commit '2b5f5cd175b20fa01e04b696dfe0cfbe2c4ad9da' 2b5f5cd Removed shorthand Deg/Rad aliases... (see below) d212db2 Trying to fix specialization before instantiation error e445e5b Trying to fix specialization before instantiation error f47e504 Trying to fix specialization before instantiation error b877d4d Trying to fix specialization before instantiation error 06e8623 Trying to fix specialization before instantiation error e97aee9 Trying to fix specialization before instantiation error 0e00e5d Merge branch 'main' of http://gitlab.passervr.com/passer/cpp/linear-algebra 52de55d Trying to fix specialization before instantiation error 47ba024 Trying to fix specialization before instantiation error 2402030 Trying to fix specialization before instantiation error 4e3f253 Fixed gitignore 06c70e2 Merge branch 'Experimental' 6f30334 Updated PolarOf, removed default Polar type 94a3105 Cleanup 89a5711 Fixed typo in documentation 64b7f11 Removed default Angle type 58beb36 Completed AngleOf documentation 5f58a2c Updated documentation 97d937e Fixed (copilot) unit tests 585a3b0 Removed default Spherical type d3226fd Added copilot test documentation a7aa679 Added a performance test 2c57c3f Fixed direction for spherical 28ee225 Normalize Spherical at creation 800eb75 improved rounding of discrete angles 536d6ce Add Binary support 18756ba Correct axis on quaternion from swingtwist 06e42c5 Normalized swing twist edbb4b1 Removes Angle::deg90/180 because of issues 0a07b23 SwingTwist == and Angle 9406b57 SwingTwist::AngleAxis using Direction 06da9e5 Removed AngleAxis, Sperhical will be used instead 2e61e1b Fix unit test 54d0318 Extended unit tests (plus fixes) 8286c1c Minor improvements 95a6fb3 Improved Angle quality 9eca318 Fixed inverse 0ebfce4 Spherical direction 136e44e Fixed unit tests fb5cee8 Proper angle implementation (unit tests still fail) 92d5ef0 Normalizing directions 1ea65d5 Made toQuestion const f1c70c7 Fix unit tests 2ad6a5a Fix include 11259a9 Extend functionality 3363388 Extended AngleAxis & Direction 09e25a8 Add Asin and Atan a646e93 Add ACos 407e771 Add WithDistance 69bad8f Added degrees, radians, cos, sin, and tan functions 5c3dfa6 Adde degrees/readians and poc ACos 05c61a3 Add distance between 6a1d043 Added Angle comparison 6b3bcfc Another anglebetween fix b5a6330 Fix anglebetween b975aed Added angle and rotate functions f483439 Moved Polar/Spherical to template class completely e0ef43a Merge branch 'Experimental' of http://gitlab.passervr.com/passer/cpp/linear-algebra into Experimental b460bec Added scaling to SphericalOf e10e010 Added Direction to library sources dedaa31 Fix unit tests 1da93ca Fix ToVector3 9c3503f Added SphericalOf 353cb1b Add conversion from Quaternion cf86ba8 Cleanup template classes ea6894e Fix generic template functions 1ce7234 Bug fixes 38ebb34 Added Direction to replace Axis c72bba4 Change Vector3::Angle return type to AngleOf 18cf653 Added scaling support 51772a1 Added add/subtract for discrete angles 49c6740 Make Angle -> float conversion explicit 608c45c Fix precision error 4591aef Added Spherical16 48828e2 Fix int/float issues 86b7c21 Added Degrees inline function a4b0491 Fix unit tests b81b77b Extend AngleOf support c70c079 Add spherical subtract 2bad384 Added spherical addition a25a8be Fixed namespace issues f8009a7 Updated namespace 91c7b20 Fix gitlab test runner 78468e3 Renamed VectorAlgebra project to LinearAlgebra 47a6368 Fix Vector3 ee62cba XYZ deprecation 62d7439 Extend ulitity functions 95713c8 Cleanup, added utility functions f7d9d97 Cleanup 66e47d7 Cleanup Vector2 and Polar e922a01 fix == operator 5693097 Initial subtractop test c411b43 Textual improvement 0c54276 equality support b1e34b6 Improved unit tests 6ddb074 Improved unit tests 5489b3c Fix test 3d971c1 Improve namespace 791bd78 namespace improvement 9bea94f Use Angle type for Polar 8a2ce21 Swapped polar constructor param order e5f8d73 namespace fix 8e7a8c6 Fix namespaces 64ca768 normalize toAngleAxis result 4385bef Add angle-axis 4b07328 Add conversion from Vector3 fe12c99 Fix Spherical reference c274c3e Add conversion from Spherical 66ff721 Fixed wrong conversion to short 264bcbc Added 32 bit angle f190dd7 Merge branch 'DiscreteAngle' into Experimental ae55a24 Further vector3 conversion fixes ec0cc47 Fix spherical to vector 89b5da8 Fix ambiguity e62fa96 Fix specialization again 430344a Fix specialization error 3e5ede0 Removed old file e62bacc First Discrete Angle functions 87d2c11 Set strict warnings 5141766 Add spherical.toVector3 test (and fixes) 8e3b9e2 Fix Polar test 7b85556 Addedfirst Polar test 395f82d Fix Spherical Unit test 0cbef79 Add first Spherical Unit test dc601a1 Removed Vector2.tofactor 159bdae Added spherical.tovector (but is is buggy) 273ebae Fixes 0468031 Bug fix 3714507 Implemented templated angles 69e7ee1 Added DiscreteAgle 2cbf259 Make negation virtual override 2b50bab Add negation for DiscreteAngle de3a647 Updated negation 254bb27 Added negation bd04285 Make superclass accessable 2bb0009 Initial implementation git-subtree-dir: LinearAlgebra git-subtree-split: 8b6f628d0c6b9e72260ce1d9c0437124c2d7b6d8 --- .gitlab-ci.yml | 2 +- Angle.cpp | 426 +++++++++++++++++++------ Angle.h | 242 ++++++++++++--- CMakeLists.txt | 51 +-- Direction.cpp | 104 +++++++ Direction.h | 102 ++++++ DoxyGen/DoxyWarnLogfile.txt | 568 +++++++++------------------------- DoxyGen/Doxyfile | 454 ++++++++++++++++++--------- FloatSingle.cpp | 12 +- FloatSingle.h | 17 +- Matrix.cpp | 311 ++++++++++++++++++- Matrix.h | 150 ++++++++- Polar.cpp | 223 +++++++++---- Polar.h | 222 ++++++++----- Quaternion.cpp | 95 ++++-- Quaternion.h | 18 +- README.md | 21 +- Spherical.cpp | 327 ++++++++++++++++--- Spherical.h | 213 ++++++++++--- SwingTwist.cpp | 172 ++++++++++ SwingTwist.h | 85 +++++ Vector2.cpp | 161 ++++++---- Vector2.h | 351 ++++++++++----------- Vector3.cpp | 189 ++++++----- Vector3.h | 390 +++++++++++------------ float16.cpp | 250 +++++++++++++++ float16.h | 74 +++++ test/Angle16_test.cc | 241 +++++++++++++++ test/Angle8_test.cc | 241 +++++++++++++++ test/AngleSingle_test.cc | 249 +++++++++++++++ test/Angle_test.cc | 167 ---------- test/Direction_test.cc | 58 ++++ test/DiscreteAngle_test.cc | 82 +++++ test/FloatSingle_test.cc | 40 +-- test/Matrix_test.cc | 85 ++++- test/Polar_test.cc | 233 ++++++++++++++ test/Quaternion_test.cc | 3 +- test/Spherical16_test.cc | 223 +++++++++++++ test/SphericalSingle_test.cc | 214 +++++++++++++ test/SwingTwistSingle_test.cc | 131 ++++++++ test/Vector2_test.cc | 74 +++-- test/Vector3_test.cc | 86 +++-- 42 files changed, 5565 insertions(+), 1792 deletions(-) create mode 100644 Direction.cpp create mode 100644 Direction.h create mode 100644 SwingTwist.cpp create mode 100644 SwingTwist.h create mode 100644 float16.cpp create mode 100644 float16.h create mode 100644 test/Angle16_test.cc create mode 100644 test/Angle8_test.cc create mode 100644 test/AngleSingle_test.cc delete mode 100644 test/Angle_test.cc create mode 100644 test/Direction_test.cc create mode 100644 test/DiscreteAngle_test.cc create mode 100644 test/Polar_test.cc create mode 100644 test/Spherical16_test.cc create mode 100644 test/SphericalSingle_test.cc create mode 100644 test/SwingTwistSingle_test.cc diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 6b28d70..40f9eec 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -21,7 +21,7 @@ unit-test-job: - cmake --build . - export GTEST_OUTPUT="xml:report.xml" - ls -la - - "./VectorAlgebraTest" + - "./LinearAlgebraTest" artifacts: when: always reports: diff --git a/Angle.cpp b/Angle.cpp index 4a039ce..d1221a1 100644 --- a/Angle.cpp +++ b/Angle.cpp @@ -3,123 +3,334 @@ // file, You can obtain one at https ://mozilla.org/MPL/2.0/. #include "Angle.h" -#include "FloatSingle.h" #include +#include "FloatSingle.h" -/* -const float Angle::Rad2Deg = 57.29578F; -const float Angle::Deg2Rad = 0.0174532924F; +namespace LinearAlgebra { -float Angle::Normalize(float angle) { - if (!isfinite(angle)) - return angle; +//===== AngleSingle, AngleOf - while (angle <= -180) - angle += 360; - while (angle > 180) - angle -= 360; +template <> +AngleOf AngleOf::Degrees(float degrees) { + if (isfinite(degrees)) { + while (degrees < -180) + degrees += 360; + while (degrees >= 180) + degrees -= 360; + } + + return AngleOf(degrees); +} + +template <> +AngleOf AngleOf::Radians(float radians) { + if (isfinite(radians)) { + while (radians <= -pi) + radians += 2 * pi; + while (radians > pi) + radians -= 2 * pi; + } + + return Binary(radians * Rad2Deg); +} + +template <> +float AngleOf::InDegrees() const { + return this->value; +} + +template <> +float AngleOf::InRadians() const { + return this->value * Deg2Rad; +} + +//===== Angle16, AngleOf + +template <> +AngleOf AngleOf::Degrees(float degrees) { + // map float [-180..180) to integer [-32768..32767] + signed short value = (signed short)roundf(degrees / 360.0F * 65536.0F); + return Binary(value); +} + +template <> +AngleOf AngleOf::Radians(float radians) { + if (!isfinite(radians)) + return AngleOf::zero; + + // map float [-PI..PI) to integer [-32768..32767] + signed short value = (signed short)roundf(radians / pi * 32768.0F); + return Binary(value); +} + +template <> +float AngleOf::InDegrees() const { + float degrees = this->value / 65536.0f * 360.0f; + return degrees; +} + +template <> +float AngleOf::InRadians() const { + float radians = this->value / 65536.0f * (2 * pi); + return radians; +} + +//===== Angle8, AngleOf + +template <> +AngleOf AngleOf::Degrees(float degrees) { + // map float [-180..180) to integer [-128..127) + signed char value = (signed char)roundf(degrees / 360.0F * 256.0F); + return Binary(value); +} + +template <> +AngleOf AngleOf::Radians(float radians) { + if (!isfinite(radians)) + return AngleOf::zero; + + // map float [-pi..pi) to integer [-128..127) + signed char value = (signed char)roundf(radians / pi * 128.0f); + return Binary(value); +} + +template <> +float AngleOf::InDegrees() const { + float degrees = this->value / 256.0f * 360.0f; + return degrees; +} + +template <> +float AngleOf::InRadians() const { + float radians = this->value / 128.0f * pi; + return radians; +} + +//===== Generic + +template +AngleOf::AngleOf() : value(0) {} + +template +AngleOf::AngleOf(T rawValue) : value(rawValue) {} + +template +const AngleOf AngleOf::zero = AngleOf(); + +template +AngleOf AngleOf::Binary(T rawValue) { + AngleOf angle = AngleOf(); + angle.SetBinary(rawValue); return angle; } -float Angle::Clamp(float angle, float min, float max) { - float normalizedAngle = Normalize(angle); - float r = Float::Clamp(normalizedAngle, min, max); +template +T AngleOf::GetBinary() const { + return this->value; +} +template +void AngleOf::SetBinary(T rawValue) { + this->value = rawValue; +} + +template +bool AngleOf::operator==(const AngleOf angle) const { + return this->value == angle.value; +} + +template +bool AngleOf::operator>(AngleOf angle) const { + return this->value > angle.value; +} + +template +bool AngleOf::operator>=(AngleOf angle) const { + return this->value >= angle.value; +} + +template +bool AngleOf::operator<(AngleOf angle) const { + return this->value < angle.value; +} + +template +bool AngleOf::operator<=(AngleOf angle) const { + return this->value <= angle.value; +} + +template +signed int AngleOf::Sign(AngleOf angle) { + if (angle.value < 0) + return -1; + if (angle.value > 0) + return 1; + return 0; +} + +template +AngleOf AngleOf::Abs(AngleOf angle) { + if (Sign(angle) < 0) + return -angle; + else + return angle; +} + +template +AngleOf AngleOf::operator-() const { + AngleOf angle = Binary(-this->value); + return angle; +} + +template <> +AngleOf AngleOf::operator-(const AngleOf& angle) const { + AngleOf r = Binary(this->value - angle.value); + r = Normalize(r); + return r; +} +template +AngleOf AngleOf::operator-(const AngleOf& angle) const { + AngleOf r = Binary(this->value - angle.value); return r; } -float Angle::Difference(float a, float b) { - float r = Normalize(b - a); +template <> +AngleOf AngleOf::operator+(const AngleOf& angle) const { + AngleOf r = Binary(this->value + angle.value); + r = Normalize(r); + return r; +} +template +AngleOf AngleOf::operator+(const AngleOf& angle) const { + AngleOf r = Binary(this->value + angle.value); return r; } -float Angle::MoveTowards(float fromAngle, float toAngle, float maxAngle) { - float d = toAngle - fromAngle; - float sign = signbit(d) ? -1 : 1; - d = sign * Float::Clamp(fabs(d), 0, maxAngle); - return fromAngle + d; +template <> +AngleOf AngleOf::operator+=(const AngleOf& angle) { + this->value += angle.value; + this->Normalize(); + return *this; } -float Angle::CosineRuleSide(float a, float b, float gamma) { - float a2 = a * a; - float b2 = b * b; - float d = a2 + b2 - 2 * a * b * cos(gamma * Angle::Deg2Rad); - // Catch edge cases where float inacuracies lead tot nans - if (d < 0) - return 0; - - float c = sqrtf(d); - return c; +template +AngleOf AngleOf::operator+=(const AngleOf& angle) { + this->value += angle.value; + return *this; } -float Angle::CosineRuleAngle(float a, float b, float c) { - float a2 = a * a; - float b2 = b * b; - float c2 = c * c; - float d = (a2 + b2 - c2) / (2 * a * b); - // Catch edge cases where float inacuracies lead tot nans - if (d >= 1) - return 0; - if (d <= -1) - return 180; +// This defintion is not matching the declaration in the header file somehow +// template +// AngleOf operator*(const AngleOf &angle, float factor) { +// return AngleOf::Degrees((float)angle.InDegrees() * factor); +// } - float gamma = acos(d) * Angle::Rad2Deg; - return gamma; -} +// This defintion is not matching the declaration in the header file somehow +// template +// AngleOf operator*(float factor, const AngleOf &angle) { +// return AngleOf::Degrees((float)factor * angle.InDegrees()); +// } -float Angle::SineRuleAngle(float a, float beta, float b) { - float alpha = asin(a * sin(beta * Angle::Deg2Rad) / b); - return alpha; -} -*/ -//---------------------- - -template <> Angle2 Angle2::pi = 3.1415927410125732421875F; - -template <> Angle2 Angle2::Rad2Deg = 360.0f / (pi * 2); -template <> Angle2 Angle2::Deg2Rad = (pi * 2) / 360.0f; - -template <> Angle2 Angle2::Normalize(Angle2 angle) { - float angleValue = angle; +template +void AngleOf::Normalize() { + float angleValue = this->InDegrees(); if (!isfinite(angleValue)) - return angleValue; + return; while (angleValue <= -180) angleValue += 360; while (angleValue > 180) angleValue -= 360; - return angleValue; + *this = AngleOf::Degrees(angleValue); } -template <> -Angle2 Angle2::Clamp(Angle2 angle, Angle2 min, - Angle2 max) { - float normalizedAngle = Normalize(angle); - float r = Float::Clamp(normalizedAngle, min, max); - return r; +template +AngleOf AngleOf::Normalize(AngleOf angle) { + float angleValue = angle.InDegrees(); + if (!isfinite(angleValue)) + return angle; + + while (angleValue <= -180) + angleValue += 360; + while (angleValue > 180) + angleValue -= 360; + return AngleOf::Degrees(angleValue); } -// template -// Angle2 Angle2::Difference(Angle2 a, Angle2 b) { -// Angle2 r = Normalize(b - a); -// return r; -// } +template +AngleOf AngleOf::Clamp(AngleOf angle, AngleOf min, AngleOf max) { + float r = Float::Clamp(angle.InDegrees(), min.InDegrees(), max.InDegrees()); + return AngleOf::Degrees(r); +} + +template +AngleOf AngleOf::MoveTowards(AngleOf fromAngle, + AngleOf toAngle, + float maxDegrees) { + maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances + AngleOf d = toAngle - fromAngle; + float dDegrees = Abs(d).InDegrees(); + d = AngleOf::Degrees(Float::Clamp(dDegrees, 0, maxDegrees)); + if (Sign(d) < 0) + d = -d; -template <> -Angle2 Angle2::MoveTowards(Angle2 fromAngle, - Angle2 toAngle, - Angle2 maxAngle) { - float d = toAngle - fromAngle; - float sign = signbit(d) ? -1 : 1; - d = sign * Float::Clamp(fabs(d), 0, maxAngle); return fromAngle + d; } -template <> -Angle2 Angle2::CosineRuleSide(float a, float b, - Angle2 gamma) { +template +float AngleOf::Cos(AngleOf angle) { + return cosf(angle.InRadians()); +} +template +float AngleOf::Sin(AngleOf angle) { + return sinf(angle.InRadians()); +} +template +float AngleOf::Tan(AngleOf angle) { + return tanf(angle.InRadians()); +} + +template +AngleOf AngleOf::Acos(float f) { + return AngleOf::Radians(acosf(f)); +} +template +AngleOf AngleOf::Asin(float f) { + return AngleOf::Radians(asinf(f)); +} +template +AngleOf AngleOf::Atan(float f) { + return AngleOf::Radians(atanf(f)); +} + +template +AngleOf AngleOf::Atan2(float y, float x) { + return AngleOf::Radians(atan2f(y, x)); +} + +// template <> +// float AngleOf::CosineRuleSide(float a, float b, AngleOf gamma) +// { +// float a2 = a * a; +// float b2 = b * b; +// float d = +// a2 + b2 - +// 2 * a * b * Cos(gamma); // cosf(gamma * +// Passer::LinearAlgebra::Deg2Rad); +// // Catch edge cases where float inacuracies lead tot nans +// if (d < 0) +// return 0.0f; + +// float c = sqrtf(d); +// return c; +// } + +template +float AngleOf::CosineRuleSide(float a, float b, AngleOf gamma) { float a2 = a * a; float b2 = b * b; - float d = a2 + b2 - 2 * a * b * cos(gamma * Angle2::Deg2Rad); + float d = + a2 + b2 - + 2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad); // Catch edge cases where float inacuracies lead tot nans if (d < 0) return 0; @@ -128,25 +339,56 @@ Angle2 Angle2::CosineRuleSide(float a, float b, return c; } -template <> -Angle2 Angle2::CosineRuleAngle(float a, float b, float c) { +// template <> +// AngleOf AngleOf::CosineRuleAngle(float a, float b, float c) { +// float a2 = a * a; +// float b2 = b * b; +// float c2 = c * c; +// float d = (a2 + b2 - c2) / (2 * a * b); +// // Catch edge cases where float inacuracies lead tot nans +// if (d >= 1) +// return 0.0f; +// if (d <= -1) +// return 180.0f; + +// float gamma = acosf(d) * Rad2Deg; +// return gamma; +// } +template +AngleOf AngleOf::CosineRuleAngle(float a, float b, float c) { float a2 = a * a; float b2 = b * b; float c2 = c * c; float d = (a2 + b2 - c2) / (2 * a * b); // Catch edge cases where float inacuracies lead tot nans if (d >= 1) - return 0; + return AngleOf(); if (d <= -1) - return 180; + return AngleOf::Degrees(180); - float gamma = acos(d) * Angle::Rad2Deg; + // float gamma = acosf(d) * Rad2Deg; + AngleOf gamma = Acos(d); return gamma; } -template <> -Angle2 Angle2::SineRuleAngle(float a, Angle2 beta, - float b) { - float alpha = asin(a * sin(beta * Angle::Deg2Rad) / b); +// template <> +// AngleOf AngleOf::SineRuleAngle(float a, +// AngleOf beta, +// float b) { +// float deg2rad = Deg2Rad; +// float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b); +// return alpha; +// } +template +AngleOf AngleOf::SineRuleAngle(float a, AngleOf beta, float b) { + // float deg2rad = Deg2Rad; + // float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b); + AngleOf alpha = Asin(a * Sin(beta) / b); return alpha; -} \ No newline at end of file +} + +template class AngleOf; +template class AngleOf; +template class AngleOf; + +} // namespace LinearAlgebra \ No newline at end of file diff --git a/Angle.h b/Angle.h index edabf65..e82b3af 100644 --- a/Angle.h +++ b/Angle.h @@ -5,51 +5,223 @@ #ifndef ANGLE_H #define ANGLE_H -template class Angle2 { -public: - Angle2(){}; - Angle2(T v) : value(v) {} - operator T() { return value; } +namespace LinearAlgebra { - static Angle2 Rad2Deg; - static Angle2 Deg2Rad; +static float pi = 3.1415927410125732421875F; - static Angle2 pi; +static float Rad2Deg = 360.0f / (pi * 2); +static float Deg2Rad = (pi * 2) / 360.0f; - static Angle2 Normalize(Angle2 angle); - static Angle2 Clamp(Angle2 angle, Angle2 min, Angle2 max); - static Angle2 Difference(Angle2 a, Angle2 b) { - Angle2 r = Normalize(b - a); - return r; - }; - static Angle2 MoveTowards(Angle2 fromAngle, Angle2 toAngle, - Angle2 maxAngle); +/// @brief An angle in various representations. +/// @tparam T The internal type used for the representation of the angle. +/// The angle is internally limited to (-180..180] degrees or (-PI...PI] +/// radians. When an angle exceeds this range, it is normalized to a value +/// within the range. +template +class AngleOf { + public: + /// @brief Create a new angle with a zero value + AngleOf(); - static Angle2 CosineRuleSide(float a, float b, Angle2 gamma); - static Angle2 CosineRuleAngle(float a, float b, float c); + /// @brief An zero value angle + const static AngleOf zero; + // const static AngleOf deg90; + // const static AngleOf deg180; - static Angle2 SineRuleAngle(float a, Angle2 beta, float c); + /// @brief Creates an angle in degrees + /// @param degrees the angle in degrees + /// @return The angle value + static AngleOf Degrees(float degrees); + /// @brief Creates an angle in radians + /// @param radians the angle in radians + /// @return The angle value + static AngleOf Radians(float radians); -private: + /// @brief Creates an angle from a raw value + /// @param rawValue the raw value to use for the angle + /// @return The the angle + static AngleOf Binary(T rawValue); + + /// @brief Get the angle value in degrees + /// @return The angle value in degrees + float InDegrees() const; + /// @brief Get the angle value in radians + /// @return The angle value in radians + float InRadians() const; + + /// @brief Get the raw value for the angle + /// @return The raw value + T GetBinary() const; + /// @brief Set the raw value of the angle + /// @param rawValue The raw value + void SetBinary(T rawValue); + + /// @brief Tests whether this angle is equal to the given angle + /// @param angle The angle to compare to + /// @return True when the angles are equal, False otherwise + /// @note The equality is determine within the limits of precision of the raw + /// type T + bool operator==(const AngleOf angle) const; + /// @brief Tests if this angle is greater than the given angle + /// @param angle The given angle + /// @return True when this angle is greater than the given angle, False + /// otherwise + bool operator>(AngleOf angle) const; + /// @brief Tests if this angle is greater than or equal to the given angle + /// @param angle The given angle + /// @return True when this angle is greater than or equal to the given angle. + /// False otherwise. + bool operator>=(AngleOf angle) const; + /// @brief Tests if this angle is less than the given angle + /// @param angle The given angle + /// @return True when this angle is less than the given angle. False + /// otherwise. + bool operator<(AngleOf angle) const; + /// @brief Tests if this angle is less than or equal to the given angle + /// @param angle The given angle + /// @return True when this angle is less than or equal to the given angle. + /// False otherwise. + bool operator<=(AngleOf angle) const; + + /// @brief Returns the sign of the angle + /// @param angle The angle + /// @return -1 when the angle is negative, 1 when it is positive and 0 + /// otherwise. + static signed int Sign(AngleOf angle); + /// @brief Returns the magnitude of the angle + /// @param angle The angle + /// @return The positive magitude of the angle. + /// Negative values are negated to get a positive result + static AngleOf Abs(AngleOf angle); + + /// @brief Negate the angle + /// @return The negated angle + AngleOf operator-() const; + /// @brief Substract another angle from this angle + /// @param angle The angle to subtract from this angle + /// @return The result of the subtraction + AngleOf operator-(const AngleOf& angle) const; + /// @brief Add another angle from this angle + /// @param angle The angle to add to this angle + /// @return The result of the addition + AngleOf operator+(const AngleOf& angle) const; + /// @brief Add another angle to this angle + /// @param angle The angle to add to this angle + /// @return The result of the addition + AngleOf operator+=(const AngleOf& angle); + + /// @brief Mutliplies the angle + /// @param angle The angle to multiply + /// @param factor The factor by which the angle is multiplied + /// @return The multiplied angle + friend AngleOf operator*(const AngleOf& angle, float factor) { + return AngleOf::Degrees((float)angle.InDegrees() * factor); + } + /// @brief Multiplies the angle + /// @param factor The factor by which the angle is multiplies + /// @param angle The angle to multiply + /// @return The multiplied angle + friend AngleOf operator*(float factor, const AngleOf& angle) { + return AngleOf::Degrees((float)factor * angle.InDegrees()); + } + + /// @brief Normalizes the angle to (-180..180] or (-PI..PI] + /// Should not be needed but available in case it is. + void Normalize(); + /// @brief Normalizes the angle to (-180..180] or (-PI..PI] + /// @param angle The angle to normalize + /// @return The normalized angle; + static AngleOf Normalize(AngleOf angle); + /// @brief Clamps the angle value between the two given angles + /// @param angle The angle to clamp + /// @param min The minimum angle + /// @param max The maximum angle + /// @return The clamped value + /// @remark When the min value is greater than the max value, angle is + /// returned unclamped. + static AngleOf Clamp(AngleOf angle, AngleOf min, AngleOf max); + // static AngleOf Difference(AngleOf a, AngleOf b) { + // AngleOf r = Normalize(b.InDegrees() - a.InDegrees()); + // return r; + // }; + + /// @brief Rotates an angle towards another angle with a max distance + /// @param fromAngle The angle to start from + /// @param toAngle The angle to rotate towards + /// @param maxAngle The maximum angle to rotate + /// @return The rotated angle + static AngleOf MoveTowards(AngleOf fromAngle, + AngleOf toAngle, + float maxAngle); + + /// @brief Calculates the cosine of an angle + /// @param angle The given angle + /// @return The cosine of the angle + static float Cos(AngleOf angle); + /// @brief Calculates the sine of an angle + /// @param angle The given angle + /// @return The sine of the angle + static float Sin(AngleOf angle); + /// @brief Calculates the tangent of an angle + /// @param angle The given angle + /// @return The tangent of the angle + static float Tan(AngleOf angle); + + /// @brief Calculates the arc cosine angle + /// @param f The value + /// @return The arc cosine for the given value + static AngleOf Acos(float f); + /// @brief Calculates the arc sine angle + /// @param f The value + /// @return The arc sine for the given value + static AngleOf Asin(float f); + /// @brief Calculates the arc tangent angle + /// @param f The value + /// @return The arc tangent for the given value + static AngleOf Atan(float f); + /// @brief Calculates the tangent for the given values + /// @param y The vertical value + /// @param x The horizontal value + /// @return The tanget for the given values + /// Uses the y and x signs to compute the quadrant + static AngleOf Atan2(float y, float x); + + /// @brief Computes the length of a side using the rule of cosines + /// @param a The length of side A + /// @param b The length of side B + /// @param gamma The angle of the corner opposing side C + /// @return The length of side C + static float CosineRuleSide(float a, float b, AngleOf gamma); + /// @brief Computes the angle of a corner using the rule of cosines + /// @param a The length of side A + /// @param b The length of side B + /// @param c The length of side C + /// @return The angle of the corner opposing side C + static AngleOf CosineRuleAngle(float a, float b, float c); + + /// @brief Computes the angle of a corner using the rule of sines + /// @param a The length of side A + /// @param beta the angle of the corner opposing side B + /// @param c The length of side C + /// @return The angle of the corner opposing side A + static AngleOf SineRuleAngle(float a, AngleOf beta, float c); + + private: T value; + + AngleOf(T rawValue); }; -using Angle = Angle2; -/* -class Angle { -public: - const static float Rad2Deg; - const static float Deg2Rad; +using AngleSingle = AngleOf; +using Angle16 = AngleOf; +using Angle8 = AngleOf; - static float Normalize(float angle); - static float Clamp(float angle, float min, float max); - static float Difference(float a, float b); - static float MoveTowards(float fromAngle, float toAngle, float maxAngle); +#if defined(ARDUINO) +using Angle = Angle16; +#else +using Angle = AngleSingle; +#endif - static float CosineRuleSide(float a, float b, float gamma); - static float CosineRuleAngle(float a, float b, float c); +} // namespace LinearAlgebra - static float SineRuleAngle(float a, float beta, float c); -}; -*/ #endif \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index d61999d..ed52791 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,15 +5,16 @@ if(ESP_PLATFORM) INCLUDE_DIRS "." ) else() - project(VectorAlgebra) # Create project "simple_example" + project(LinearAlgebra) - set(CMAKE_CXX_STANDARD 11) # Enable c++11 standard + set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard set(CMAKE_POSITION_INDEPENDENT_CODE ON) add_compile_definitions(GTEST) include(FetchContent) FetchContent_Declare( googletest + DOWNLOAD_EXTRACT_TIMESTAMP ON URL https://github.com/google/googletest/archive/refs/heads/main.zip ) @@ -22,35 +23,43 @@ else() FetchContent_MakeAvailable(googletest) include_directories(.) - add_library(VectorAlgebra STATIC - "FloatSingle.cpp" - "Angle.cpp" - "Vector2.cpp" - "Vector3.cpp" - "Quaternion.cpp" - "Polar.cpp" - "Spherical.cpp" - "Matrix.cpp" + file(GLOB srcs + *.cpp + ) + add_library(LinearAlgebra STATIC ${srcs} + # "FloatSingle.cpp" + # "Angle.cpp" + # "Vector2.cpp" + # "Vector3.cpp" + # "Quaternion.cpp" + # "Polar.cpp" + # "Spherical.cpp" + # "Matrix.cpp" + # "SwingTwist.cpp" + # "Direction.cpp" ) enable_testing() + file(GLOB_RECURSE test_srcs test/*_test.cc) add_executable( - VectorAlgebraTest - "test/Angle_test.cc" - "test/FloatSingle_test.cc" - "test/Vector2_test.cc" - "test/Vector3_test.cc" - "test/Quaternion_test.cc" - "test/Matrix_test.cc" + LinearAlgebraTest + ${test_srcs} ) target_link_libraries( - VectorAlgebraTest + LinearAlgebraTest gtest_main - VectorAlgebra + LinearAlgebra ) + if(MSVC) + target_compile_options(LinearAlgebraTest PRIVATE /W4 /WX) +# else() +# target_compile_options(LinearAlgebraTest PRIVATE -Wall -Wextra -Wpedantic -Werror) + endif() + + include(GoogleTest) - gtest_discover_tests(VectorAlgebraTest) + gtest_discover_tests(LinearAlgebraTest) endif() diff --git a/Direction.cpp b/Direction.cpp new file mode 100644 index 0000000..86fa77c --- /dev/null +++ b/Direction.cpp @@ -0,0 +1,104 @@ +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0.If a copy of the MPL was not distributed with this +// file, You can obtain one at https ://mozilla.org/MPL/2.0/. + +#include "Direction.h" + +#include "Quaternion.h" +#include "Vector3.h" + +#include + +namespace LinearAlgebra { +template +DirectionOf::DirectionOf() { + this->horizontal = AngleOf(); + this->vertical = AngleOf(); +} + +template +DirectionOf::DirectionOf(AngleOf horizontal, AngleOf vertical) { + this->horizontal = horizontal; + this->vertical = vertical; + Normalize(); +}; + +template +const DirectionOf DirectionOf::forward = + DirectionOf(AngleOf(), AngleOf()); +template +const DirectionOf DirectionOf::back = + DirectionOf(AngleOf::Degrees(180), AngleOf()); +template +const DirectionOf DirectionOf::up = + DirectionOf(AngleOf(), AngleOf::Degrees(90)); +template +const DirectionOf DirectionOf::down = + DirectionOf(AngleOf(), AngleOf::Degrees(-90)); +template +const DirectionOf DirectionOf::left = + DirectionOf(AngleOf::Degrees(-90), AngleOf()); +template +const DirectionOf DirectionOf::right = + DirectionOf(AngleOf::Degrees(90), AngleOf()); + +template +Vector3 DirectionOf::ToVector3() const { + Quaternion q = Quaternion::Euler(-this->vertical.InDegrees(), + this->horizontal.InDegrees(), 0); + Vector3 v = q * Vector3::forward; + return v; +} + +template +DirectionOf DirectionOf::FromVector3(Vector3 vector) { + DirectionOf d; + d.horizontal = AngleOf::Atan2( + vector.Right(), + vector + .Forward()); // AngleOf::Radians(atan2f(v.Right(), v.Forward())); + d.vertical = + AngleOf::Degrees(-90) - + AngleOf::Acos( + vector.Up()); // AngleOf::Radians(-(0.5f * pi) - acosf(v.Up())); + d.Normalize(); + return d; +} + +template +DirectionOf DirectionOf::Degrees(float horizontal, float vertical) { + return DirectionOf(AngleOf::Degrees(horizontal), + AngleOf::Degrees(vertical)); +} + +template +DirectionOf DirectionOf::Radians(float horizontal, float vertical) { + return DirectionOf(AngleOf::Radians(horizontal), + AngleOf::Radians(vertical)); +} + +template +bool DirectionOf::operator==(const DirectionOf direction) const { + return (this->horizontal == direction.horizontal) && + (this->vertical == direction.vertical); +} + +template +DirectionOf DirectionOf::operator-() const { + DirectionOf r = DirectionOf(this->horizontal + AngleOf::Degrees(180), + -this->vertical); + return r; +} + +template +void DirectionOf::Normalize() { + if (this->vertical > AngleOf::Degrees(90) || + this->vertical < AngleOf::Degrees(-90)) { + this->horizontal += AngleOf::Degrees(180); + this->vertical = AngleOf::Degrees(180) - this->vertical; + } +} + +template class LinearAlgebra::DirectionOf; +template class LinearAlgebra::DirectionOf; +} \ No newline at end of file diff --git a/Direction.h b/Direction.h new file mode 100644 index 0000000..8c58b6c --- /dev/null +++ b/Direction.h @@ -0,0 +1,102 @@ +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0.If a copy of the MPL was not distributed with this +// file, You can obtain one at https ://mozilla.org/MPL/2.0/. + +#ifndef DIRECTION_H +#define DIRECTION_H + +#include "Angle.h" + +namespace LinearAlgebra { + +struct Vector3; + +/// @brief A direction using angles in various representations +/// @tparam T The implementation type used for the representation of the angles +/// A direction is represented using two angles: +/// * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive) +/// degrees which is a rotation in the horizontal plane +/// * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees +/// which is the rotation in the up/down direction applied after the horizontal +/// rotation has been applied. +/// The angles are automatically normalized to stay within the abovenmentioned +/// ranges. +template +class DirectionOf { + public: + /// @brief horizontal angle, range= (-180..180] + AngleOf horizontal; + /// @brief vertical angle, range in degrees = (-90..90] + AngleOf vertical; + + /// @brief Create a new direction with zero angles + DirectionOf(); + /// @brief Create a new direction + /// @param horizontal The horizontal angle + /// @param vertical The vertical angle. + DirectionOf(AngleOf horizontal, AngleOf vertical); + + /// @brief Convert the direction into a carthesian vector + /// @return The carthesian vector corresponding to this direction. + Vector3 ToVector3() const; + /// @brief Convert a carthesian vector into a direction + /// @param v The carthesian vector + /// @return The direction. + /// @note Information about the length of the carthesian vector is not + /// included in this transformation. + static DirectionOf FromVector3(Vector3 vector); + + /// @brief Create a direction using angle values in degrees + /// @param horizontal The horizontal angle in degrees + /// @param vertical The vertical angle in degrees + /// @return The direction + static DirectionOf Degrees(float horizontal, float vertical); + /// @brief Create a direction using angle values in radians + /// @param horizontal The horizontal angle in radians + /// @param vertical The vertical angle in radians + /// @return The direction + static DirectionOf Radians(float horizontal, float vertical); + + /// @brief A forward direction with zero for both angles + const static DirectionOf forward; + /// @brief A backward direction with horizontal angle -180 and zero vertical + /// angle + const static DirectionOf back; + /// @brief A upward direction with zero horizontal angle and vertical angle 90 + const static DirectionOf up; + /// @brief A downward direction with zero horizontal angle and vertical angle + /// -90 + const static DirectionOf down; + /// @brief A left-pointing direction with horizontal angle -90 and zero + /// vertical angle + const static DirectionOf left; + /// @brief A right-pointing direction with horizontal angle 90 and zero + /// vertical angle + const static DirectionOf right; + + /// @brief Test whether this direction is equal to another direction + /// @param direction The direction to compare to + /// @return True when the direction angles are equal, false otherwise. + bool operator==(const DirectionOf direction) const; + + /// @brief Negate/reverse the direction + /// @return The reversed direction. + DirectionOf operator-() const; + + protected: + /// @brief Normalize this vector to the specified ranges + void Normalize(); +}; + +using DirectionSingle = DirectionOf; +using Direction16 = DirectionOf; + +#if defined(ARDUINO) +using Direction = Direction16; +#else +using Direction = DirectionSingle; +#endif + +} // namespace LinearAlgebra + +#endif \ No newline at end of file diff --git a/DoxyGen/DoxyWarnLogfile.txt b/DoxyGen/DoxyWarnLogfile.txt index d37d508..3f3c14a 100644 --- a/DoxyGen/DoxyWarnLogfile.txt +++ b/DoxyGen/DoxyWarnLogfile.txt @@ -1,420 +1,154 @@ -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_for_dummies.md:566: warning: multiple use of section label 'OrderedCalls' while adding section, (first occurrence: D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md, line 1674) -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:411: warning: multiple use of section label 'DefaultValue' while adding section, (first occurrence: D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md, line 2088) -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/gtest-message.h:62: warning: Found ';' while parsing initializer list! (doxygen could be confused by a macro call without semicolon) -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/internal/gtest-internal.h:158: warning: Found ';' while parsing initializer list! (doxygen could be confused by a macro call without semicolon) -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-output-test_.cc:47: warning: Found ';' while parsing initializer list! (doxygen could be confused by a macro call without semicolon) -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest-typed-test_test.cc:43: warning: Found ';' while parsing initializer list! (doxygen could be confused by a macro call without semicolon) -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/include/gmock/gmock-matchers.h:256: warning: found documented #define GOOGLEMOCK_INCLUDE_GMOCK_GMOCK_MATCHERS_H_ but ignoring it because ENABLE_PREPROCESSING is NO. - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/gtest-printers.h:288: warning: Detected potential recursive class relation between class testing::internal::FindFirstPrinter and base class testing::internal::FindFirstPrinter< T, E, Printers... >! - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/gtest-printers.h:288: warning: Detected potential recursive class relation between class testing::internal::FindFirstPrinter and base class FindFirstPrinter< T, E, Printers... >! - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/gtest-printers.h:288: warning: Detected potential recursive class relation between class testing::internal::FindFirstPrinter and base class testing::internal::FindFirstPrinter< T, E, Printers... >! - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/gtest-printers.h:288: warning: Detected potential recursive class relation between class testing::internal::FindFirstPrinter and base class FindFirstPrinter< T, E, Printers... >! - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-cardinalities.cc:129: warning: documented symbol 'void testing::Cardinality::DescribeActualCallCountTo' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-matchers.cc:248: warning: documented symbol 'bool testing::internal::MatchMatrix::NextGraph' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-matchers.cc:262: warning: documented symbol 'void testing::internal::MatchMatrix::Randomize' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-matchers.cc:271: warning: no uniquely matching class member found for - std::string testing::internal::MatchMatrix::DebugString() const - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-matchers.cc:284: warning: documented symbol 'void testing::internal::UnorderedElementsAreMatcherImplBase::DescribeToImpl' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-matchers.cc:325: warning: documented symbol 'void testing::internal::UnorderedElementsAreMatcherImplBase::DescribeNegationToImpl' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-matchers.cc:371: warning: documented symbol 'bool testing::internal::UnorderedElementsAreMatcherImplBase::VerifyMatchMatrix' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-matchers.cc:421: warning: documented symbol 'bool testing::internal::UnorderedElementsAreMatcherImplBase::FindPairing' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:82: warning: documented symbol 'testing::internal::ExpectationBase::ExpectationBase' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:98: warning: documented symbol 'testing::internal::ExpectationBase::~ExpectationBase' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:102: warning: documented symbol 'void testing::internal::ExpectationBase::SpecifyCardinality' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:108: warning: documented symbol 'void testing::internal::ExpectationBase::RetireAllPreRequisites' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:135: warning: documented symbol 'bool testing::internal::ExpectationBase::AllPrerequisitesAreSatisfied' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:155: warning: documented symbol 'void testing::internal::ExpectationBase::FindUnsatisfiedPrerequisites' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:186: warning: documented symbol 'void testing::internal::ExpectationBase::DescribeCallCountTo' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:209: warning: documented symbol 'void testing::internal::ExpectationBase::CheckActionCountIfNotDone' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:261: warning: documented symbol 'void testing::internal::ExpectationBase::UntypedTimes' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:308: warning: documented symbol 'testing::internal::UntypedFunctionMockerBase::UntypedFunctionMockerBase' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:311: warning: documented symbol 'testing::internal::UntypedFunctionMockerBase::~UntypedFunctionMockerBase' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:317: warning: documented symbol 'void testing::internal::UntypedFunctionMockerBase::RegisterOwner' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:329: warning: documented symbol 'void testing::internal::UntypedFunctionMockerBase::SetOwnerAndName' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:341: warning: documented symbol 'const void * testing::internal::UntypedFunctionMockerBase::MockObject' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:358: warning: documented symbol 'const char * testing::internal::UntypedFunctionMockerBase::Name' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:376: warning: documented symbol 'UntypedActionResultHolderBase * testing::internal::UntypedFunctionMockerBase::UntypedInvokeWith' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:505: warning: documented symbol 'Expectation testing::internal::UntypedFunctionMockerBase::GetHandleOf' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:525: warning: documented symbol 'bool testing::internal::UntypedFunctionMockerBase::VerifyAndClearExpectationsLocked' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:685: warning: documented symbol 'void Mock::AllowUninterestingCalls' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:692: warning: documented symbol 'void Mock::WarnUninterestingCalls' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:699: warning: documented symbol 'void Mock::FailUninterestingCalls' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:706: warning: documented symbol 'void Mock::UnregisterCallReaction' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:714: warning: documented symbol 'internal::CallReaction Mock::GetReactionOnUninterestingCalls' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:726: warning: documented symbol 'void Mock::AllowLeak' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:735: warning: documented symbol 'bool Mock::VerifyAndClearExpectations' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:744: warning: documented symbol 'bool Mock::VerifyAndClear' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:754: warning: documented symbol 'bool Mock::VerifyAndClearExpectationsLocked' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:779: warning: documented symbol 'bool Mock::IsNaggy' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:783: warning: documented symbol 'bool Mock::IsNice' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:787: warning: documented symbol 'bool Mock::IsStrict' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:793: warning: no uniquely matching class member found for - void Mock::Register(const void *mock_obj, internal::UntypedFunctionMockerBase *mocker) - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:803: warning: documented symbol 'void Mock::RegisterUseByOnCallOrExpectCall' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:824: warning: documented symbol 'void Mock::UnregisterLocked' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:842: warning: documented symbol 'void Mock::ClearDefaultActionsLocked' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:864: warning: documented symbol 'testing::Expectation::Expectation' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:866: warning: documented symbol 'testing::Expectation::Expectation' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:870: warning: documented symbol 'testing::Expectation::~Expectation' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:873: warning: documented symbol 'void testing::Sequence::AddExpectation' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:884: warning: documented symbol 'testing::InSequence::InSequence' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/src/gmock-spec-builders.cc:895: warning: documented symbol 'testing::InSequence::~InSequence' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/internal/gtest-internal.h:111: warning: no matching file member found for -std::string testing::PrintToString(const T &value) +warning: source 'images' is not a readable file or directory... skipping. +d:/PlatformIO/linear-algebra/Quaternion.cpp:100: warning: no uniquely matching class member found for + Quaternion Quaternion::operator*(const Quaternion &r2) const Possible candidates: - '::std::string PrintToString(const T &value)' at line 1037 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/gtest-printers.h - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/internal/gtest-internal.h:160: warning: no matching file member found for -testing::internal::GTEST_DISABLE_MSC_WARNINGS_PUSH_ + 'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(const AngleOf< T > &angle, float factor)' at line 117 of file d:/PlatformIO/linear-algebra/Angle.h + 'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(float factor, const AngleOf< T > &angle)' at line 124 of file d:/PlatformIO/linear-algebra/Angle.h + 'Vector3 Passer::LinearAlgebra::MatrixOf< T >::operator*(const Vector3 v) const' at line 64 of file d:/PlatformIO/linear-algebra/Matrix.h + 'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(const PolarOf &v, float f)' at line 118 of file d:/PlatformIO/linear-algebra/Polar.h + 'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(float f, const PolarOf &v)' at line 121 of file d:/PlatformIO/linear-algebra/Polar.h + 'Vector3 Passer::LinearAlgebra::Quaternion::operator*(const Vector3 &vector) const' at line 98 of file d:/PlatformIO/linear-algebra/Quaternion.h + 'Quaternion Passer::LinearAlgebra::Quaternion::operator*(const Quaternion &rotation) const' at line 106 of file d:/PlatformIO/linear-algebra/Quaternion.h + 'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(const SphericalOf< T > &v, float f)' at line 111 of file d:/PlatformIO/linear-algebra/Spherical.h + 'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(float f, const SphericalOf< T > &v)' at line 114 of file d:/PlatformIO/linear-algebra/Spherical.h + 'SphericalOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SphericalOf< T > &vector) const' at line 46 of file d:/PlatformIO/linear-algebra/SwingTwist.h + 'SwingTwistOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SwingTwistOf< T > &rotation) const' at line 54 of file d:/PlatformIO/linear-algebra/SwingTwist.h + 'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(const Vector2 &v, float f)' at line 141 of file d:/PlatformIO/linear-algebra/Vector2.h + 'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(float f, const Vector2 &v)' at line 144 of file d:/PlatformIO/linear-algebra/Vector2.h + 'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(const Vector3 &v, float f)' at line 149 of file d:/PlatformIO/linear-algebra/Vector3.h + 'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(float f, const Vector3 &v)' at line 152 of file d:/PlatformIO/linear-algebra/Vector3.h +d:/PlatformIO/linear-algebra/Quaternion.cpp:108: warning: no uniquely matching class member found for + Vector3 Quaternion::operator*(const Vector3 &p) const Possible candidates: - 'GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 \) namespace testing' at line 48 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/include/gmock/gmock-cardinalities.h - 'GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 GMOCK_MAYBE_5046_) namespace testing' at line 283 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/include/gmock/gmock-matchers.h - 'GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 \) namespace testing' at line 84 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googlemock/include/gmock/gmock-spec-builders.h - 'GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 GTEST_MAYBE_5046_) namespace testing' at line 59 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/gtest-matchers.h - 'GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 \) namespace testing' at line 38 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/gtest-spi.h - 'GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 \) namespace testing' at line 42 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/gtest-test-part.h - 'GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 \) GTEST_DECLARE_bool_(also_run_disabled_tests)' at line 72 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/gtest.h - 'GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 \) class GTEST_API_ DeathTest' at line 60 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/internal/gtest-death-test-internal.h - 'GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 \) namespace testing' at line 47 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/include/gtest/internal/gtest-filepath.h - 'GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 \) GTEST_DECLARE_bool_(death_test_use_fork)' at line 64 of file D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-internal-inl.h - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-assertion-result.cc:59: warning: documented symbol 'AssertionResult testing::AssertionResult::operator!' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-death-test.cc:373: warning: documented symbol 'testing::internal::DeathTest::DeathTest' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-death-test.cc:383: warning: no matching class member found for - bool testing::internal::DeathTest::Create(const char *statement, Matcher< const std::string & > matcher, const char *file, int line, DeathTest **test) - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-death-test.cc:390: warning: documented symbol 'const char * testing::internal::DeathTest::LastMessage' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-death-test.cc:394: warning: documented symbol 'void testing::internal::DeathTest::set_last_death_test_message' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-death-test.cc:398: warning: documented symbol 'std::string testing::internal::DeathTest::last_death_test_message_' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:94: warning: documented symbol 'FilePath testing::internal::FilePath::GetCurrentDir' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:121: warning: documented symbol 'FilePath testing::internal::FilePath::RemoveExtension' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:133: warning: documented symbol 'const char * testing::internal::FilePath::FindLastPathSeparator' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:152: warning: documented symbol 'FilePath testing::internal::FilePath::RemoveDirectoryName' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:163: warning: documented symbol 'FilePath testing::internal::FilePath::RemoveFileName' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:180: warning: documented symbol 'FilePath testing::internal::FilePath::MakeFileName' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:196: warning: documented symbol 'FilePath testing::internal::FilePath::ConcatPaths' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:206: warning: documented symbol 'bool testing::internal::FilePath::FileOrDirectoryExists' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:220: warning: documented symbol 'bool testing::internal::FilePath::DirectoryExists' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:250: warning: documented symbol 'bool testing::internal::FilePath::IsRootDirectory' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:259: warning: documented symbol 'bool testing::internal::FilePath::IsAbsolutePath' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:280: warning: documented symbol 'FilePath testing::internal::FilePath::GenerateUniqueFileName' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:294: warning: documented symbol 'bool testing::internal::FilePath::IsDirectory' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:302: warning: documented symbol 'bool testing::internal::FilePath::CreateDirectoriesRecursively' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:319: warning: documented symbol 'bool testing::internal::FilePath::CreateFolder' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:343: warning: documented symbol 'FilePath testing::internal::FilePath::RemoveTrailingPathSeparator' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-filepath.cc:352: warning: no uniquely matching class member found for - void testing::internal::FilePath::Normalize() - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-matchers.cc:45: warning: documented symbol 'testing::Matcher< typename >::Matcher' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-matchers.cc:49: warning: documented symbol 'testing::Matcher< typename >::Matcher' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-matchers.cc:55: warning: documented symbol 'testing::Matcher< typename >::Matcher' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-matchers.cc:59: warning: documented symbol 'testing::Matcher< typename >::Matcher' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-matchers.cc:64: warning: documented symbol 'testing::Matcher< typename >::Matcher' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-matchers.cc:70: warning: documented symbol 'testing::Matcher< typename >::Matcher' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-matchers.cc:76: warning: documented symbol 'testing::Matcher< typename >::Matcher' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-matchers.cc:82: warning: documented symbol 'testing::Matcher< typename >::Matcher' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-matchers.cc:86: warning: documented symbol 'testing::Matcher< typename >::Matcher' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-matchers.cc:92: warning: documented symbol 'testing::Matcher< typename >::Matcher' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-port.cc:301: warning: no matching class member found for - void testing::internal::AutoHandle::Reset(HANDLE handle) + 'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(const AngleOf< T > &angle, float factor)' at line 117 of file d:/PlatformIO/linear-algebra/Angle.h + 'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(float factor, const AngleOf< T > &angle)' at line 124 of file d:/PlatformIO/linear-algebra/Angle.h + 'Vector3 Passer::LinearAlgebra::MatrixOf< T >::operator*(const Vector3 v) const' at line 64 of file d:/PlatformIO/linear-algebra/Matrix.h + 'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(const PolarOf &v, float f)' at line 118 of file d:/PlatformIO/linear-algebra/Polar.h + 'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(float f, const PolarOf &v)' at line 121 of file d:/PlatformIO/linear-algebra/Polar.h + 'Vector3 Passer::LinearAlgebra::Quaternion::operator*(const Vector3 &vector) const' at line 98 of file d:/PlatformIO/linear-algebra/Quaternion.h + 'Quaternion Passer::LinearAlgebra::Quaternion::operator*(const Quaternion &rotation) const' at line 106 of file d:/PlatformIO/linear-algebra/Quaternion.h + 'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(const SphericalOf< T > &v, float f)' at line 111 of file d:/PlatformIO/linear-algebra/Spherical.h + 'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(float f, const SphericalOf< T > &v)' at line 114 of file d:/PlatformIO/linear-algebra/Spherical.h + 'SphericalOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SphericalOf< T > &vector) const' at line 46 of file d:/PlatformIO/linear-algebra/SwingTwist.h + 'SwingTwistOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SwingTwistOf< T > &rotation) const' at line 54 of file d:/PlatformIO/linear-algebra/SwingTwist.h + 'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(const Vector2 &v, float f)' at line 141 of file d:/PlatformIO/linear-algebra/Vector2.h + 'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(float f, const Vector2 &v)' at line 144 of file d:/PlatformIO/linear-algebra/Vector2.h + 'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(const Vector3 &v, float f)' at line 149 of file d:/PlatformIO/linear-algebra/Vector3.h + 'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(float f, const Vector3 &v)' at line 152 of file d:/PlatformIO/linear-algebra/Vector3.h +d:/PlatformIO/linear-algebra/Quaternion.cpp:152: warning: no uniquely matching class member found for + Quaternion Quaternion::LookRotation(const Vector3 &forward, const Vector3 &up) Possible candidates: - 'void testing::internal::AutoHandle::Reset()' - 'void testing::internal::AutoHandle::Reset(Handle handle)' - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-test-part.cc:44: warning: documented symbol 'std::string testing::TestPartResult::ExtractSummary' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-test-part.cc:66: warning: no uniquely matching class member found for - void testing::TestPartResultArray::Append(const TestPartResult &result) - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-test-part.cc:71: warning: no matching class member found for - const TestPartResult & testing::TestPartResultArray::GetTestPartResult(int index) const - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-test-part.cc:81: warning: no uniquely matching class member found for - int testing::TestPartResultArray::size() const - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-test-part.cc:87: warning: documented symbol 'testing::internal::HasNewFatalFailureHelper::HasNewFatalFailureHelper' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-test-part.cc:94: warning: documented symbol 'testing::internal::HasNewFatalFailureHelper::~HasNewFatalFailureHelper' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-test-part.cc:99: warning: no uniquely matching class member found for - void testing::internal::HasNewFatalFailureHelper::ReportTestPartResult(const TestPartResult &result) - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest-typed-test.cc:58: warning: documented symbol 'const char * testing::internal::TypedTestSuitePState::VerifyRegisteredTestNames' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest.cc:858: warning: documented symbol 'testing::ScopedFakeTestPartResultReporter::ScopedFakeTestPartResultReporter' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest.cc:868: warning: documented symbol 'testing::ScopedFakeTestPartResultReporter::ScopedFakeTestPartResultReporter' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest.cc:875: warning: no uniquely matching class member found for - void testing::ScopedFakeTestPartResultReporter::Init() - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest.cc:888: warning: documented symbol 'testing::ScopedFakeTestPartResultReporter::~ScopedFakeTestPartResultReporter' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest.cc:899: warning: no uniquely matching class member found for - void testing::ScopedFakeTestPartResultReporter::ReportTestPartResult(const TestPartResult &result) - -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest.cc:965: warning: documented symbol 'testing::internal::SingleFailureChecker::SingleFailureChecker' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest.cc:974: warning: documented symbol 'testing::internal::SingleFailureChecker::~SingleFailureChecker' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/src/gtest.cc:2602: warning: documented symbol 'testing::internal::GoogleTestFailureException::GoogleTestFailureException' was not declared or defined. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:52: warning: explicit link request to 'testing::AssertionResult' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:229: warning: explicit link request to 'Bar()' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:283: warning: explicit link request to 'testing::Environment' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:283: warning: explicit link request to 'testing::Test' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:376: warning: explicit link request to 'testing::PrintToString(x)' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:786: warning: explicit link request to 'testing::Test' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:813: warning: explicit link request to 'testing::Test' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:850: warning: explicit link request to 'testing::Test' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:953: warning: explicit link request to 'testing::Environment' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:970: warning: explicit link request to 'testing::AddGlobalTestEnvironment()' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1243: warning: explicit link request to 'testing::Test' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1364: warning: explicit link request to 'testing::Types' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1397: warning: explicit link request to 'including' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1496: warning: explicit link request to 'including' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1538: warning: explicit link request to 'testing::RegisterTest' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1764: warning: explicit link request to 'testing::InitGoogleTest()' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1813: warning: found at different nesting level (5) than expected (2) -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1822: warning: found tag while expecting -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1823: warning: found tag while expecting -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1824: warning: found tag while expecting -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1986: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1986: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1987: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1987: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1988: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1988: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1989: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1989: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1990: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1990: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1991: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1991: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1992: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1992: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1994: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1994: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1996: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1996: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1997: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1997: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1998: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1998: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1999: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:1999: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:2000: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:2000: warning: Unsupported xml/html tag found -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/advanced.md:2380: warning: end of comment block while expecting command -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/faq.md:658: warning: explicit link request to 'testing::Test' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cheat_sheet.md:6: warning: found subsection command (id: 'MockClass') outside of section context! -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md:9: warning: explicit link request to 'testing::Foo' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md:186: warning: found subsection command (id: 'MockingNonVirtualMethods') outside of section context! -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md:1540: warning: explicit link request to 'Set()' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md:1950: warning: explicit link request to 'testing::ActionInterface' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md:2045: warning: explicit link request to 'testing::InSequence' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md:2098: warning: explicit link request to 'testing::DefaultValue' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md:2223: warning: explicit link request to 'testing::Invoke' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md:2309: warning: explicit link request to 'testing::InvokeWithoutArgs' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_cook_book.md:4107: warning: explicit link request to 'testing::ActionInterface' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/gmock_for_dummies.md:524: warning: found subsection command (id: 'MultiExpectations') outside of section context! -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/primer.md:213: warning: explicit link request to 'testing::Test' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/primer.md:457: warning: explicit link request to 'testing::InitGoogleTest()' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/actions.md:5: warning: explicit link request to 'testing' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/assertions.md:529: warning: explicit link request to 'testing::GTEST_FLAG(death_test_style)' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/matchers.md:20: warning: explicit link request to 'testing' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/matchers.md:145: warning: explicit link request to 'std::tuple' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:92: warning: explicit link request to 'EXPECT_CALL.With' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:122: warning: explicit link request to 'EXPECT_CALL.Times' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:129: warning: explicit link request to 'testing' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:151: warning: explicit link request to 'EXPECT_CALL.InSequence' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:181: warning: explicit link request to 'EXPECT_CALL.After' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:222: warning: explicit link request to 'EXPECT_CALL.WillOnce' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:253: warning: explicit link request to 'EXPECT_CALL.WillRepeatedly' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:286: warning: explicit link request to 'EXPECT_CALL.RetiresOnSaturation' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:347: warning: explicit link request to 'ON_CALL.With' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:378: warning: explicit link request to 'ON_CALL.WillByDefault' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:413: warning: explicit link request to 'testing::DefaultValue' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:437: warning: explicit link request to 'testing::NiceMock' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:468: warning: explicit link request to 'testing::NaggyMock' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:495: warning: explicit link request to 'testing::StrictMock' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:527: warning: explicit link request to 'testing::Sequence' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:534: warning: explicit link request to 'testing::InSequence' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:559: warning: explicit link request to 'testing::Expectation' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/mocking.md:574: warning: explicit link request to 'testing::ExpectationSet' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:13: warning: found subsection command (id: 'TEST') outside of section context! -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:100: warning: explicit link request to 'testing' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:389: warning: explicit link request to 'testing::AssertionResult' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:403: warning: explicit link request to 'testing::AssertionException' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:410: warning: explicit link request to 'testing::EmptyTestEventListener' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:418: warning: explicit link request to 'testing::Environment' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:425: warning: explicit link request to 'Environment::SetUp' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:431: warning: explicit link request to 'Environment::TearDown' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:439: warning: explicit link request to 'testing::ScopedTrace' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:466: warning: explicit link request to 'testing::Test' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:472: warning: explicit link request to 'Test::SetUpTestSuite' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:479: warning: explicit link request to 'Test::TearDownTestSuite' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:486: warning: explicit link request to 'Test::HasFatalFailure' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:492: warning: explicit link request to 'Test::HasNonfatalFailure' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:498: warning: explicit link request to 'Test::HasFailure' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:505: warning: explicit link request to 'Test::IsSkipped' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:511: warning: explicit link request to 'Test::RecordProperty' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:538: warning: explicit link request to 'Test::SetUp' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:545: warning: explicit link request to 'Test::TearDown' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:554: warning: explicit link request to 'testing::TestWithParam' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:565: warning: explicit link request to 'TestSuite::name' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:571: warning: explicit link request to 'TestSuite::type_param' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:579: warning: explicit link request to 'TestSuite::should_run' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:585: warning: explicit link request to 'TestSuite::successful_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:591: warning: explicit link request to 'TestSuite::skipped_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:597: warning: explicit link request to 'TestSuite::failed_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:603: warning: explicit link request to 'TestSuite::reportable_disabled_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:609: warning: explicit link request to 'TestSuite::disabled_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:615: warning: explicit link request to 'TestSuite::reportable_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:621: warning: explicit link request to 'TestSuite::test_to_run_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:627: warning: explicit link request to 'TestSuite::total_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:633: warning: explicit link request to 'TestSuite::Passed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:639: warning: explicit link request to 'TestSuite::Failed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:645: warning: explicit link request to 'TestSuite::elapsed_time' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:651: warning: explicit link request to 'TestSuite::start_timestamp' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:657: warning: explicit link request to 'TestSuite::GetTestInfo' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:665: warning: explicit link request to 'TestSuite::ad_hoc_test_result' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:674: warning: explicit link request to 'testing::TestInfo' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:680: warning: explicit link request to 'TestInfo::test_suite_name' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:686: warning: explicit link request to 'TestInfo::name' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:692: warning: explicit link request to 'TestInfo::type_param' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:700: warning: explicit link request to 'TestInfo::value_param' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:708: warning: explicit link request to 'TestInfo::file' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:714: warning: explicit link request to 'TestInfo::line' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:720: warning: explicit link request to 'TestInfo::is_in_another_shard' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:726: warning: explicit link request to 'TestInfo::should_run' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:739: warning: explicit link request to 'TestInfo::is_reportable' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:745: warning: explicit link request to 'TestInfo::result' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:753: warning: explicit link request to 'testing::TestParamInfo' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:763: warning: explicit link request to 'testing::UnitTest' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:774: warning: explicit link request to 'UnitTest::GetInstance' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:782: warning: explicit link request to 'UnitTest::original_working_dir' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:789: warning: explicit link request to 'UnitTest::current_test_suite' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:796: warning: explicit link request to 'UnitTest::current_test_info' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:803: warning: explicit link request to 'UnitTest::random_seed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:809: warning: explicit link request to 'UnitTest::successful_test_suite_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:815: warning: explicit link request to 'UnitTest::failed_test_suite_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:821: warning: explicit link request to 'UnitTest::total_test_suite_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:827: warning: explicit link request to 'UnitTest::test_suite_to_run_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:834: warning: explicit link request to 'UnitTest::successful_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:840: warning: explicit link request to 'UnitTest::skipped_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:846: warning: explicit link request to 'UnitTest::failed_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:852: warning: explicit link request to 'UnitTest::reportable_disabled_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:858: warning: explicit link request to 'UnitTest::disabled_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:864: warning: explicit link request to 'UnitTest::reportable_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:870: warning: explicit link request to 'UnitTest::total_test_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:876: warning: explicit link request to 'UnitTest::test_to_run_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:882: warning: explicit link request to 'UnitTest::start_timestamp' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:888: warning: explicit link request to 'UnitTest::elapsed_time' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:894: warning: explicit link request to 'UnitTest::Passed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:900: warning: explicit link request to 'UnitTest::Failed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:907: warning: explicit link request to 'UnitTest::GetTestSuite' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:915: warning: explicit link request to 'UnitTest::ad_hoc_test_result' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:922: warning: explicit link request to 'UnitTest::listeners' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:931: warning: explicit link request to 'testing::TestEventListener' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:938: warning: explicit link request to 'TestEventListener::OnTestProgramStart' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:944: warning: explicit link request to 'TestEventListener::OnTestIterationStart' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:953: warning: explicit link request to 'TestEventListener::OnEnvironmentsSetUpStart' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:960: warning: explicit link request to 'TestEventListener::OnEnvironmentsSetUpEnd' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:967: warning: explicit link request to 'TestEventListener::OnTestSuiteStart' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:973: warning: explicit link request to 'TestEventListener::OnTestStart' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:979: warning: explicit link request to 'TestEventListener::OnTestPartResult' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:988: warning: explicit link request to 'TestEventListener::OnTestEnd' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:994: warning: explicit link request to 'TestEventListener::OnTestSuiteEnd' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1000: warning: explicit link request to 'TestEventListener::OnEnvironmentsTearDownStart' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1007: warning: explicit link request to 'TestEventListener::OnEnvironmentsTearDownEnd' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1014: warning: explicit link request to 'TestEventListener::OnTestIterationEnd' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1021: warning: explicit link request to 'TestEventListener::OnTestProgramEnd' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1029: warning: explicit link request to 'testing::TestEventListeners' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1035: warning: explicit link request to 'TestEventListeners::Append' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1043: warning: explicit link request to 'TestEventListeners::Release' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1051: warning: explicit link request to 'TestEventListeners::default_result_printer' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1061: warning: explicit link request to 'TestEventListeners::default_xml_generator' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1074: warning: explicit link request to 'testing::TestPartResult' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1081: warning: explicit link request to 'TestPartResult::type' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1098: warning: explicit link request to 'TestPartResult::file_name' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1105: warning: explicit link request to 'TestPartResult::line_number' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1112: warning: explicit link request to 'TestPartResult::summary' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1118: warning: explicit link request to 'TestPartResult::message' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1124: warning: explicit link request to 'TestPartResult::skipped' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1130: warning: explicit link request to 'TestPartResult::passed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1136: warning: explicit link request to 'TestPartResult::nonfatally_failed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1142: warning: explicit link request to 'TestPartResult::fatally_failed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1148: warning: explicit link request to 'TestPartResult::failed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1156: warning: explicit link request to 'testing::TestProperty' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1183: warning: explicit link request to 'testing::TestResult' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1191: warning: explicit link request to 'TestResult::total_part_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1198: warning: explicit link request to 'TestResult::test_property_count' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1204: warning: explicit link request to 'TestResult::Passed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1210: warning: explicit link request to 'TestResult::Skipped' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1216: warning: explicit link request to 'TestResult::Failed' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1222: warning: explicit link request to 'TestResult::HasFatalFailure' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1228: warning: explicit link request to 'TestResult::HasNonfatalFailure' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1234: warning: explicit link request to 'TestResult::elapsed_time' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1240: warning: explicit link request to 'TestResult::start_timestamp' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1246: warning: explicit link request to 'TestResult::GetTestPartResult' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1254: warning: explicit link request to 'TestResult::GetTestProperty' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1264: warning: explicit link request to 'testing::TimeInMillis' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1270: warning: explicit link request to 'testing::Types' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1286: warning: explicit link request to 'testing::WithParamInterface' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1312: warning: explicit link request to 'testing::InitGoogleTest(int* argc, char** argv)' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1313: warning: explicit link request to 'testing::InitGoogleTest(int* argc, wchar_t** argv)' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1314: warning: explicit link request to 'testing::InitGoogleTest()' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1331: warning: explicit link request to 'testing::AddGlobalTestEnvironment(Environment* env)' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1383: warning: explicit link request to 'testing::AssertionSuccess()' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1390: warning: explicit link request to 'testing::AssertionFailure()' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1403: warning: explicit link request to 'testing::StaticAssertTypeEq' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1412: warning: explicit link request to 'testing::PrintToString(x)' could not be resolved -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/docs/reference/testing.md:1422: warning: explicit link request to 'testing::PrintToStringParamName' could not be resolved -D:/C/VectorAlgebra/include/Vector2.h:233: warning: Member ToFactor(Vector2 a, Vector2 b) (function) of struct Vector2 is not documented. -D:/C/VectorAlgebra/include/Vector2.h:205: warning: argument 'axis' of command @param is not found in the argument list of Vector2::SignedAngle(Vector2 from, Vector2 to) -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:119: warning: Member testCatchesCxxExceptionsInFixtureConstructor(self) (function) of class googletest-catch-exceptions-test::CatchCxxExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:133: warning: Member testCatchesCxxExceptionsInFixtureDestructor(self) (function) of class googletest-catch-exceptions-test::CatchCxxExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:143: warning: Member testCatchesCxxExceptionsInSetUpTestCase(self) (function) of class googletest-catch-exceptions-test::CatchCxxExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:166: warning: Member testCatchesCxxExceptionsInTearDownTestCase(self) (function) of class googletest-catch-exceptions-test::CatchCxxExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:171: warning: Member testCatchesCxxExceptionsInSetUp(self) (function) of class googletest-catch-exceptions-test::CatchCxxExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:189: warning: Member testCatchesCxxExceptionsInTearDown(self) (function) of class googletest-catch-exceptions-test::CatchCxxExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:200: warning: Member testCatchesCxxExceptionsInTestBody(self) (function) of class googletest-catch-exceptions-test::CatchCxxExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:214: warning: Member testCatchesNonStdCxxExceptions(self) (function) of class googletest-catch-exceptions-test::CatchCxxExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:219: warning: Member testUnhandledCxxExceptionsAbortTheProgram(self) (function) of class googletest-catch-exceptions-test::CatchCxxExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:85: warning: Member TestSehExceptions(self, test_output) (function) of class googletest-catch-exceptions-test::CatchSehExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:103: warning: Member testCatchesSehExceptionsWithCxxExceptionsEnabled(self) (function) of class googletest-catch-exceptions-test::CatchSehExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-catch-exceptions-test.py:106: warning: Member testCatchesSehExceptionsWithCxxExceptionsDisabled(self) (function) of class googletest-catch-exceptions-test::CatchSehExceptionsTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-failfast-unittest.py:122: warning: Member testGoogletestFlag(self) (function) of class googletest-failfast-unittest::GTestFailFastUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-failfast-unittest.py:187: warning: Member testEventListener(self) (function) of class googletest-failfast-unittest::GTestFailFastUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-failfast-unittest.py:214: warning: Member assertXmlResultCount(self, result, count, xml) (function) of class googletest-failfast-unittest::GTestFailFastUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-failfast-unittest.py:220: warning: Member assertXmlStatusCount(self, status, count, xml) (function) of class googletest-failfast-unittest::GTestFailFastUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-filter-unittest.py:517: warning: Member testNegativeFilters(self) (function) of class googletest-filter-unittest::GTestFilterUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-json-outfiles-test.py:134: warning: Member setUp(self) (function) of class googletest-json-outfiles-test::GTestJsonOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-json-outfiles-test.py:142: warning: Member tearDown(self) (function) of class googletest-json-outfiles-test::GTestJsonOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-json-outfiles-test.py:145: warning: Member DeleteFilesAndDir(self) (function) of class googletest-json-outfiles-test::GTestJsonOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-json-outfiles-test.py:159: warning: Member testOutfile1(self) (function) of class googletest-json-outfiles-test::GTestJsonOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-json-outfiles-test.py:162: warning: Member testOutfile2(self) (function) of class googletest-json-outfiles-test::GTestJsonOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-json-outfiles-test.py:138: warning: Member output_dir_ (variable) of class googletest-json-outfiles-test::GTestJsonOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:170: warning: Member setUp(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:173: warning: Member testShufflePreservesNumberOfTests(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:179: warning: Member testShuffleChangesTestOrder(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:187: warning: Member testShuffleChangesTestCaseOrder(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:200: warning: Member testShuffleDoesNotRepeatTest(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:214: warning: Member testShuffleDoesNotCreateNewTest(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:224: warning: Member testShuffleIncludesAllTests(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:234: warning: Member testShuffleLeavesDeathTestsAtFront(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:253: warning: Member testShuffleDoesNotInterleaveTestCases(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:259: warning: Member testShuffleRestoresOrderAfterEachIteration(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:291: warning: Member testShuffleGeneratesNewOrderInEachIteration(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/googletest-shuffle-test.py:303: warning: Member testShuffleShardedTestsPreservesPartition(self) (function) of class googletest-shuffle-test::GTestShuffleUnitTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_help_test.py:140: warning: Member testPrintsHelpWithFullFlag(self) (function) of class gtest_help_test::GTestHelpTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_help_test.py:143: warning: Member testPrintsHelpWithShortFlag(self) (function) of class gtest_help_test::GTestHelpTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_help_test.py:146: warning: Member testPrintsHelpWithQuestionFlag(self) (function) of class gtest_help_test::GTestHelpTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_help_test.py:149: warning: Member testPrintsHelpWithWindowsStyleQuestionFlag(self) (function) of class gtest_help_test::GTestHelpTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_help_test.py:152: warning: Member testPrintsHelpWithUnrecognizedGoogleTestFlag(self) (function) of class gtest_help_test::GTestHelpTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_help_test.py:155: warning: Member testPrintsHelpWithIncorrectFlagStyle(self) (function) of class gtest_help_test::GTestHelpTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_xml_outfiles_test.py:75: warning: Member setUp(self) (function) of class gtest_xml_outfiles_test::GTestXMLOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_xml_outfiles_test.py:83: warning: Member tearDown(self) (function) of class gtest_xml_outfiles_test::GTestXMLOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_xml_outfiles_test.py:86: warning: Member DeleteFilesAndDir(self) (function) of class gtest_xml_outfiles_test::GTestXMLOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_xml_outfiles_test.py:100: warning: Member testOutfile1(self) (function) of class gtest_xml_outfiles_test::GTestXMLOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_xml_outfiles_test.py:103: warning: Member testOutfile2(self) (function) of class gtest_xml_outfiles_test::GTestXMLOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_xml_outfiles_test.py:79: warning: Member output_dir_ (variable) of class gtest_xml_outfiles_test::GTestXMLOutFilesTest is not documented. -D:/C/VectorAlgebra/out/build/x64-Debug/_deps/googletest-src/googletest/test/gtest_xml_test_utils.py:103: warning: Member identifying_attribute (variable) of class gtest_xml_test_utils::GTestXMLTestCase is not documented. + 'static Quaternion Passer::LinearAlgebra::Quaternion::LookRotation(const Vector3 &forward, const Vector3 &upwards)' at line 132 of file d:/PlatformIO/linear-algebra/Quaternion.h + 'static Quaternion Passer::LinearAlgebra::Quaternion::LookRotation(const Vector3 &forward)' at line 143 of file d:/PlatformIO/linear-algebra/Quaternion.h +d:/PlatformIO/linear-algebra/Quaternion.cpp:330: warning: no uniquely matching class member found for + Quaternion Quaternion::Euler(Vector3 euler) +Possible candidates: + 'static Quaternion Passer::LinearAlgebra::Quaternion::Euler(float x, float y, float z)' at line 215 of file d:/PlatformIO/linear-algebra/Quaternion.h + 'static Quaternion Passer::LinearAlgebra::Quaternion::Euler(Vector3 eulerAngles)' at line 222 of file d:/PlatformIO/linear-algebra/Quaternion.h +d:/PlatformIO/linear-algebra/Quaternion.cpp:362: warning: no uniquely matching class member found for + Quaternion Quaternion::EulerXYZ(Vector3 euler) +Possible candidates: + 'static Quaternion Passer::LinearAlgebra::Quaternion::EulerXYZ(float x, float y, float z)' at line 232 of file d:/PlatformIO/linear-algebra/Quaternion.h + 'static Quaternion Passer::LinearAlgebra::Quaternion::EulerXYZ(Vector3 eulerAngles)' at line 239 of file d:/PlatformIO/linear-algebra/Quaternion.h +d:/PlatformIO/linear-algebra/Spherical.cpp:137: warning: no uniquely matching class member found for + template < T > + SphericalOf< T > SphericalOf::operator-(const SphericalOf< T > &s2) const +Possible candidates: + 'AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator-() const' at line 99 of file d:/PlatformIO/linear-algebra/Angle.h + 'AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator-(const AngleOf< T > &angle) const' at line 103 of file d:/PlatformIO/linear-algebra/Angle.h + 'DirectionOf< T > Passer::LinearAlgebra::DirectionOf< T >::operator-() const' at line 84 of file d:/PlatformIO/linear-algebra/Direction.h + 'PolarOf Passer::LinearAlgebra::PolarOf< T >::operator-() const' at line 100 of file d:/PlatformIO/linear-algebra/Polar.h + 'PolarOf Passer::LinearAlgebra::PolarOf< T >::operator-(const PolarOf &v) const' at line 105 of file d:/PlatformIO/linear-algebra/Polar.h + 'SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator-() const' at line 93 of file d:/PlatformIO/linear-algebra/Spherical.h + 'SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator-(const SphericalOf< T > &v) const' at line 98 of file d:/PlatformIO/linear-algebra/Spherical.h + 'Vector2 Passer::LinearAlgebra::Vector2::operator-()' at line 116 of file d:/PlatformIO/linear-algebra/Vector2.h + 'Vector2 Passer::LinearAlgebra::Vector2::operator-(const Vector2 &v) const' at line 121 of file d:/PlatformIO/linear-algebra/Vector2.h + 'Vector3 Passer::LinearAlgebra::Vector3::operator-() const' at line 124 of file d:/PlatformIO/linear-algebra/Vector3.h + 'Vector3 Passer::LinearAlgebra::Vector3::operator-(const Vector3 &v) const' at line 129 of file d:/PlatformIO/linear-algebra/Vector3.h +d:/PlatformIO/linear-algebra/Vector2.cpp:20: warning: no uniquely matching class member found for + Vector2::Vector2(float _x, float _y) +Possible candidates: + 'Passer::LinearAlgebra::Vector2::Vector2()' at line 43 of file d:/PlatformIO/linear-algebra/Vector2.h + 'Passer::LinearAlgebra::Vector2::Vector2(float right, float forward)' at line 47 of file d:/PlatformIO/linear-algebra/Vector2.h + 'Passer::LinearAlgebra::Vector2::Vector2(Vector3 v)' at line 51 of file d:/PlatformIO/linear-algebra/Vector2.h + 'Passer::LinearAlgebra::Vector2::Vector2(PolarOf< float > v)' at line 54 of file d:/PlatformIO/linear-algebra/Vector2.h +d:/PlatformIO/linear-algebra/Vector2.cpp:32: warning: no uniquely matching class member found for + Vector2::Vector2(PolarSingle p) +Possible candidates: + 'Passer::LinearAlgebra::Vector2::Vector2()' at line 43 of file d:/PlatformIO/linear-algebra/Vector2.h + 'Passer::LinearAlgebra::Vector2::Vector2(float right, float forward)' at line 47 of file d:/PlatformIO/linear-algebra/Vector2.h + 'Passer::LinearAlgebra::Vector2::Vector2(Vector3 v)' at line 51 of file d:/PlatformIO/linear-algebra/Vector2.h + 'Passer::LinearAlgebra::Vector2::Vector2(PolarOf< float > v)' at line 54 of file d:/PlatformIO/linear-algebra/Vector2.h +d:/PlatformIO/linear-algebra/Vector3.cpp:33: warning: no uniquely matching class member found for + Vector3::Vector3(SphericalOf< float > s) +Possible candidates: + 'Passer::LinearAlgebra::Vector3::Vector3()' at line 47 of file d:/PlatformIO/linear-algebra/Vector3.h + 'Passer::LinearAlgebra::Vector3::Vector3(float right, float up, float forward)' at line 52 of file d:/PlatformIO/linear-algebra/Vector3.h + 'Passer::LinearAlgebra::Vector3::Vector3(Vector2 v)' at line 55 of file d:/PlatformIO/linear-algebra/Vector3.h + 'Passer::LinearAlgebra::Vector3::Vector3(SphericalOf< float > v)' at line 59 of file d:/PlatformIO/linear-algebra/Vector3.h +d:/PlatformIO/linear-algebra/Direction.h:43: warning: argument 'v' of command @param is not found in the argument list of Passer::LinearAlgebra::DirectionOf< T >::FromVector3(Vector3 vector) +d:/PlatformIO/linear-algebra/Direction.h:43: warning: The following parameter of Passer::LinearAlgebra::DirectionOf::FromVector3(Vector3 vector) is not documented: + parameter 'vector' +d:/PlatformIO/linear-algebra/Matrix.h:12: warning: Member MatrixOf(unsigned int rows, unsigned int cols) (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:13: warning: Member MatrixOf(unsigned int rows, unsigned int cols, const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:17: warning: Member MatrixOf(Vector3 v) (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:59: warning: Member Multiply(const MatrixOf< T > *m, MatrixOf< T > *r) const (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:64: warning: Member operator*(const Vector3 v) const (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:66: warning: Member Get(unsigned int rowIx, unsigned int colIx) const (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:71: warning: Member Set(unsigned int rowIx, unsigned int colIx, T value) (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:77: warning: Member Set(const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:84: warning: Member SetRow(unsigned int rowIx, const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:91: warning: Member SetCol(unsigned int colIx, const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:98: warning: Member CopyFrom(const MatrixOf< T > *m) (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:108: warning: Member RowCount() const (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:109: warning: Member ColCount() const (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:57: warning: Member Multiply(const MatrixOf< T > *m1, const MatrixOf< T > *m2, MatrixOf< T > *r) (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Matrix.h:63: warning: Member Multiply(const MatrixOf< T > *m, Vector3 v) (function) of class Passer::LinearAlgebra::MatrixOf is not documented. +d:/PlatformIO/linear-algebra/Polar.h:106: warning: Member operator-=(const PolarOf &v) (function) of class Passer::LinearAlgebra::PolarOf is not documented. +d:/PlatformIO/linear-algebra/Polar.h:111: warning: Member operator+=(const PolarOf &v) (function) of class Passer::LinearAlgebra::PolarOf is not documented. +d:/PlatformIO/linear-algebra/Polar.h:124: warning: Member operator*=(float f) (function) of class Passer::LinearAlgebra::PolarOf is not documented. +d:/PlatformIO/linear-algebra/Polar.h:136: warning: Member operator/=(float f) (function) of class Passer::LinearAlgebra::PolarOf is not documented. +d:/PlatformIO/linear-algebra/Polar.h:121: warning: Member operator*(float f, const PolarOf &v) (friend) of class Passer::LinearAlgebra::PolarOf is not documented. +d:/PlatformIO/linear-algebra/Polar.h:133: warning: Member operator/(float f, const PolarOf &v) (friend) of class Passer::LinearAlgebra::PolarOf is not documented. +d:/PlatformIO/linear-algebra/Polar.h:59: warning: argument 's' of command @param is not found in the argument list of Passer::LinearAlgebra::PolarOf< T >::FromSpherical(SphericalOf< T > v) +d:/PlatformIO/linear-algebra/Polar.h:59: warning: The following parameter of Passer::LinearAlgebra::PolarOf::FromSpherical(SphericalOf< T > v) is not documented: + parameter 'v' +d:/PlatformIO/linear-algebra/Spherical.h:29: warning: Member SphericalOf(float distance, AngleOf< T > horizontal, AngleOf< T > vertical) (function) of class Passer::LinearAlgebra::SphericalOf is not documented. +d:/PlatformIO/linear-algebra/Spherical.h:29: warning: Member SphericalOf(float distance, DirectionOf< T > direction) (function) of class Passer::LinearAlgebra::SphericalOf is not documented. +d:/PlatformIO/linear-algebra/Spherical.h:99: warning: Member operator-=(const SphericalOf< T > &v) (function) of class Passer::LinearAlgebra::SphericalOf is not documented. +d:/PlatformIO/linear-algebra/Spherical.h:104: warning: Member operator+=(const SphericalOf< T > &v) (function) of class Passer::LinearAlgebra::SphericalOf is not documented. +d:/PlatformIO/linear-algebra/Spherical.h:117: warning: Member operator*=(float f) (function) of class Passer::LinearAlgebra::SphericalOf is not documented. +d:/PlatformIO/linear-algebra/Spherical.h:129: warning: Member operator/=(float f) (function) of class Passer::LinearAlgebra::SphericalOf is not documented. +d:/PlatformIO/linear-algebra/Spherical.h:54: warning: Member Rad (variable) of class Passer::LinearAlgebra::SphericalOf is not documented. +d:/PlatformIO/linear-algebra/Spherical.h:114: warning: Member operator*(float f, const SphericalOf< T > &v) (friend) of class Passer::LinearAlgebra::SphericalOf is not documented. +d:/PlatformIO/linear-algebra/Spherical.h:126: warning: Member operator/(float f, const SphericalOf< T > &v) (friend) of class Passer::LinearAlgebra::SphericalOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:22: warning: Member SwingTwistOf(DirectionOf< T > swing, AngleOf< T > twist) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:22: warning: Member SwingTwistOf(AngleOf< T > horizontal, AngleOf< T > vertical, AngleOf< T > twist) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:31: warning: Member ToQuaternion() const (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:34: warning: Member ToAngleAxis() const (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:39: warning: Member operator==(const SwingTwistOf< T > d) const (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:55: warning: Member operator*=(const SwingTwistOf< T > &rotation) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:69: warning: Member Normalize() (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:28: warning: Member Degrees(float horizontal, float vertical=0, float twist=0) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:32: warning: Member FromQuaternion(Quaternion q) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:35: warning: Member FromAngleAxis(SphericalOf< T > aa) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:57: warning: Member Inverse(SwingTwistOf< T > rotation) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:67: warning: Member Angle(const SwingTwistOf< T > &r1, const SwingTwistOf< T > &r2) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:21: warning: Member swing (variable) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:22: warning: Member twist (variable) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/SwingTwist.h:37: warning: Member identity (variable) of class Passer::LinearAlgebra::SwingTwistOf is not documented. +d:/PlatformIO/linear-algebra/Vector2.h:122: warning: Member operator-=(const Vector2 &v) (function) of struct Passer::LinearAlgebra::Vector2 is not documented. +d:/PlatformIO/linear-algebra/Vector2.h:127: warning: Member operator+=(const Vector2 &v) (function) of struct Passer::LinearAlgebra::Vector2 is not documented. +d:/PlatformIO/linear-algebra/Vector2.h:148: warning: Member operator*=(float f) (function) of struct Passer::LinearAlgebra::Vector2 is not documented. +d:/PlatformIO/linear-algebra/Vector2.h:159: warning: Member operator/=(float f) (function) of struct Passer::LinearAlgebra::Vector2 is not documented. +d:/PlatformIO/linear-algebra/Vector2.h:144: warning: Member operator*(float f, const Vector2 &v) (friend) of struct Passer::LinearAlgebra::Vector2 is not documented. +d:/PlatformIO/linear-algebra/Vector2.h:156: warning: Member operator/(float f, const Vector2 &v) (friend) of struct Passer::LinearAlgebra::Vector2 is not documented. +d:/PlatformIO/linear-algebra/Vector3.h:82: warning: Member Forward() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented. +d:/PlatformIO/linear-algebra/Vector3.h:83: warning: Member Up() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented. +d:/PlatformIO/linear-algebra/Vector3.h:84: warning: Member Right() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented. +d:/PlatformIO/linear-algebra/Vector3.h:130: warning: Member operator-=(const Vector3 &v) (function) of struct Passer::LinearAlgebra::Vector3 is not documented. +d:/PlatformIO/linear-algebra/Vector3.h:135: warning: Member operator+=(const Vector3 &v) (function) of struct Passer::LinearAlgebra::Vector3 is not documented. +d:/PlatformIO/linear-algebra/Vector3.h:156: warning: Member operator*=(float f) (function) of struct Passer::LinearAlgebra::Vector3 is not documented. +d:/PlatformIO/linear-algebra/Vector3.h:168: warning: Member operator/=(float f) (function) of struct Passer::LinearAlgebra::Vector3 is not documented. +d:/PlatformIO/linear-algebra/Vector3.h:152: warning: Member operator*(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented. +d:/PlatformIO/linear-algebra/Vector3.h:164: warning: Member operator/(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented. diff --git a/DoxyGen/Doxyfile b/DoxyGen/Doxyfile index 8f5e552..e57d669 100644 --- a/DoxyGen/Doxyfile +++ b/DoxyGen/Doxyfile @@ -1,4 +1,4 @@ -# Doxyfile 1.9.2 +# Doxyfile 1.9.8 # This file describes the settings to be used by the documentation system # doxygen (www.doxygen.org) for a project. @@ -12,6 +12,16 @@ # For lists, items can also be appended using: # TAG += value [value, ...] # Values that contain spaces should be placed between quotes (\" \"). +# +# Note: +# +# Use doxygen to compare the used configuration file with the template +# configuration file: +# doxygen -x [configFile] +# Use doxygen to compare the used configuration file with the template +# configuration file without replacing the environment variables or CMake type +# replacement variables: +# doxygen -x_noenv [configFile] #--------------------------------------------------------------------------- # Project related configuration options @@ -32,7 +42,7 @@ DOXYFILE_ENCODING = UTF-8 # title of most generated pages and in a few other places. # The default value is: My Project. -PROJECT_NAME = VectorAlgebra +PROJECT_NAME = LinearAlgebra # The PROJECT_NUMBER tag can be used to enter a project or revision number. This # could be handy for archiving the generated documentation or if some version @@ -58,18 +68,30 @@ PROJECT_LOGO = //intranet/home/Afbeeldingen/PasserVR/Logos/Logo3NameRi # entered, it will be relative to the location where doxygen was started. If # left blank the current directory will be used. -OUTPUT_DIRECTORY = //serrarens.nl/web/apis/VectorAlgebra +OUTPUT_DIRECTORY = //intranet/web/passer_life/apis/LinearAlgebra -# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- -# directories (in 2 levels) under the output directory of each output format and -# will distribute the generated files over these directories. Enabling this +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create up to 4096 +# sub-directories (in 2 levels) under the output directory of each output format +# and will distribute the generated files over these directories. Enabling this # option can be useful when feeding doxygen a huge amount of source files, where # putting all generated files in the same directory would otherwise causes -# performance problems for the file system. +# performance problems for the file system. Adapt CREATE_SUBDIRS_LEVEL to +# control the number of sub-directories. # The default value is: NO. CREATE_SUBDIRS = NO +# Controls the number of sub-directories that will be created when +# CREATE_SUBDIRS tag is set to YES. Level 0 represents 16 directories, and every +# level increment doubles the number of directories, resulting in 4096 +# directories at level 8 which is the default and also the maximum value. The +# sub-directories are organized in 2 levels, the first level always has a fixed +# number of 16 directories. +# Minimum value: 0, maximum value: 8, default value: 8. +# This tag requires that the tag CREATE_SUBDIRS is set to YES. + +CREATE_SUBDIRS_LEVEL = 8 + # If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII # characters to appear in the names of generated files. If set to NO, non-ASCII # characters will be escaped, for example _xE3_x81_x84 will be used for Unicode @@ -81,14 +103,14 @@ ALLOW_UNICODE_NAMES = NO # The OUTPUT_LANGUAGE tag is used to specify the language in which all # documentation generated by doxygen is written. Doxygen will use this # information to generate all constant output in the proper language. -# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, -# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), -# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, -# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), -# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, -# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, -# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, -# Ukrainian and Vietnamese. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Bulgarian, +# Catalan, Chinese, Chinese-Traditional, Croatian, Czech, Danish, Dutch, English +# (United States), Esperanto, Farsi (Persian), Finnish, French, German, Greek, +# Hindi, Hungarian, Indonesian, Italian, Japanese, Japanese-en (Japanese with +# English messages), Korean, Korean-en (Korean with English messages), Latvian, +# Lithuanian, Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, +# Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, +# Swedish, Turkish, Ukrainian and Vietnamese. # The default value is: English. OUTPUT_LANGUAGE = English @@ -341,6 +363,17 @@ MARKDOWN_SUPPORT = YES TOC_INCLUDE_HEADINGS = 0 +# The MARKDOWN_ID_STYLE tag can be used to specify the algorithm used to +# generate identifiers for the Markdown headings. Note: Every identifier is +# unique. +# Possible values are: DOXYGEN use a fixed 'autotoc_md' string followed by a +# sequence number starting at 0 and GITHUB use the lower case version of title +# with any whitespace replaced by '-' and punctuation characters removed. +# The default value is: DOXYGEN. +# This tag requires that the tag MARKDOWN_SUPPORT is set to YES. + +MARKDOWN_ID_STYLE = DOXYGEN + # When enabled doxygen tries to link words that correspond to documented # classes, or namespaces to their corresponding documentation. Such a link can # be prevented in individual cases by putting a % sign in front of the word or @@ -452,7 +485,7 @@ TYPEDEF_HIDES_STRUCT = NO LOOKUP_CACHE_SIZE = 0 -# The NUM_PROC_THREADS specifies the number threads doxygen is allowed to use +# The NUM_PROC_THREADS specifies the number of threads doxygen is allowed to use # during processing. When set to 0 doxygen will based this on the number of # cores available in the system. You can set it explicitly to a value larger # than 0 to get more control over the balance between CPU load and processing @@ -465,6 +498,14 @@ LOOKUP_CACHE_SIZE = 0 NUM_PROC_THREADS = 1 +# If the TIMESTAMP tag is set different from NO then each generated page will +# contain the date or date and time when the page was generated. Setting this to +# NO can help when comparing the output of multiple runs. +# Possible values are: YES, NO, DATETIME and DATE. +# The default value is: NO. + +TIMESTAMP = NO + #--------------------------------------------------------------------------- # Build related configuration options #--------------------------------------------------------------------------- @@ -546,7 +587,8 @@ HIDE_UNDOC_MEMBERS = NO # If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all # undocumented classes that are normally visible in the class hierarchy. If set # to NO, these classes will be included in the various overviews. This option -# has no effect if EXTRACT_ALL is enabled. +# will also hide undocumented C++ concepts if enabled. This option has no effect +# if EXTRACT_ALL is enabled. # The default value is: NO. HIDE_UNDOC_CLASSES = YES @@ -577,14 +619,15 @@ INTERNAL_DOCS = NO # filesystem is case sensitive (i.e. it supports files in the same directory # whose names only differ in casing), the option must be set to YES to properly # deal with such files in case they appear in the input. For filesystems that -# are not case sensitive the option should be be set to NO to properly deal with +# are not case sensitive the option should be set to NO to properly deal with # output files written for symbols that only differ in casing, such as for two # classes, one named CLASS and the other named Class, and to also support # references to files without having to specify the exact matching casing. On # Windows (including Cygwin) and MacOS, users should typically set this option # to NO, whereas on Linux or other Unix flavors it should typically be set to # YES. -# The default value is: system dependent. +# Possible values are: SYSTEM, NO and YES. +# The default value is: SYSTEM. CASE_SENSE_NAMES = NO @@ -836,11 +879,26 @@ WARN_IF_INCOMPLETE_DOC = YES WARN_NO_PARAMDOC = NO +# If WARN_IF_UNDOC_ENUM_VAL option is set to YES, doxygen will warn about +# undocumented enumeration values. If set to NO, doxygen will accept +# undocumented enumeration values. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: NO. + +WARN_IF_UNDOC_ENUM_VAL = NO + # If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when # a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS # then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but # at the end of the doxygen process doxygen will return with a non-zero status. -# Possible values are: NO, YES and FAIL_ON_WARNINGS. +# If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS_PRINT then doxygen behaves +# like FAIL_ON_WARNINGS but in case no WARN_LOGFILE is defined doxygen will not +# write the warning messages in between other messages but write them at the end +# of a run, in case a WARN_LOGFILE is defined the warning messages will be +# besides being in the defined file also be shown at the end of a run, unless +# the WARN_LOGFILE is defined as - i.e. standard output (stdout) in that case +# the behavior will remain as with the setting FAIL_ON_WARNINGS. +# Possible values are: NO, YES, FAIL_ON_WARNINGS and FAIL_ON_WARNINGS_PRINT. # The default value is: NO. WARN_AS_ERROR = NO @@ -851,13 +909,27 @@ WARN_AS_ERROR = NO # and the warning text. Optionally the format may contain $version, which will # be replaced by the version of the file (if it could be obtained via # FILE_VERSION_FILTER) +# See also: WARN_LINE_FORMAT # The default value is: $file:$line: $text. WARN_FORMAT = "$file:$line: $text" +# In the $text part of the WARN_FORMAT command it is possible that a reference +# to a more specific place is given. To make it easier to jump to this place +# (outside of doxygen) the user can define a custom "cut" / "paste" string. +# Example: +# WARN_LINE_FORMAT = "'vi $file +$line'" +# See also: WARN_FORMAT +# The default value is: at line $line of file $file. + +WARN_LINE_FORMAT = "at line $line of file $file" + # The WARN_LOGFILE tag can be used to specify a file to which warning and error # messages should be written. If left blank the output is written to standard -# error (stderr). +# error (stderr). In case the file specified cannot be opened for writing the +# warning and error messages are written to standard error. When as file - is +# specified the warning and error messages are written to standard output +# (stdout). WARN_LOGFILE = DoxyWarnLogfile.txt @@ -871,8 +943,7 @@ WARN_LOGFILE = DoxyWarnLogfile.txt # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING # Note: If this tag is empty the current directory is searched. -INPUT = ../include \ - ../src \ +INPUT = .. \ ../README.md # This tag can be used to specify the character encoding of the source files @@ -880,10 +951,21 @@ INPUT = ../include \ # libiconv (or the iconv built into libc) for the transcoding. See the libiconv # documentation (see: # https://www.gnu.org/software/libiconv/) for the list of possible encodings. +# See also: INPUT_FILE_ENCODING # The default value is: UTF-8. INPUT_ENCODING = UTF-8 +# This tag can be used to specify the character encoding of the source files +# that doxygen parses The INPUT_FILE_ENCODING tag can be used to specify +# character encoding on a per file pattern basis. Doxygen will compare the file +# name with each pattern and apply the encoding instead of the default +# INPUT_ENCODING) if there is a match. The character encodings are a list of the +# form: pattern=encoding (like *.php=ISO-8859-1). See cfg_input_encoding +# "INPUT_ENCODING" for further information on supported encodings. + +INPUT_FILE_ENCODING = + # If the value of the INPUT tag contains directories, you can use the # FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and # *.h) to filter out the source-files in the directories. @@ -895,12 +977,12 @@ INPUT_ENCODING = UTF-8 # Note the list of default checked file patterns might differ from the list of # default file extension mappings. # -# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, -# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, -# *.hh, *.hxx, *.hpp, *.h++, *.l, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, -# *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C -# comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd, -# *.vhdl, *.ucf, *.qsf and *.ice. +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cxxm, +# *.cpp, *.cppm, *.c++, *.c++m, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, +# *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp, *.h++, *.ixx, *.l, *.cs, *.d, *.php, +# *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to be +# provided as doxygen C comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, +# *.f18, *.f, *.for, *.vhd, *.vhdl, *.ucf, *.qsf and *.ice. FILE_PATTERNS = *.c \ *.cc \ @@ -978,16 +1060,14 @@ EXCLUDE_SYMLINKS = NO # Note that the wildcards are matched against the file with absolute path, so to # exclude all test directories for example use the pattern */test/* -EXCLUDE_PATTERNS = +EXCLUDE_PATTERNS = gtest* \ + googletest* # The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names # (namespaces, classes, functions, etc.) that should be excluded from the # output. The symbol name can be a fully qualified name, a word, or if the # wildcard * is used, a substring. Examples: ANamespace, AClass, -# AClass::ANamespace, ANamespace::*Test -# -# Note that the wildcards are matched against the file with absolute path, so to -# exclude all test directories use the pattern */test/* +# ANamespace::AClass, ANamespace::*Test EXCLUDE_SYMBOLS = @@ -1033,6 +1113,11 @@ IMAGE_PATH = images \ # code is scanned, but not when the output code is generated. If lines are added # or removed, the anchors will not be placed correctly. # +# Note that doxygen will use the data processed and written to standard output +# for further processing, therefore nothing else, like debug statements or used +# commands (so in case of a Windows batch file always use @echo OFF), should be +# written to standard output. +# # Note that for custom extensions or not directly supported extensions you also # need to set EXTENSION_MAPPING for the extension otherwise the files are not # properly processed by doxygen. @@ -1074,6 +1159,15 @@ FILTER_SOURCE_PATTERNS = USE_MDFILE_AS_MAINPAGE = +# The Fortran standard specifies that for fixed formatted Fortran code all +# characters from position 72 are to be considered as comment. A common +# extension is to allow longer lines before the automatic comment starts. The +# setting FORTRAN_COMMENT_AFTER will also make it possible that longer lines can +# be processed before the automatic comment starts. +# Minimum value: 7, maximum value: 10000, default value: 72. + +FORTRAN_COMMENT_AFTER = 72 + #--------------------------------------------------------------------------- # Configuration options related to source browsing #--------------------------------------------------------------------------- @@ -1211,10 +1305,11 @@ CLANG_DATABASE_PATH = ALPHABETICAL_INDEX = YES -# In case all classes in a project start with a common prefix, all classes will -# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag -# can be used to specify a prefix (or a list of prefixes) that should be ignored -# while generating the index headers. +# The IGNORE_PREFIX tag can be used to specify a prefix (or a list of prefixes) +# that should be ignored while generating the index headers. The IGNORE_PREFIX +# tag works for classes, function and member names. The entity will be placed in +# the alphabetical list under the first letter of the entity name that remains +# after removing the prefix. # This tag requires that the tag ALPHABETICAL_INDEX is set to YES. IGNORE_PREFIX = @@ -1293,7 +1388,12 @@ HTML_STYLESHEET = # Doxygen will copy the style sheet files to the output directory. # Note: The order of the extra style sheet files is of importance (e.g. the last # style sheet in the list overrules the setting of the previous ones in the -# list). For an example see the documentation. +# list). +# Note: Since the styling of scrollbars can currently not be overruled in +# Webkit/Chromium, the styling will be left out of the default doxygen.css if +# one or more extra stylesheets have been specified. So if scrollbar +# customization is desired it has to be added explicitly. For an example see the +# documentation. # This tag requires that the tag GENERATE_HTML is set to YES. HTML_EXTRA_STYLESHEET = custom_doxygen.css @@ -1308,6 +1408,19 @@ HTML_EXTRA_STYLESHEET = custom_doxygen.css HTML_EXTRA_FILES = +# The HTML_COLORSTYLE tag can be used to specify if the generated HTML output +# should be rendered with a dark or light theme. +# Possible values are: LIGHT always generate light mode output, DARK always +# generate dark mode output, AUTO_LIGHT automatically set the mode according to +# the user preference, use light mode if no preference is set (the default), +# AUTO_DARK automatically set the mode according to the user preference, use +# dark mode if no preference is set and TOGGLE allow to user to switch between +# light and dark mode via a button. +# The default value is: AUTO_LIGHT. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE = LIGHT + # The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen # will adjust the colors in the style sheet and background images according to # this color. Hue is specified as an angle on a color-wheel, see @@ -1338,15 +1451,6 @@ HTML_COLORSTYLE_SAT = 0 HTML_COLORSTYLE_GAMMA = 103 -# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML -# page will contain the date and time when the page was generated. Setting this -# to YES can help to show when doxygen was last run and thus if the -# documentation is up to date. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_TIMESTAMP = NO - # If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML # documentation will contain a main index with vertical navigation menus that # are dynamically created via JavaScript. If disabled, the navigation index will @@ -1366,6 +1470,13 @@ HTML_DYNAMIC_MENUS = YES HTML_DYNAMIC_SECTIONS = NO +# If the HTML_CODE_FOLDING tag is set to YES then classes and functions can be +# dynamically folded and expanded in the generated HTML source code. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_CODE_FOLDING = YES + # With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries # shown in the various tree structured indices initially; the user can expand # and collapse entries dynamically later on. Doxygen will expand the tree to @@ -1402,6 +1513,13 @@ GENERATE_DOCSET = NO DOCSET_FEEDNAME = "Doxygen generated docs" +# This tag determines the URL of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDURL = + # This tag specifies a string that should uniquely identify the documentation # set bundle. This should be a reverse domain-name style string, e.g. # com.mycompany.MyDocSet. Doxygen will append .docset to the name. @@ -1489,6 +1607,16 @@ BINARY_TOC = NO TOC_EXPAND = NO +# The SITEMAP_URL tag is used to specify the full URL of the place where the +# generated documentation will be placed on the server by the user during the +# deployment of the documentation. The generated sitemap is called sitemap.xml +# and placed on the directory specified by HTML_OUTPUT. In case no SITEMAP_URL +# is specified no sitemap is generated. For information about the sitemap +# protocol see https://www.sitemaps.org +# This tag requires that the tag GENERATE_HTML is set to YES. + +SITEMAP_URL = + # If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and # QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that # can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help @@ -1606,7 +1734,7 @@ GENERATE_TREEVIEW = NO # area (value NO) or if it should extend to the full height of the window (value # YES). Setting this to YES gives a layout similar to # https://docs.readthedocs.io with more room for contents, but less room for the -# project logo, title, and description. If either GENERATOR_TREEVIEW or +# project logo, title, and description. If either GENERATE_TREEVIEW or # DISABLE_INDEX is set to NO, this option has no effect. # The default value is: NO. # This tag requires that the tag GENERATE_HTML is set to YES. @@ -1637,6 +1765,13 @@ TREEVIEW_WIDTH = 250 EXT_LINKS_IN_WINDOW = NO +# If the OBFUSCATE_EMAILS tag is set to YES, doxygen will obfuscate email +# addresses. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +OBFUSCATE_EMAILS = YES + # If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg # tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see # https://inkscape.org) to generate formulas as SVG images instead of PNGs for @@ -1657,17 +1792,6 @@ HTML_FORMULA_FORMAT = png FORMULA_FONTSIZE = 10 -# Use the FORMULA_TRANSPARENT tag to determine whether or not the images -# generated for formulas are transparent PNGs. Transparent PNGs are not -# supported properly for IE 6.0, but are supported on all modern browsers. -# -# Note that when changing this option you need to delete any form_*.png files in -# the HTML output directory before the changes have effect. -# The default value is: YES. -# This tag requires that the tag GENERATE_HTML is set to YES. - -FORMULA_TRANSPARENT = YES - # The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands # to create new LaTeX commands to be used in formulas as building blocks. See # the section "Including formulas" for details. @@ -1981,9 +2105,16 @@ PDF_HYPERLINKS = YES USE_PDFLATEX = YES -# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode -# command to the generated LaTeX files. This will instruct LaTeX to keep running -# if errors occur, instead of asking the user for help. +# The LATEX_BATCHMODE tag signals the behavior of LaTeX in case of an error. +# Possible values are: NO same as ERROR_STOP, YES same as BATCH, BATCH In batch +# mode nothing is printed on the terminal, errors are scrolled as if is +# hit at every error; missing files that TeX tries to input or request from +# keyboard input (\read on a not open input stream) cause the job to abort, +# NON_STOP In nonstop mode the diagnostic message will appear on the terminal, +# but there is no possibility of user interaction just like in batch mode, +# SCROLL In scroll mode, TeX will stop only for missing files to input or if +# keyboard input is necessary and ERROR_STOP In errorstop mode, TeX will stop at +# each error, asking for user intervention. # The default value is: NO. # This tag requires that the tag GENERATE_LATEX is set to YES. @@ -2004,14 +2135,6 @@ LATEX_HIDE_INDICES = NO LATEX_BIB_STYLE = plain -# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated -# page will contain the date and time when the page was generated. Setting this -# to NO can help when comparing the output of multiple runs. -# The default value is: NO. -# This tag requires that the tag GENERATE_LATEX is set to YES. - -LATEX_TIMESTAMP = NO - # The LATEX_EMOJI_DIRECTORY tag is used to specify the (relative or absolute) # path from which the emoji images will be read. If a relative path is entered, # it will be relative to the LATEX_OUTPUT directory. If left blank the @@ -2177,7 +2300,7 @@ DOCBOOK_OUTPUT = docbook #--------------------------------------------------------------------------- # If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an -# AutoGen Definitions (see http://autogen.sourceforge.net/) file that captures +# AutoGen Definitions (see https://autogen.sourceforge.net/) file that captures # the structure of the code including all documentation. Note that this feature # is still experimental and incomplete at the moment. # The default value is: NO. @@ -2188,6 +2311,28 @@ GENERATE_AUTOGEN_DEF = NO # Configuration options related to Sqlite3 output #--------------------------------------------------------------------------- +# If the GENERATE_SQLITE3 tag is set to YES doxygen will generate a Sqlite3 +# database with symbols found by doxygen stored in tables. +# The default value is: NO. + +GENERATE_SQLITE3 = NO + +# The SQLITE3_OUTPUT tag is used to specify where the Sqlite3 database will be +# put. If a relative path is entered the value of OUTPUT_DIRECTORY will be put +# in front of it. +# The default directory is: sqlite3. +# This tag requires that the tag GENERATE_SQLITE3 is set to YES. + +SQLITE3_OUTPUT = sqlite3 + +# The SQLITE3_OVERWRITE_DB tag is set to YES, the existing doxygen_sqlite3.db +# database file will be recreated with each doxygen run. If set to NO, doxygen +# will warn if an a database file is already found and not modify it. +# The default value is: YES. +# This tag requires that the tag GENERATE_SQLITE3 is set to YES. + +SQLITE3_RECREATE_DB = YES + #--------------------------------------------------------------------------- # Configuration options related to the Perl module output #--------------------------------------------------------------------------- @@ -2262,7 +2407,8 @@ SEARCH_INCLUDES = YES # The INCLUDE_PATH tag can be used to specify one or more directories that # contain include files that are not input files but should be processed by the -# preprocessor. +# preprocessor. Note that the INCLUDE_PATH is not recursive, so the setting of +# RECURSIVE has no effect here. # This tag requires that the tag SEARCH_INCLUDES is set to YES. INCLUDE_PATH = @@ -2329,15 +2475,15 @@ TAGFILES = GENERATE_TAGFILE = -# If the ALLEXTERNALS tag is set to YES, all external class will be listed in -# the class index. If set to NO, only the inherited external classes will be -# listed. +# If the ALLEXTERNALS tag is set to YES, all external classes and namespaces +# will be listed in the class and namespace index. If set to NO, only the +# inherited external classes will be listed. # The default value is: NO. ALLEXTERNALS = NO # If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed -# in the modules index. If set to NO, only the current project's groups will be +# in the topic index. If set to NO, only the current project's groups will be # listed. # The default value is: YES. @@ -2351,25 +2497,9 @@ EXTERNAL_GROUPS = YES EXTERNAL_PAGES = YES #--------------------------------------------------------------------------- -# Configuration options related to the dot tool +# Configuration options related to diagram generator tools #--------------------------------------------------------------------------- -# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram -# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to -# NO turns the diagrams off. Note that this option also works with HAVE_DOT -# disabled, but it is recommended to install and use dot, since it yields more -# powerful graphs. -# The default value is: YES. - -CLASS_DIAGRAMS = YES - -# You can include diagrams made with dia in doxygen documentation. Doxygen will -# then run dia to produce the diagram and insert it in the documentation. The -# DIA_PATH tag allows you to specify the directory where the dia binary resides. -# If left empty dia is assumed to be found in the default search path. - -DIA_PATH = - # If set to YES the inheritance and collaboration graphs will hide inheritance # and usage relations if the target is undocumented or is not a class. # The default value is: YES. @@ -2378,7 +2508,7 @@ HIDE_UNDOC_RELATIONS = YES # If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is # available from the path. This tool is part of Graphviz (see: -# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent +# https://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent # Bell Labs. The other options in this section have no effect if this option is # set to NO # The default value is: NO. @@ -2395,49 +2525,73 @@ HAVE_DOT = NO DOT_NUM_THREADS = 0 -# When you want a differently looking font in the dot files that doxygen -# generates you can specify the font name using DOT_FONTNAME. You need to make -# sure dot is able to find the font, which can be done by putting it in a -# standard location or by setting the DOTFONTPATH environment variable or by -# setting DOT_FONTPATH to the directory containing the font. -# The default value is: Helvetica. +# DOT_COMMON_ATTR is common attributes for nodes, edges and labels of +# subgraphs. When you want a differently looking font in the dot files that +# doxygen generates you can specify fontname, fontcolor and fontsize attributes. +# For details please see Node, +# Edge and Graph Attributes specification You need to make sure dot is able +# to find the font, which can be done by putting it in a standard location or by +# setting the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the +# directory containing the font. Default graphviz fontsize is 14. +# The default value is: fontname=Helvetica,fontsize=10. # This tag requires that the tag HAVE_DOT is set to YES. -DOT_FONTNAME = Helvetica +DOT_COMMON_ATTR = "fontname=Helvetica,fontsize=10" -# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of -# dot graphs. -# Minimum value: 4, maximum value: 24, default value: 10. +# DOT_EDGE_ATTR is concatenated with DOT_COMMON_ATTR. For elegant style you can +# add 'arrowhead=open, arrowtail=open, arrowsize=0.5'. Complete documentation about +# arrows shapes. +# The default value is: labelfontname=Helvetica,labelfontsize=10. # This tag requires that the tag HAVE_DOT is set to YES. -DOT_FONTSIZE = 10 +DOT_EDGE_ATTR = "labelfontname=Helvetica,labelfontsize=10" -# By default doxygen will tell dot to use the default font as specified with -# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set -# the path where dot can find it using this tag. +# DOT_NODE_ATTR is concatenated with DOT_COMMON_ATTR. For view without boxes +# around nodes set 'shape=plain' or 'shape=plaintext' Shapes specification +# The default value is: shape=box,height=0.2,width=0.4. +# This tag requires that the tag HAVE_DOT is set to YES. + +DOT_NODE_ATTR = "shape=box,height=0.2,width=0.4" + +# You can set the path where dot can find font specified with fontname in +# DOT_COMMON_ATTR and others dot attributes. # This tag requires that the tag HAVE_DOT is set to YES. DOT_FONTPATH = -# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for -# each documented class showing the direct and indirect inheritance relations. -# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO. +# If the CLASS_GRAPH tag is set to YES or GRAPH or BUILTIN then doxygen will +# generate a graph for each documented class showing the direct and indirect +# inheritance relations. In case the CLASS_GRAPH tag is set to YES or GRAPH and +# HAVE_DOT is enabled as well, then dot will be used to draw the graph. In case +# the CLASS_GRAPH tag is set to YES and HAVE_DOT is disabled or if the +# CLASS_GRAPH tag is set to BUILTIN, then the built-in generator will be used. +# If the CLASS_GRAPH tag is set to TEXT the direct and indirect inheritance +# relations will be shown as texts / links. +# Possible values are: NO, YES, TEXT, GRAPH and BUILTIN. # The default value is: YES. -# This tag requires that the tag HAVE_DOT is set to YES. CLASS_GRAPH = YES # If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a # graph for each documented class showing the direct and indirect implementation # dependencies (inheritance, containment, and class references variables) of the -# class with other documented classes. +# class with other documented classes. Explicit enabling a collaboration graph, +# when COLLABORATION_GRAPH is set to NO, can be accomplished by means of the +# command \collaborationgraph. Disabling a collaboration graph can be +# accomplished by means of the command \hidecollaborationgraph. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. COLLABORATION_GRAPH = YES # If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for -# groups, showing the direct groups dependencies. +# groups, showing the direct groups dependencies. Explicit enabling a group +# dependency graph, when GROUP_GRAPHS is set to NO, can be accomplished by means +# of the command \groupgraph. Disabling a directory graph can be accomplished by +# means of the command \hidegroupgraph. See also the chapter Grouping in the +# manual. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. @@ -2497,7 +2651,9 @@ TEMPLATE_RELATIONS = NO # If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to # YES then doxygen will generate a graph for each documented file showing the # direct and indirect include dependencies of the file with other documented -# files. +# files. Explicit enabling an include graph, when INCLUDE_GRAPH is is set to NO, +# can be accomplished by means of the command \includegraph. Disabling an +# include graph can be accomplished by means of the command \hideincludegraph. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. @@ -2506,7 +2662,10 @@ INCLUDE_GRAPH = YES # If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are # set to YES then doxygen will generate a graph for each documented file showing # the direct and indirect include dependencies of the file with other documented -# files. +# files. Explicit enabling an included by graph, when INCLUDED_BY_GRAPH is set +# to NO, can be accomplished by means of the command \includedbygraph. Disabling +# an included by graph can be accomplished by means of the command +# \hideincludedbygraph. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. @@ -2546,16 +2705,26 @@ GRAPHICAL_HIERARCHY = YES # If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the # dependencies a directory has on other directories in a graphical way. The # dependency relations are determined by the #include relations between the -# files in the directories. +# files in the directories. Explicit enabling a directory graph, when +# DIRECTORY_GRAPH is set to NO, can be accomplished by means of the command +# \directorygraph. Disabling a directory graph can be accomplished by means of +# the command \hidedirectorygraph. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. DIRECTORY_GRAPH = YES +# The DIR_GRAPH_MAX_DEPTH tag can be used to limit the maximum number of levels +# of child directories generated in directory dependency graphs by dot. +# Minimum value: 1, maximum value: 25, default value: 1. +# This tag requires that the tag DIRECTORY_GRAPH is set to YES. + +DIR_GRAPH_MAX_DEPTH = 1 + # The DOT_IMAGE_FORMAT tag can be used to set the image format of the images # generated by dot. For an explanation of the image formats see the section # output formats in the documentation of the dot tool (Graphviz (see: -# http://www.graphviz.org/)). +# https://www.graphviz.org/)). # Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order # to make the SVG files visible in IE 9+ (other browsers do not have this # requirement). @@ -2592,11 +2761,12 @@ DOT_PATH = DOTFILE_DIRS = -# The MSCFILE_DIRS tag can be used to specify one or more directories that -# contain msc files that are included in the documentation (see the \mscfile -# command). +# You can include diagrams made with dia in doxygen documentation. Doxygen will +# then run dia to produce the diagram and insert it in the documentation. The +# DIA_PATH tag allows you to specify the directory where the dia binary resides. +# If left empty dia is assumed to be found in the default search path. -MSCFILE_DIRS = +DIA_PATH = # The DIAFILE_DIRS tag can be used to specify one or more directories that # contain dia files that are included in the documentation (see the \diafile @@ -2605,10 +2775,10 @@ MSCFILE_DIRS = DIAFILE_DIRS = # When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the -# path where java can find the plantuml.jar file. If left blank, it is assumed -# PlantUML is not used or called during a preprocessing step. Doxygen will -# generate a warning when it encounters a \startuml command in this case and -# will not generate output for the diagram. +# path where java can find the plantuml.jar file or to the filename of jar file +# to be used. If left blank, it is assumed PlantUML is not used or called during +# a preprocessing step. Doxygen will generate a warning when it encounters a +# \startuml command in this case and will not generate output for the diagram. PLANTUML_JAR_PATH = @@ -2646,18 +2816,6 @@ DOT_GRAPH_MAX_NODES = 50 MAX_DOT_GRAPH_DEPTH = 0 -# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent -# background. This is disabled by default, because dot on Windows does not seem -# to support this out of the box. -# -# Warning: Depending on the platform used, enabling this option may lead to -# badly anti-aliased labels on the edges of a graph (i.e. they become hard to -# read). -# The default value is: NO. -# This tag requires that the tag HAVE_DOT is set to YES. - -DOT_TRANSPARENT = NO - # Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output # files in one run (i.e. multiple -o and -T options on the command line). This # makes dot run faster, but since only newer versions of dot (>1.8.10) support @@ -2670,6 +2828,8 @@ DOT_MULTI_TARGETS = NO # If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page # explaining the meaning of the various boxes and arrows in the dot generated # graphs. +# Note: This tag requires that UML_LOOK isn't set, i.e. the doxygen internal +# graphical representation for inheritance and collaboration diagrams is used. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. @@ -2683,3 +2843,19 @@ GENERATE_LEGEND = YES # The default value is: YES. DOT_CLEANUP = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. If the MSCGEN_TOOL tag is left empty (the default), then doxygen will +# use a built-in version of mscgen tool to produce the charts. Alternatively, +# the MSCGEN_TOOL tag can also specify the name an external tool. For instance, +# specifying prog as the value, doxygen will call the tool as prog -T +# -o . The external tool should support +# output file formats "png", "eps", "svg", and "ismap". + +MSCGEN_TOOL = + +# The MSCFILE_DIRS tag can be used to specify one or more directories that +# contain msc files that are included in the documentation (see the \mscfile +# command). + +MSCFILE_DIRS = diff --git a/FloatSingle.cpp b/FloatSingle.cpp index fb6f301..902d85e 100644 --- a/FloatSingle.cpp +++ b/FloatSingle.cpp @@ -2,16 +2,18 @@ // License, v. 2.0.If a copy of the MPL was not distributed with this // file, You can obtain one at https ://mozilla.org/MPL/2.0/. -#include #include "FloatSingle.h" +#include const float Float::epsilon = 1e-05f; const float Float::sqrEpsilon = 1e-10f; float Float::Clamp(float f, float min, float max) { - if (f < min) - return min; - if (f > max) - return max; + if (max < min) return f; + if (f < min) + return min; + if (f > max) + return max; + return f; } \ No newline at end of file diff --git a/FloatSingle.h b/FloatSingle.h index 7aa2251..db3e06e 100644 --- a/FloatSingle.h +++ b/FloatSingle.h @@ -5,11 +5,18 @@ #ifndef FLOAT_H #define FLOAT_H -class Float { -public: - static const float epsilon; - static const float sqrEpsilon; +namespace LinearAlgebra { - static float Clamp(float f, float min, float max); +class Float { + public: + static const float epsilon; + static const float sqrEpsilon; + + static float Clamp(float f, float min, float max); }; + +} // namespace LinearAlgebra + +using namespace LinearAlgebra; + #endif diff --git a/Matrix.cpp b/Matrix.cpp index 7a67504..c7e53c7 100644 --- a/Matrix.cpp +++ b/Matrix.cpp @@ -1,6 +1,290 @@ #include "Matrix.h" +#if !defined(NO_STD) +#include +#endif -template <> MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { +namespace LinearAlgebra { + +#pragma region Matrix1 + +Matrix1::Matrix1(int size) : size(size) { + if (this->size == 0) + data = nullptr; + else { + this->data = new float[size](); + this->externalData = false; + } +} + +Matrix1::Matrix1(float* data, int size) : data(data), size(size) { + this->externalData = true; +} + +Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) { + Matrix1 r = Matrix1(4); + float* data = r.data; + data[0] = q.x; + data[1] = q.y; + data[2] = q.z; + data[3] = q.w; + return r; +} + +Quaternion LinearAlgebra::Matrix1::ToQuaternion() { + return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]); +} + +// Matrix1 +#pragma endregion + +#pragma region Matrix2 + +Matrix2::Matrix2() {} + +Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { + this->nValues = nRows * nCols; + if (this->nValues == 0) + this->data = nullptr; + else { + this->data = new float[this->nValues]; + this->externalData = false; + } +} + +Matrix2::Matrix2(float* data, int nRows, int nCols) + : nRows(nRows), nCols(nCols), data(data) { + this->nValues = nRows * nCols; + this->externalData = true; +} + +Matrix2::Matrix2(const Matrix2& m) + : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) { + if (this->nValues == 0) + this->data = nullptr; + else { + this->data = new float[this->nValues]; + + for (int ix = 0; ix < this->nValues; ++ix) + this->data[ix] = m.data[ix]; + } +} + +Matrix2& Matrix2::operator=(const Matrix2& m) { + if (this != &m) { + delete[] this->data; // Free the current memory + + this->nRows = m.nRows; + this->nCols = m.nCols; + this->nValues = m.nValues; + if (this->nValues == 0) + this->data = nullptr; + else { + this->data = new float[this->nValues]; + for (int ix = 0; ix < this->nValues; ++ix) + this->data[ix] = m.data[ix]; + } + } + return *this; +} + +Matrix2::~Matrix2() { + if (!this->externalData) + delete[] data; +} + +Matrix2 Matrix2::Clone() const { + Matrix2 r = Matrix2(this->nRows, this->nCols); + for (int ix = 0; ix < this->nValues; ++ix) + r.data[ix] = this->data[ix]; + return r; +} + +// Move constructor +Matrix2::Matrix2(Matrix2&& other) noexcept + : nRows(other.nRows), + 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::Zero(int nRows, int nCols) { + Matrix2 r = Matrix2(nRows, nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = 0; + return r; +} + +void Matrix2::Clear() { + for (int ix = 0; ix < this->nValues; ix++) + this->data[ix] = 0; +} + +Matrix2 Matrix2::Identity(int size) { + return Diagonal(1, size); +} + +Matrix2 Matrix2::Diagonal(float f, int size) { + Matrix2 r = Matrix2::Zero(size, size); + float* data = r.data; + int valueIx = 0; + for (int ix = 0; ix < size; ix++) { + data[valueIx] = f; + valueIx += size + 1; + } + return r; +} + +Matrix2 Matrix2::SkewMatrix(const Vector3& v) { + Matrix2 r = Matrix2(3, 3); + float* data = r.data; + data[0 * 3 + 1] = -v.z; // result(0, 1) + data[0 * 3 + 2] = v.y; // result(0, 2) + data[1 * 3 + 0] = v.z; // result(1, 0) + data[1 * 3 + 2] = -v.x; // result(1, 2) + data[2 * 3 + 0] = -v.y; // result(2, 0) + data[2 * 3 + 1] = v.x; // result(2, 1) + return r; +} + +Matrix2 Matrix2::Transpose() const { + Matrix2 r = Matrix2(this->nCols, this->nRows); + + for (int rowIx = 0; rowIx < this->nRows; rowIx++) { + for (int colIx = 0; colIx < this->nCols; colIx++) + r.data[colIx * this->nCols + rowIx] = + this->data[rowIx * this->nCols + colIx]; + } + return r; +} + +Matrix2 LinearAlgebra::Matrix2::operator-() const { + Matrix2 r = Matrix2(this->nRows, this->nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = -this->data[ix]; + return r; +} + +Matrix2 LinearAlgebra::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 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 r = Matrix2(this->nRows, B.nCols); + + int ACols = this->nCols; + int BCols = B.nCols; + int ARows = this->nRows; + // int BRows = B.nRows; + + for (int i = 0; i < ARows; ++i) { + // Pre-compute row offsets + int ARowOffset = i * ACols; // ARowOffset is constant for each row of A + int BColOffset = i * BCols; // BColOffset is constant for each row of B + for (int j = 0; j < BCols; ++j) { + float sum = 0; + std::cout << " 0"; + int BIndex = j; + for (int k = 0; k < ACols; ++k) { + std::cout << " + " << this->data[ARowOffset + k] << " * " + << B.data[BIndex]; + sum += this->data[ARowOffset + k] * B.data[BIndex]; + BIndex += BCols; + } + r.data[BColOffset + j] = sum; + std::cout << " = " << sum << " ix: " << BColOffset + j << "\n"; + } + } + return r; +} + +Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) { + Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart); + + int resultRowIx = 0; + int resultColIx = 0; + for (int i = rowStart; i < rowStop; i++) { + for (int j = colStart; j < colStop; j++) + r.data[resultRowIx * r.nCols + resultColIx] = + this->data[i * this->nCols + j]; + } + return r; +} + +void Matrix2::UpdateSlice(int rowStart, + int rowStop, + int colStart, + int colStop, + 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; + mRowDataIx += m.nCols; + for (int colIx = colStart; colIx < colStop; colIx++) { + this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)]; + } + } +} + +/// @brief Compute the Omega matrix of a 3D vector +/// @param v The vector +/// @return 4x4 Omega matrix +Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) { + Matrix2 r = Matrix2::Zero(4, 4); + r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v)); + + // set last row to -v + int ix = 3 * 4; + r.data[ix++] = -v.x; + r.data[ix++] = -v.y; + r.data[ix] = -v.z; + + // Set last column to v + ix = 3; + r.data[ix += 4] = v.x; + r.data[ix += 4] = v.y; + r.data[ix] = v.z; + return r; +} + +// Matrix2 +#pragma endregion + +} // namespace LinearAlgebra + +template <> +MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { if (rows <= 0 || cols <= 0) { this->rows = 0; this->cols = 0; @@ -14,20 +298,22 @@ template <> MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { this->data = new float[matrixSize]{0.0f}; } -template <> MatrixOf::MatrixOf(Vector3 v) : MatrixOf(3, 1) { - Set(0, 0, v.x); - Set(1, 0, v.y); - Set(2, 0, v.z); +template <> +MatrixOf::MatrixOf(Vector3 v) : MatrixOf(3, 1) { + Set(0, 0, v.Right()); + Set(1, 0, v.Up()); + Set(2, 0, v.Forward()); } template <> -void MatrixOf::Multiply(const MatrixOf *m1, - const MatrixOf *m2, MatrixOf *r) { +void MatrixOf::Multiply(const MatrixOf* m1, + const MatrixOf* m2, + MatrixOf* r) { for (unsigned int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) { for (unsigned int colIx2 = 0; colIx2 < m2->cols; colIx2++) { unsigned int rDataIx = colIx2 * m2->cols + rowIx1; r->data[rDataIx] = 0.0F; - for (int kIx = 0; kIx < m2->rows; kIx++) { + for (unsigned int kIx = 0; kIx < m2->rows; kIx++) { unsigned int dataIx1 = rowIx1 * m1->cols + kIx; unsigned int dataIx2 = kIx * m2->cols + colIx2; r->data[rDataIx] += m1->data[dataIx1] * m2->data[dataIx2]; @@ -37,7 +323,7 @@ void MatrixOf::Multiply(const MatrixOf *m1, } template <> -Vector3 MatrixOf::Multiply(const MatrixOf *m, Vector3 v) { +Vector3 MatrixOf::Multiply(const MatrixOf* m, Vector3 v) { MatrixOf v_m = MatrixOf(v); MatrixOf r_m = MatrixOf(3, 1); @@ -47,10 +333,11 @@ Vector3 MatrixOf::Multiply(const MatrixOf *m, Vector3 v) { return r; } -template Vector3 MatrixOf::operator*(const Vector3 v) const { - float *vData = new float[3]{v.x, v.y, v.z}; +template +Vector3 MatrixOf::operator*(const Vector3 v) const { + float* vData = new float[3]{v.Right(), v.Up(), v.Forward()}; MatrixOf v_m = MatrixOf(3, 1, vData); - float *rData = new float[3]{}; + float* rData = new float[3]{}; MatrixOf r_m = MatrixOf(3, 1, rData); Multiply(this, &v_m, &r_m); diff --git a/Matrix.h b/Matrix.h index 5ef0588..ef72922 100644 --- a/Matrix.h +++ b/Matrix.h @@ -1,17 +1,129 @@ #ifndef MATRIX_H #define MATRIX_H +#include "Quaternion.h" #include "Vector3.h" +namespace LinearAlgebra { + +/// @brief A 1-dimensional matrix or vector of arbitrary size +class Matrix1 { + public: + float* data = nullptr; + int size = 0; + + Matrix1(int size); + Matrix1(float* data, int size); + + static Matrix1 FromQuaternion(Quaternion q); + Quaternion ToQuaternion(); + + private: + bool externalData = true; +}; + +/// @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(const Matrix2& m); + Matrix2& operator=(const Matrix2& other); + + ~Matrix2(); + + Matrix2 Clone() const; + + static Matrix2 Zero(int nRows, int nCols); + void Clear(); + + static Matrix2 Identity(int size); + + static Matrix2 Diagonal(float f, int size); + + static Matrix2 SkewMatrix(const Vector3& v); + + Matrix2 Transpose() const; + + Matrix2 operator-() const; + + /// @brief Add a matrix to this matrix + /// @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& m) const; + friend Matrix2 operator*(const Matrix2& m, float f) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = m.data[ix] * f; + return r; + } + friend Matrix2 operator*(float f, const Matrix2& m) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = f * m.data[ix]; + return r; + } + + 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); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = m.data[ix] / f; + return r; + } + friend Matrix2 operator/(float f, const Matrix2& m) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = f / m.data[ix]; + return r; + } + + Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop); + + void UpdateSlice(int rowStart, + int rowStop, + int colStart, + int colStop, + const Matrix2& m) const; + // private: + // move constructor and move assignment operator + Matrix2(Matrix2&& other) noexcept; + Matrix2& operator=(Matrix2&& other) noexcept; + + static Matrix2 Omega(const Vector3& v); + + private: + bool externalData = true; +}; + /// @brief Single precision float matrix -template class MatrixOf { -public: +template +class MatrixOf { + public: MatrixOf(unsigned int rows, unsigned int cols); - MatrixOf(unsigned int rows, unsigned int cols, const T *source) + MatrixOf(unsigned int rows, unsigned int cols, const T* source) : MatrixOf(rows, cols) { Set(source); } - MatrixOf(Vector3 v); // creates a 3,1 matrix + MatrixOf(Vector3 v); // creates a 3,1 matrix ~MatrixOf() { if (this->data == nullptr) @@ -22,7 +134,7 @@ public: /// @brief Transpose with result in matrix m /// @param r The matrix in which the transposed matrix is stored - void Transpose(MatrixOf *r) const { + void Transpose(MatrixOf* r) const { // Check dimensions first // We dont care about the rows and cols (we overwrite them) // but the data size should be equal to avoid problems @@ -33,7 +145,7 @@ public: // Return a null matrix; // We dont set data to nullptr because it is allocated memory // Instead we write all zeros - for (int dataIx = 0; dataIx < resultSize; dataIx++) + for (unsigned int dataIx = 0; dataIx < resultSize; dataIx++) r->data[dataIx] = 0.0f; r->rows = 0; r->cols = 0; @@ -43,7 +155,7 @@ public: r->cols = this->rows; r->rows = this->cols; - for (int rDataIx = 0; rDataIx < matrixSize; rDataIx++) { + for (unsigned int rDataIx = 0; rDataIx < matrixSize; rDataIx++) { unsigned int rowIx = rDataIx / this->rows; unsigned int colIx = rDataIx % this->rows; unsigned int mDataIx = this->cols * colIx + rowIx; @@ -51,13 +163,14 @@ public: } } - static void Multiply(const MatrixOf *m1, const MatrixOf *m2, - MatrixOf *r); - void Multiply(const MatrixOf *m, MatrixOf *r) const { + static void Multiply(const MatrixOf* m1, + const MatrixOf* m2, + MatrixOf* r); + void Multiply(const MatrixOf* m, MatrixOf* r) const { Multiply(this, m, r); } - static Vector3 Multiply(const MatrixOf *m, Vector3 v); + static Vector3 Multiply(const MatrixOf* m, Vector3 v); Vector3 operator*(const Vector3 v) const; T Get(unsigned int rowIx, unsigned int colIx) const { @@ -71,28 +184,28 @@ public: } // This function does not check on source size! - void Set(const T *source) { + void Set(const T* source) { unsigned int matrixSize = this->cols * this->rows; for (unsigned int dataIx = 0; dataIx < matrixSize; dataIx++) this->data[dataIx] = source[dataIx]; } // This function does not check on source size! - void SetRow(unsigned int rowIx, const T *source) { + void SetRow(unsigned int rowIx, const T* source) { unsigned int dataIx = rowIx * this->cols; for (unsigned int sourceIx = 0; sourceIx < this->cols; dataIx++, sourceIx++) this->data[dataIx] = source[sourceIx]; } // This function does not check on source size! - void SetCol(unsigned int colIx, const T *source) { + void SetCol(unsigned int colIx, const T* source) { unsigned int dataIx = colIx; for (unsigned int sourceIx = 0; sourceIx < this->cols; dataIx += this->cols, sourceIx++) this->data[dataIx] = source[sourceIx]; } - void CopyFrom(const MatrixOf *m) { + void CopyFrom(const MatrixOf* m) { unsigned int thisMatrixSize = this->cols * this->rows; unsigned int mMatrixSize = m->cols * m->rows; if (mMatrixSize != thisMatrixSize) @@ -105,10 +218,13 @@ public: unsigned int RowCount() const { return rows; } unsigned int ColCount() const { return cols; } -private: + private: unsigned int rows; unsigned int cols; - T *data; + T* data; }; +} // namespace LinearAlgebra +// using namespace LinearAlgebra; + #endif \ No newline at end of file diff --git a/Polar.cpp b/Polar.cpp index f72a692..15ac40f 100644 --- a/Polar.cpp +++ b/Polar.cpp @@ -1,84 +1,179 @@ #include -#include "Angle.h" #include "Polar.h" #include "Vector2.h" -Polar::Polar() { - angle = 0.0F; - distance = 0.0F; +template +PolarOf::PolarOf() { + this->distance = 0.0f; + this->angle = AngleOf(); } - -Polar::Polar(float newAngle, float newDistance) { +template +PolarOf::PolarOf(float distance, AngleOf angle) { // distance should always be 0 or greater - if (newDistance < 0) { - angle = Angle::Normalize(newAngle - 180); - distance = -newDistance; + if (distance < 0.0f) { + this->distance = -distance; + this->angle = AngleOf::Normalize(angle - AngleOf::Degrees(180)); } else { - angle = Angle::Normalize(newAngle); - distance = newDistance; + this->distance = distance; + if (this->distance == 0.0f) + // angle is always 0 if distance is 0 + this->angle = AngleOf(); + else + this->angle = AngleOf::Normalize(angle); } } -Polar::Polar(Vector2 v) { - float signY = (v.y >= 0) - (v.y < 0); - angle = atan2(v.y, signY * sqrt(v.y * v.y + v.x * v.x)) * Angle::Rad2Deg; - distance = v.magnitude(); +template +PolarOf PolarOf::Degrees(float distance, float degrees) { + AngleOf angle = AngleOf::Degrees(degrees); + PolarOf r = PolarOf(distance, angle); + return r; } -const Polar Polar::zero = Polar(0, 0); +template +PolarOf PolarOf::Radians(float distance, float radians) { + return PolarOf(distance, AngleOf::Radians(radians)); +} -float Polar::Distance(const Polar &v1, const Polar &v2) { +template +PolarOf PolarOf::FromVector2(Vector2 v) { + float distance = v.magnitude(); + AngleOf angle = + AngleOf::Degrees(Vector2::SignedAngle(Vector2::forward, v)); + PolarOf p = PolarOf(distance, angle); + return p; +} +template +PolarOf PolarOf::FromSpherical(SphericalOf v) { + float distance = + v.distance * cosf(v.direction.vertical.InDegrees() * Deg2Rad); + AngleOf angle = v.direction.horizontal; + PolarOf p = PolarOf(distance, angle); + return p; +} + +template +const PolarOf PolarOf::zero = PolarOf(0.0f, AngleOf()); +template +const PolarOf PolarOf::forward = PolarOf(1.0f, AngleOf()); +template +const PolarOf PolarOf::back = PolarOf(1.0, AngleOf::Degrees(180)); +template +const PolarOf PolarOf::right = PolarOf(1.0, AngleOf::Degrees(90)); +template +const PolarOf PolarOf::left = PolarOf(1.0, AngleOf::Degrees(-90)); + +template +bool PolarOf::operator==(const PolarOf& v) const { + return (this->distance == v.distance && + this->angle.InDegrees() == v.angle.InDegrees()); +} + +template +PolarOf PolarOf::Normalize(const PolarOf& v) { + PolarOf r = PolarOf(1, v.angle); + return r; +} +template +PolarOf PolarOf::normalized() const { + PolarOf r = PolarOf(1, this->angle); + return r; +} + +template +PolarOf PolarOf::operator-() const { + PolarOf v = + PolarOf(this->distance, this->angle + AngleOf::Degrees(180)); + return v; +} + +template +PolarOf PolarOf::operator-(const PolarOf& v) const { + PolarOf r = -v; + return *this + r; +} +template +PolarOf PolarOf::operator-=(const PolarOf& v) { + *this = *this - v; + return *this; + // angle = AngleOf::Normalize(newAngle); + // distance = newDistance; +} + +// Polar::Polar(Vector2 v) { +// float signY = (v.y >= 0) - (v.y < 0); +// angle = atan2(v.y, signY * sqrt(v.y * v.y + v.x * v.x)) * Angle::Rad2Deg; +// distance = v.magnitude(); +// } + +// const Polar Polar::zero = Polar(0, 0); + +// float Polar::Distance(const Polar &v1, const Polar &v2) { +// float d = +// Angle::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle); +// return d; +// } + +template +PolarOf PolarOf::operator+(const PolarOf& v) const { + if (v.distance == 0) + return PolarOf(this->distance, this->angle); + if (this->distance == 0.0f) + return v; + + float deltaAngle = AngleOf::Normalize(v.angle - this->angle).InDegrees(); + float rotation = + deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle; + + if (rotation == 180.0f && v.distance > 0.0f) { + // angle is too small, take this angle and add the distances + return PolarOf(this->distance + v.distance, this->angle); + } + + float newDistance = AngleOf::CosineRuleSide(v.distance, this->distance, + AngleOf::Degrees(rotation)); + + float angle = + AngleSingle::CosineRuleAngle(newDistance, this->distance, v.distance) + .InDegrees(); + + float newAngle = deltaAngle < 0.0f ? this->angle.InDegrees() - angle + : this->angle.InDegrees() + angle; + AngleOf newAngleA = AngleOf::Normalize(AngleOf::Degrees(newAngle)); + PolarOf vector = PolarOf(newDistance, newAngleA); + return vector; +} +template +PolarOf PolarOf::operator+=(const PolarOf& v) { + *this = *this + v; + return *this; +} + +template +PolarOf PolarOf::operator*=(float f) { + this->distance *= f; + return *this; +} +template +PolarOf PolarOf::operator/=(float f) { + this->distance /= f; + return *this; +} + +template +float PolarOf::Distance(const PolarOf& v1, const PolarOf& v2) { float d = - Angle::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle); + AngleOf::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle); return d; } -Polar Polar::operator+(const Polar &v2) const { - if (v2.distance == 0) - return Polar(this->angle, this->distance); - if (this->distance == 0) - return v2; - - float deltaAngle = Angle::Normalize(v2.angle - this->angle); - float rotation = deltaAngle < 0 ? 180 + deltaAngle : 180 - deltaAngle; - - if (rotation == 180 && v2.distance > 0) { - // angle is too small, take this angle and add the distances - return Polar(this->angle, this->distance + v2.distance); - } - - float newDistance = - Angle::CosineRuleSide(v2.distance, this->distance, rotation); - - float angle = - Angle::CosineRuleAngle(newDistance, this->distance, v2.distance); - - float newAngle = deltaAngle < 0 ? Angle::Normalize(this->angle - angle) - : Angle::Normalize(this->angle + angle); - Polar vector = Polar(newAngle, newDistance); - return vector; +template +PolarOf PolarOf::Rotate(const PolarOf& v, AngleOf angle) { + AngleOf a = AngleOf::Normalize(v.angle + angle); + PolarOf r = PolarOf(v.distance, a); + return r; } -Polar Polar::operator-() { - Polar vector = Polar(this->angle - 180, this->distance); - return vector; -} - -Polar Polar::operator-(const Polar &v2) const { - Polar vector = *this + (Polar(v2.angle - 180, v2.distance)); - return vector; -} - -Polar Polar::operator*(float f) const { - return Polar(this->angle, this->distance * f); -} - -Polar Polar::operator/(const float &f) { - return Polar(this->angle, this->distance / f); -} - -Polar Polar::Rotate(Polar v, float angle) { - v.angle = Angle::Normalize(v.angle + angle); - return v; -} \ No newline at end of file +template class LinearAlgebra::PolarOf; +template class LinearAlgebra::PolarOf; \ No newline at end of file diff --git a/Polar.h b/Polar.h index 6060364..541bec8 100644 --- a/Polar.h +++ b/Polar.h @@ -5,98 +5,158 @@ #ifndef POLAR_H #define POLAR_H -class Vector2; +#include "Angle.h" -/// -/// A polar vector -/// -/// This will use the polar coordinate system consisting of a angle from a -/// reference direction and a distance. -struct Polar { -public: - /// - /// The angle in degrees, clockwise rotation - /// - /// The angle is normalized to -180 .. 180 - float angle; - /// - /// The distance in meters - /// - /// The distance should never be negative +namespace LinearAlgebra { + +struct Vector2; +template +class SphericalOf; + +/// @brief A polar vector using an angle in various representations +/// @tparam T The implementation type used for the representation of the angle +template +class PolarOf { + public: + /// @brief The distance in meters + /// @remark The distance shall never be negative float distance; + /// @brief The angle in degrees clockwise rotation + /// @remark The angle shall be between -180 .. 180 + AngleOf angle; - /// - /// Create a new polar vector with zero degrees and distance - /// - Polar(); - /// - /// Create a new polar vector - /// - /// The angle in degrees, clockwise rotation - /// The distance in meters - Polar(float angle, float distance); + /// @brief A new vector with polar coordinates with zero degrees and + /// distance + PolarOf(); + /// @brief A new vector with polar coordinates + /// @param distance The distance in meters + /// @param angle The angle in degrees, clockwise rotation + /// @note The distance is automatically converted to a positive value. + /// @note The angle is automatically normalized to -180 .. 180 + PolarOf(float distance, AngleOf angle); - Polar(Vector2 v); + /// @brief Create polar vector without using AngleOf type. All given angles + /// are in degrees + /// @param distance The distance in meters + /// @param degrees The angle in degrees + /// @return The polar vector + static PolarOf Degrees(float distance, float degrees); + /// @brief Short-hand Deg alias for the Degrees function + constexpr static auto Deg = Degrees; + /// @brief Create polar vector without using AngleOf type. All given angles + /// are in radians. + /// @param distance The distance in meters + /// @param radians The angle in radians + /// @return The polar vector + static PolarOf Radians(float distance, float radians); + /// @brief Short-hand Rad alias for the Radians function + constexpr static auto Rad = Radians; - /// - /// A polar vector with zero degrees and distance - /// - const static Polar zero; + /// @brief Convert a vector from 2D carthesian coordinates to polar + /// coordinates + /// @param v The vector to convert + static PolarOf FromVector2(Vector2 v); + /// @brief Convert a vector from spherical coordinates to polar coordinates + /// @param s The vector to convert + /// @note The resulting vector will be projected on the horizontal plane + static PolarOf FromSpherical(SphericalOf v); - /// - /// Negate the polar vector. - /// + /// @brief A polar vector with zero degrees and distance + const static PolarOf zero; + /// @brief A normalized forward-oriented vector + const static PolarOf forward; + /// @brief A normalized back-oriented vector + const static PolarOf back; + /// @brief A normalized right-oriented vector + const static PolarOf right; + /// @brief A normalized left-oriented vector + const static PolarOf left; + + /// @brief Equality test to another vector + /// @param v The vector to check against + /// @return true: if it is identical to the given vector + /// @note This uses float comparison to check equality which may have + /// strange effects. Equality on floats should be avoided. + bool operator==(const PolarOf& v) const; + + /// @brief The vector length + /// @param v The vector for which you need the length + /// @return The vector length; + inline static float Magnitude(const PolarOf& v) { return v.distance; } + /// @brief The vector length + /// @return The vector length + inline float magnitude() const { return this->distance; } + + /// @brief Convert the vector to a length of 1 + /// @param v The vector to convert + /// @return The vector normalized to a length of 1 + static PolarOf Normalize(const PolarOf& v); + /// @brief Convert the vector to a length of a + /// @return The vector normalized to a length of 1 + PolarOf normalized() const; + + /// @brief Negate the vector + /// @return The negated vector /// This will rotate the vector by 180 degrees. Distance will stay the same. - /// The negated vector + PolarOf operator-() const; - Polar operator-(); - /// - /// Substract a polar vector from this coordinate - /// - /// The vector to subtract from this vector - /// The result of the subtraction - Polar operator-(const Polar &v) const; + /// @brief Subtract a polar vector from this vector + /// @param v The vector to subtract + /// @return The result of the subtraction + PolarOf operator-(const PolarOf& v) const; + PolarOf operator-=(const PolarOf& v); + /// @brief Add a polar vector to this vector + /// @param v The vector to add + /// @return The result of the addition + PolarOf operator+(const PolarOf& v) const; + PolarOf operator+=(const PolarOf& v); - /// - /// Add another polar vector to this polar vector - /// - /// The vector to add - /// The result of adding the vector - Polar operator+(const Polar &v) const; + /// @brief Scale the vector uniformly up + /// @param f The scaling factor + /// @return The scaled vector + /// @remark This operation will scale the distance of the vector. The angle + /// will be unaffected. + friend PolarOf operator*(const PolarOf& v, float f) { + return PolarOf(v.distance * f, v.angle); + } + friend PolarOf operator*(float f, const PolarOf& v) { + return PolarOf(f * v.distance, v.angle); + } + PolarOf operator*=(float f); + /// @brief Scale the vector uniformly down + /// @param f The scaling factor + /// @return The scaled factor + /// @remark This operation will scale the distance of the vector. The angle + /// will be unaffected. + friend PolarOf operator/(const PolarOf& v, float f) { + return PolarOf(v.distance / f, v.angle); + } + friend PolarOf operator/(float f, const PolarOf& v) { + return PolarOf(f / v.distance, v.angle); + } + PolarOf operator/=(float f); - /// - /// Scale the vector uniformly up - /// - /// The scaling factor - /// The scaled vector - /// This operation will scale the distance of the vector. The angle will be - /// unaffected. - Polar operator*(float factor) const; + /// @brief The distance between two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The distance between the two vectors + static float Distance(const PolarOf& v1, const PolarOf& v2); - /// - /// Scale the vector uniformly down - /// - /// The scaling factor - /// The scaled vector - /// This operation will scale the distance of the vector. The angle will be - /// unaffected. - Polar operator/(const float &factor); - - /// - /// The distance between two vectors - /// - /// The first vector - /// The second vector - /// The distance between the two vectors - static float Distance(const Polar &v1, const Polar &v2); - - /// - /// Rotate the vector - /// - /// The vector to rotate - /// Angle in radias to rotate - /// The rotated vector - static Polar Rotate(Polar v, float angle); + /// @brief Rotate a vector + /// @param v The vector to rotate + /// @param a The angle in degreesto rotate + /// @return The rotated vector + static PolarOf Rotate(const PolarOf& v, AngleOf a); }; +using PolarSingle = PolarOf; +using Polar16 = PolarOf; +// using Polar = PolarSingle; + +} // namespace LinearAlgebra +using namespace LinearAlgebra; + +#include "Spherical.h" +#include "Vector2.h" + #endif \ No newline at end of file diff --git a/Quaternion.cpp b/Quaternion.cpp index e621bb0..dda8c18 100644 --- a/Quaternion.cpp +++ b/Quaternion.cpp @@ -5,6 +5,8 @@ #include "Quaternion.h" #include #include +#include "Angle.h" +#include "Matrix.h" #include "Vector3.h" void CopyQuat(const Quat& q1, Quat& q2) { @@ -96,6 +98,28 @@ 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; + 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); + data[1 * 3 + 0] = 2 * (x * y + w * z); + data[1 * 3 + 1] = 1 - 2 * (x * x + z * z); + data[1 * 3 + 2] = 2 * (y * z - w * x); + data[2 * 3 + 0] = 2 * (x * z - w * y); + data[2 * 3 + 1] = 2 * (y * z + w * x); + data[2 * 3 + 2] = 1 - 2 * (x * x + y * y); + + return r; +} + Quaternion Quaternion::operator*(const Quaternion& r2) const { return Quaternion( this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x, @@ -117,17 +141,25 @@ Vector3 Quaternion::operator*(const Vector3& p) const { float num10 = this->w * num; float num11 = this->w * num2; float num12 = this->w * num3; - Vector3 result = Vector3::zero; - result.x = - (1 - (num5 + num6)) * p.x + (num7 - num12) * p.y + (num8 + num11) * p.z; - result.y = - (num7 + num12) * p.x + (1 - (num4 + num6)) * p.y + (num9 - num10) * p.z; - result.z = - (num8 - num11) * p.x + (num9 + num10) * p.y + (1 - (num4 + num5)) * p.z; + + float px = p.Right(); + float py = p.Up(); + float pz = p.Forward(); + // Vector3 result = Vector3::zero; + // result.x = + float rx = + (1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz; + // result.y = + float ry = + (num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz; + // result.z = + float rz = + (num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz; + Vector3 result = Vector3(rx, ry, rz); return result; } -bool Quaternion::operator==(const Quaternion& q) { +bool Quaternion::operator==(const Quaternion& q) const { return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w); } @@ -144,15 +176,15 @@ Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) { Vector3 nForward = Vector3::Normalize(forward); Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward)); Vector3 nUp = Vector3::Cross(nForward, nRight); - float m00 = nRight.x; - float m01 = nRight.y; - float m02 = nRight.z; - float m10 = nUp.x; - float m11 = nUp.y; - float m12 = nUp.z; - float m20 = nForward.x; - float m21 = nForward.y; - float m22 = nForward.z; + float m00 = nRight.Right(); // x; + float m01 = nRight.Up(); // y; + float m02 = nRight.Forward(); // z; + float m10 = nUp.Right(); // x; + float m11 = nUp.Up(); // y; + float m12 = nUp.Forward(); // z; + float m20 = nForward.Right(); // x; + float m21 = nForward.Up(); // y; + float m22 = nForward.Forward(); // z; float num8 = (m00 + m11) + m22; Quaternion quaternion = Quaternion(); @@ -196,8 +228,8 @@ Quaternion Quaternion::FromToRotation(Vector3 fromDirection, Vector3 toDirection) { Vector3 axis = Vector3::Cross(fromDirection, toDirection); axis = Vector3::Normalize(axis); - float angle = Vector3::SignedAngle(fromDirection, toDirection, axis); - Quaternion rotation = Quaternion::AngleAxis(angle, axis); + AngleOf angle = Vector3::SignedAngle(fromDirection, toDirection, axis); + Quaternion rotation = Quaternion::AngleAxis(angle.InDegrees(), axis); return rotation; } @@ -221,9 +253,9 @@ Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) { radians *= 0.5f; Vector3 axis2 = axis * (float)sin(radians); - result.x = axis2.x; - result.y = axis2.y; - result.z = axis2.z; + result.x = axis2.Right(); // x; + result.y = axis2.Up(); // y; + result.z = axis2.Forward(); // z; result.w = (float)cos(radians); return Quaternion::Normalize(result); @@ -246,7 +278,7 @@ void Quaternion::ToAxisAngleRad(const Quaternion& q, *angle = 2.0f * acosf(q1.w); // angle float den = sqrtf(1.0F - q1.w * q1.w); if (den > 0.0001f) { - *axis = q1.xyz() / den; + *axis = Vector3::Normalize(q1.xyz() / den); } else { // This occurs when the angle is zero. // Not a problem: just set an arbitrary normalized axis. @@ -297,7 +329,8 @@ Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, blendB = t; } Vector3 v = axyz * blendA + b2.xyz() * blendB; - Quaternion result = Quaternion(v.x, v.y, v.z, blendA * a.w + blendB * b2.w); + Quaternion result = + Quaternion(v.Right(), v.Up(), v.Forward(), blendA * a.w + blendB * b2.w); if (result.GetLengthSquared() > 0.0f) return Quaternion::Normalize(result); else @@ -322,9 +355,9 @@ Quaternion Quaternion::Euler(Vector3 euler) { } Quaternion Quaternion::FromEulerRad(Vector3 euler) { - float yaw = euler.x; - float pitch = euler.y; - float roll = euler.z; + float yaw = euler.Right(); + float pitch = euler.Up(); + float roll = euler.Forward(); float rollOver2 = roll * 0.5f; float sinRollOver2 = (float)sin((float)rollOver2); float cosRollOver2 = (float)cos((float)rollOver2); @@ -353,9 +386,9 @@ Quaternion Quaternion::EulerXYZ(Vector3 euler) { return Quaternion::FromEulerRadXYZ(euler * Deg2Rad); } Quaternion Quaternion::FromEulerRadXYZ(Vector3 euler) { - float yaw = euler.x; - float pitch = euler.y; - float roll = euler.z; + float yaw = euler.Right(); // x; + float pitch = euler.Up(); // y; + float roll = euler.Forward(); // z; float rollOver2 = roll * 0.5f; float sinRollOver2 = (float)sin((float)rollOver2); float cosRollOver2 = (float)cos((float)rollOver2); @@ -394,7 +427,7 @@ Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) { Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z); // rotation axis Vector3 p = Vector3::Project( ra, axis); // return projection ra on to axis (parallel component) - Quaternion twist = Quaternion(p.x, p.y, p.z, rotation.w); + Quaternion twist = Quaternion(p.Right(), p.Up(), p.Forward(), rotation.w); twist = Quaternion::Normalize(twist); return twist; } diff --git a/Quaternion.h b/Quaternion.h index eb40a5d..1687dc9 100644 --- a/Quaternion.h +++ b/Quaternion.h @@ -9,7 +9,7 @@ extern "C" { /// -/// A quaternion +/// A quaternion (C-style) /// /// This is a C-style implementation typedef struct Quat { @@ -32,6 +32,10 @@ typedef struct Quat { } Quat; } +namespace LinearAlgebra { + +class Matrix2; + /// /// A quaternion /// @@ -54,6 +58,9 @@ struct Quaternion : Quat { /// /// Quaternion(Quat q); + /// + /// Quaternion destructor + /// ~Quaternion(); /// @@ -84,6 +91,8 @@ struct Quaternion : Quat { /// The euler angles performed in the order: Z, X, Y static Vector3 ToAngles(const Quaternion& q); + Matrix2 ToRotationMatrix(); + /// /// Rotate a vector using this quaterion /// @@ -108,7 +117,7 @@ struct Quaternion : Quat { /// themselves. Two quaternions with the same rotational effect may have /// different components. Use Quaternion::Angle to check if the rotations are /// the same. - bool operator==(const Quaternion& quaternion); + bool operator==(const Quaternion& quaternion) const; /// /// The inverse of quaterion @@ -282,4 +291,7 @@ struct Quaternion : Quat { Vector3 xyz() const; }; -#endif \ No newline at end of file +} // namespace LinearAlgebra +using namespace LinearAlgebra; + +#endif diff --git a/README.md b/README.md index 42397d5..afc4fbb 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,21 @@ -\mainpage Vector Algebra +\mainpage Linear Algebra -Vector algebra library +Linear algebra library Main components --------------- -* [Vector3](https://passervr.com/apis/VectorAlgebra/struct_vector3.html) -* [Quaternion](https://passervr.com/apis/VectorAlgebra/struct_quaternion.html) +Carthesian coordinate systems +* [Vector3](#Passer::LinearAlgebra::Vector3): A 3D carthesian vector +* [Vector2](#Passer::LinearAlgebra::Vector2): A 2D carthesian vector -* [Vector2](https://passervr.com/apis/VectorAlgebra/struct_vector2.html) +Other coodinate systems +* [Polar](#Passer::LinearAlgebra::PolarOf): A 2D polar vector +* [Spherical](#Passer::LinearAlgebra::SphericalOf): A 3D spherical vector -* [Polar](https://passervr.com/apis/VectorAlgebra/struct_polar.html) +Rotations +* [Quaternion](#Passer::LinearAlgebra::Quaternion): A quaternion rotation +* [SwingTwist](#Passer::LinearAlgebra::SwingTwistOf): A swing/twist angular rotation + +Basics +* [Angle](#Passer::LinearAlgebra::AngleOf): An angle +* [Direction](#Passer::LinearAlgebra::DirectionOf): A direction using angles diff --git a/Spherical.cpp b/Spherical.cpp index 026d33b..0371f92 100644 --- a/Spherical.cpp +++ b/Spherical.cpp @@ -1,50 +1,307 @@ #include "Spherical.h" #include "Angle.h" +#include "Quaternion.h" #include -// using Angle = float; +namespace LinearAlgebra { -Spherical::Spherical() { - this->horizontalAngle = 0; - this->verticalAngle = 0; - this->distance = 0; +template +SphericalOf::SphericalOf() { + this->distance = 0.0f; + this->direction = DirectionOf(); } -// Spherical::Spherical(float polarAngle, float elevationAngle, float distance) -// { -// this->horizontalAngle = polarAngle; -// this->verticalAngle = elevationAngle; -// this->distance = distance; -// } - -Spherical::Spherical(Polar polar) { - this->horizontalAngle = polar.angle; - this->verticalAngle = 0.0F; - this->distance = polar.distance; +template +SphericalOf::SphericalOf(float distance, + AngleOf horizontal, + AngleOf vertical) { + if (distance < 0) { + this->distance = -distance; + this->direction = -DirectionOf(horizontal, vertical); + } else { + this->distance = distance; + this->direction = DirectionOf(horizontal, vertical); + } } -Spherical::Spherical(float distance, Angle horizontalAngle, Angle verticalAngle) - : distance(distance), horizontalAngle(horizontalAngle), - verticalAngle(verticalAngle) {} - -Spherical::Spherical(Vector3 v) { - float signZ = (v.z >= 0) - (v.z < 0); - horizontalAngle = - atan2(v.y, signZ * sqrt(v.z * v.z + v.x * v.x)) * Angle::Rad2Deg; - verticalAngle = -atan2(v.x, sqrt(v.z * v.z + v.y * v.y)) * Angle::Rad2Deg; - distance = v.magnitude(); +template +SphericalOf::SphericalOf(float distance, DirectionOf direction) { + if (distance < 0) { + this->distance = -distance; + this->direction = -direction; + } else { + this->distance = distance; + this->direction = direction; + } } -const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F); - -float Spherical::GetSwing() { - // Not sure if this is correct - return sqrtf(horizontalAngle * horizontalAngle + - verticalAngle * verticalAngle); +template +SphericalOf SphericalOf::Degrees(float distance, + float horizontal, + float vertical) { + AngleOf horizontalAngle = AngleOf::Degrees(horizontal); + AngleOf verticalAngle = AngleOf::Degrees(vertical); + SphericalOf r = SphericalOf(distance, horizontalAngle, verticalAngle); + return r; } -Polar Spherical::ProjectOnHorizontalPlane() { - return Polar(horizontalAngle, distance); -} \ No newline at end of file +template +SphericalOf SphericalOf::Radians(float distance, + float horizontal, + float vertical) { + return SphericalOf(distance, AngleOf::Radians(horizontal), + AngleOf::Radians(vertical)); +} + +template +SphericalOf SphericalOf::FromPolar(PolarOf polar) { + AngleOf horizontal = polar.angle; + AngleOf vertical = AngleOf(); + SphericalOf r = SphericalOf(polar.distance, horizontal, vertical); + return r; +} + +template +SphericalOf SphericalOf::FromVector3(Vector3 v) { + float distance = v.magnitude(); + if (distance == 0.0f) { + return SphericalOf(distance, AngleOf(), AngleOf()); + } else { + AngleOf verticalAngle = + AngleOf::Radians((pi / 2 - acosf(v.Up() / distance))); + AngleOf horizontalAngle = + AngleOf::Radians(atan2f(v.Right(), v.Forward())); + return SphericalOf(distance, horizontalAngle, verticalAngle); + } +} + +/** + * @brief Converts spherical coordinates to a 3D vector. + * + * This function converts the spherical coordinates represented by the + * SphericalOf object to a 3D vector (Vector3). The conversion is based + * on the distance and direction (vertical and horizontal angles) of the + * spherical coordinates. + * + * @tparam T The type of the distance and direction values. + * @return Vector3 The 3D vector representation of the spherical coordinates. + */ +template +Vector3 SphericalOf::ToVector3() const { + float verticalRad = (pi / 2) - this->direction.vertical.InRadians(); + float horizontalRad = this->direction.horizontal.InRadians(); + + float cosVertical = cosf(verticalRad); + float sinVertical = sinf(verticalRad); + float cosHorizontal = cosf(horizontalRad); + float sinHorizontal = sinf(horizontalRad); + + float x = this->distance * sinVertical * sinHorizontal; + float y = this->distance * cosVertical; + float z = this->distance * sinVertical * cosHorizontal; + + Vector3 v = Vector3(x, y, z); + return v; +} + +template +const SphericalOf SphericalOf::zero = + SphericalOf(0.0f, AngleOf(), AngleOf()); +template +const SphericalOf SphericalOf::forward = + SphericalOf(1.0f, AngleOf(), AngleOf()); +template +const SphericalOf SphericalOf::back = + SphericalOf(1.0f, AngleOf::Degrees(180), AngleOf()); +template +const SphericalOf SphericalOf::right = + SphericalOf(1.0f, AngleOf::Degrees(90), AngleOf()); +template +const SphericalOf SphericalOf::left = + SphericalOf(1.0f, AngleOf::Degrees(-90), AngleOf()); +template +const SphericalOf SphericalOf::up = + SphericalOf(1.0f, AngleOf(), AngleOf::Degrees(90)); +template +const SphericalOf SphericalOf::down = + SphericalOf(1.0f, AngleOf(), AngleOf::Degrees(-90)); + +template +SphericalOf SphericalOf::WithDistance(float distance) { + SphericalOf v = SphericalOf(distance, this->direction); + return v; +} + +template +SphericalOf SphericalOf::operator-() const { + SphericalOf v = SphericalOf( + this->distance, this->direction.horizontal + AngleOf::Degrees(180), + this->direction.vertical + AngleOf::Degrees(180)); + return v; +} + +template +SphericalOf SphericalOf::operator-(const SphericalOf& s2) const { + // let's do it the easy way... + Vector3 v1 = this->ToVector3(); + Vector3 v2 = s2.ToVector3(); + Vector3 v = v1 - v2; + SphericalOf r = SphericalOf::FromVector3(v); + return r; +} +template +SphericalOf SphericalOf::operator-=(const SphericalOf& v) { + *this = *this - v; + return *this; +} + +template +SphericalOf SphericalOf::operator+(const SphericalOf& s2) const { + // let's do it the easy way... + Vector3 v1 = this->ToVector3(); + Vector3 v2 = s2.ToVector3(); + Vector3 v = v1 + v2; + SphericalOf r = SphericalOf::FromVector3(v); + return r; + /* + // This is the hard way... + if (v2.distance <= 0) + return Spherical(this->distance, this->horizontalAngle, + this->verticalAngle); + if (this->distance <= 0) + return v2; + + float deltaHorizontalAngle = + (float)Angle::Normalize(v2.horizontalAngle - this->horizontalAngle); + float horizontalRotation = deltaHorizontalAngle < 0 + ? 180 + deltaHorizontalAngle + : 180 - deltaHorizontalAngle; + float deltaVerticalAngle = + Angle::Normalize(v2.verticalAngle - this->verticalAngle); + float verticalRotation = deltaVerticalAngle < 0 ? 180 + deltaVerticalAngle + : 180 - deltaVerticalAngle; + + if (horizontalRotation == 180 && verticalRotation == 180) + // angle is too small, take this angle and add the distances + return Spherical(this->distance + v2.distance, this->horizontalAngle, + this->verticalAngle); + + Angle rotation = AngleBetween(*this, v2); + float newDistance = + Angle::CosineRuleSide(v2.distance, this->distance, rotation); + float angle = + Angle::CosineRuleAngle(newDistance, this->distance, v2.distance); + + // Now we have to project the angle to the horizontal and vertical planes... + // The axis for the angle is the cross product of the two spherical vectors + // (which function we do not have either...) + float horizontalAngle = 0; + float verticalAngle = 0; + + float newHorizontalAngle = + deltaHorizontalAngle < 0 + ? Angle::Normalize(this->horizontalAngle - horizontalAngle) + : Angle::Normalize(this->horizontalAngle + horizontalAngle); + float newVerticalAngle = + deltaVerticalAngle < 0 + ? Angle::Normalize(this->verticalAngle - verticalAngle) + : Angle::Normalize(this->verticalAngle + verticalAngle); + + Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle); + */ +} +template +SphericalOf SphericalOf::operator+=(const SphericalOf& v) { + *this = *this + v; + return *this; +} + +template +SphericalOf SphericalOf::operator*=(float f) { + this->distance *= f; + return *this; +} + +template +SphericalOf SphericalOf::operator/=(float f) { + this->distance /= f; + return *this; +} + +#include "FloatSingle.h" +#include "Vector3.h" + +const float epsilon = 1E-05f; + +template +float SphericalOf::DistanceBetween(const SphericalOf& v1, + const SphericalOf& v2) { + // SphericalOf difference = v1 - v2; + // return difference.distance; + Vector3 vec1 = v1.ToVector3(); + Vector3 vec2 = v2.ToVector3(); + float distance = Vector3::Distance(vec1, vec2); + return distance; +} + +template +AngleOf SphericalOf::AngleBetween(const SphericalOf& v1, + const SphericalOf& v2) { + // float denominator = v1.distance * v2.distance; + // if (denominator < epsilon) + // return 0.0f; + + Vector3 v1_3 = v1.ToVector3(); + Vector3 v2_3 = v2.ToVector3(); + // float dot = Vector3::Dot(v1_3, v2_3); + // float fraction = dot / denominator; + // if (isnan(fraction)) + // return fraction; // short cut to returning NaN universally + + // float cdot = Float::Clamp(fraction, -1.0, 1.0); + // float r = ((float)acos(cdot)) * Rad2Deg; + AngleSingle r = Vector3::Angle(v1_3, v2_3); + return AngleOf::Degrees(r.InDegrees()); +} + +template +AngleOf SphericalOf::SignedAngleBetween(const SphericalOf& v1, + const SphericalOf& v2, + const SphericalOf& axis) { + Vector3 v1_vector = v1.ToVector3(); + Vector3 v2_vector = v2.ToVector3(); + Vector3 axis_vector = axis.ToVector3(); + AngleSingle r = Vector3::SignedAngle(v1_vector, v2_vector, axis_vector); + return AngleOf::Degrees(r.InDegrees()); +} + +template +SphericalOf SphericalOf::Rotate(const SphericalOf& v, + AngleOf horizontalAngle, + AngleOf verticalAngle) { + SphericalOf r = + SphericalOf(v.distance, v.direction.horizontal + horizontalAngle, + v.direction.vertical + verticalAngle); + return r; +} +template +SphericalOf SphericalOf::RotateHorizontal(const SphericalOf& v, + AngleOf a) { + SphericalOf r = + SphericalOf(v.distance, v.direction.horizontal + a, v.direction.vertical); + return r; +} +template +SphericalOf SphericalOf::RotateVertical(const SphericalOf& v, + AngleOf a) { + SphericalOf r = + SphericalOf(v.distance, v.direction.horizontal, v.direction.vertical + a); + return r; +} + +template class SphericalOf; +template class SphericalOf; + +} // namespace LinearAlgebra \ No newline at end of file diff --git a/Spherical.h b/Spherical.h index 7c98263..309db03 100644 --- a/Spherical.h +++ b/Spherical.h @@ -1,66 +1,193 @@ -/// @copyright -/// This Source Code Form is subject to the terms of the Mozilla Public -/// License, v. 2.0.If a copy of the MPL was not distributed with this -/// file, You can obtain one at https ://mozilla.org/MPL/2.0/. +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0.If a copy of the MPL was not distributed with this +// file, You can obtain one at https ://mozilla.org/MPL/2.0/. #ifndef SPHERICAL_H #define SPHERICAL_H -#include "Angle.h" -#include "Polar.h" +#include "Direction.h" + +namespace LinearAlgebra { struct Vector3; +template +class PolarOf; -namespace Passer { - -/// @brief A spherical vector -/// @details -/// This is a vector in 3D space using a spherical coordinate system. -/// It consists of a distance and the polar and elevation angles from a -/// reference direction. The reference direction is typically thought of -/// as a forward direction. -struct Spherical { -public: +/// @brief A spherical vector using angles in various representations +/// @tparam T The implementation type used for the representations of the agles +template +class SphericalOf { + public: + /// @brief The distance in meters + /// @remark The distance should never be negative float distance; + /// @brief The direction of the vector + DirectionOf direction; - /// @brief The angle in the horizontal plane in degrees, clockwise rotation - /// @details The angle is automatically normalized to -180 .. 180 - Angle horizontalAngle; - /// @brief The angle in the vertical plane in degrees. Positive is upward. - /// @details The angle is automatically normalized to -180 .. 180 - Angle verticalAngle; + SphericalOf(); + SphericalOf(float distance, AngleOf horizontal, AngleOf vertical); + SphericalOf(float distance, DirectionOf direction); - /// @brief Create a new spherical vector with zero degrees and distance - Spherical(); - /// @brief Create a new spherical vector - /// @param polarAngle The angle in the horizontal plane in degrees, - /// clockwise rotation - /// @param elevationAngle The angle in the vertical plan in degrees, - /// zero is forward, positive is upward + /// @brief Create spherical vector without using AngleOf type. All given + /// angles are in degrees /// @param distance The distance in meters - // Spherical(float polarAngle, float elevationAngle, float distance); + /// @param horizontal The horizontal angle in degrees + /// @param vertical The vertical angle in degrees + /// @return The spherical vector + static SphericalOf Degrees(float distance, + float horizontal, + float vertical); + /// @brief Short-hand Deg alias for the Degrees function + constexpr static auto Deg = Degrees; + /// @brief Create sperical vector without using the AngleOf type. All given + /// angles are in radians + /// @param distance The distance in meters + /// @param horizontal The horizontal angle in radians + /// @param vertical The vertical angle in radians + /// @return The spherical vectpr + static SphericalOf Radians(float distance, + float horizontal, + float vertical); + // Short-hand Rad alias for the Radians function + constexpr static auto Rad = Radians; - Spherical(float distance, Angle horizontalAngle, Angle verticalAngle); + /// @brief Create a Spherical coordinate from a Polar coordinate + /// @param v The polar coordinate + /// @return The spherical coordinate with the vertical angle set to zero. + static SphericalOf FromPolar(PolarOf v); - /// @brief Convert polar coordinates to spherical coordinates - /// @param polar The polar coordinate - Spherical(Polar polar); - - /// @brief Convert 3D carthesian coordinates to spherical coordinates - /// @param v Vector in 3D carthesian coordinates; - Spherical(Vector3 v); + /// @brief Create a Spherical coordinate from a Vector3 coordinate + /// @param v The vector coordinate + /// @return The spherical coordinate + static SphericalOf FromVector3(Vector3 v); + /// @brief Convert the spherical coordinate to a Vector3 coordinate + /// @return The vector coordinate + Vector3 ToVector3() const; /// @brief A spherical vector with zero degree angles and distance - const static Spherical zero; + const static SphericalOf zero; + /// @brief A normalized forward-oriented vector + const static SphericalOf forward; + /// @brief A normalized back-oriented vector + const static SphericalOf back; + /// @brief A normalized right-oriented vector + const static SphericalOf right; + /// @brief A normalized left-oriented vector + const static SphericalOf left; + /// @brief A normalized up-oriented vector + const static SphericalOf up; + /// @brief A normalized down-oriented vector + const static SphericalOf down; - float GetSwing(); + /// @brief Update the distance component of the spherical coordinate + /// @param distance The new distance + /// @return The updated coordinate + SphericalOf WithDistance(float distance); - Polar ProjectOnHorizontalPlane(); + /// @brief Negate the vector + /// @return The negated vector + /// This will rotate the vector by 180 degrees horizontally and + /// vertically. Distance will stay the same. + SphericalOf operator-() const; + + /// @brief Subtract a spherical vector from this vector + /// @param v The vector to subtract + /// @return The result of the subtraction + SphericalOf operator-(const SphericalOf& v) const; + SphericalOf operator-=(const SphericalOf& v); + /// @brief Add a spherical vector to this vector + /// @param v The vector to add + /// @return The result of the addition + SphericalOf operator+(const SphericalOf& v) const; + SphericalOf operator+=(const SphericalOf& v); + + /// @brief Scale the vector uniformly up + /// @param f The scaling factor + /// @return The scaled vector + /// @remark This operation will scale the distance of the vector. The angle + /// will be unaffected. + friend SphericalOf operator*(const SphericalOf& v, float f) { + return SphericalOf(v.distance * f, v.direction); + } + friend SphericalOf operator*(float f, const SphericalOf& v) { + return SphericalOf(f * v.distance, v.direction); + } + SphericalOf operator*=(float f); + /// @brief Scale the vector uniformly down + /// @param f The scaling factor + /// @return The scaled factor + /// @remark This operation will scale the distance of the vector. The angle + /// will be unaffected. + friend SphericalOf operator/(const SphericalOf& v, float f) { + return SphericalOf(v.distance / f, v.direction); + } + friend SphericalOf operator/(float f, const SphericalOf& v) { + return SphericalOf(f / v.distance, v.direction); + } + SphericalOf operator/=(float f); + + /// @brief Calculate the distance between two spherical coordinates + /// @param v1 The first coordinate + /// @param v2 The second coordinate + /// @return The distance between the coordinates in meters + static float DistanceBetween(const SphericalOf& v1, + const SphericalOf& v2); + /// @brief Calculate the unsigned angle between two spherical vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The unsigned angle between the vectors + static AngleOf AngleBetween(const SphericalOf& v1, + const SphericalOf& v2); + /// @brief Calculate the signed angle between two spherical vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @param axis The axis are which the angle is calculated + /// @return The signed angle between the vectors + static AngleOf SignedAngleBetween(const SphericalOf& v1, + const SphericalOf& v2, + const SphericalOf& axis); + + /// @brief Rotate a spherical vector + /// @param v The vector to rotate + /// @param horizontalAngle The horizontal rotation angle in local space + /// @param verticalAngle The vertical rotation angle in local space + /// @return The rotated vector + static SphericalOf Rotate(const SphericalOf& v, + AngleOf horizontalAngle, + AngleOf verticalAngle); + /// @brief Rotate a spherical vector horizontally + /// @param v The vector to rotate + /// @param angle The horizontal rotation angle in local space + /// @return The rotated vector + static SphericalOf RotateHorizontal(const SphericalOf& v, + AngleOf angle); + /// @brief Rotate a spherical vector vertically + /// @param v The vector to rotate + /// @param angle The vertical rotation angle in local space + /// @return The rotated vector + static SphericalOf RotateVertical(const SphericalOf& v, + AngleOf angle); }; -} // namespace Passer -using namespace Passer; +/// @brief Shorthand notation for a spherical vector using single precision +/// floats for the angles This is the fastest implementation on devices with +/// floating point harware +using SphericalSingle = SphericalOf; +/// @brief Shorthand notation for a spherical vector using signed 16-bit words +/// for the angles +/// @note This is the fastest implementation on devices without floating point +/// hardware +using Spherical16 = SphericalOf; +#if defined(ARDUINO) +using Spherical = Spherical16; +#else +using Spherical = SphericalSingle; +#endif + +} // namespace LinearAlgebra + +#include "Polar.h" #include "Vector3.h" #endif \ No newline at end of file diff --git a/SwingTwist.cpp b/SwingTwist.cpp new file mode 100644 index 0000000..deca73c --- /dev/null +++ b/SwingTwist.cpp @@ -0,0 +1,172 @@ +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0.If a copy of the MPL was not distributed with this +// file, You can obtain one at https ://mozilla.org/MPL/2.0/. + +#include "SwingTwist.h" + +namespace LinearAlgebra { + +template +SwingTwistOf::SwingTwistOf() { + this->swing = DirectionOf(AngleOf(), AngleOf()); + this->twist = AngleOf(); +} + +template +SwingTwistOf::SwingTwistOf(DirectionOf swing, AngleOf twist) { + // Normalize angles + AngleOf deg90 = AngleOf::Degrees(90); + AngleOf deg180 = AngleOf::Degrees(180); + + if (swing.vertical > deg90 || swing.vertical < -deg90) { + swing.horizontal += deg180; + swing.vertical = deg180 - swing.vertical; + twist += deg180; + } + + this->swing = swing; + this->twist = twist; +} + +template +SwingTwistOf::SwingTwistOf(AngleOf horizontal, + AngleOf vertical, + AngleOf twist) { + // Normalize angles + AngleOf deg90 = AngleOf::Degrees(90); + AngleOf deg180 = AngleOf::Degrees(180); + + if (vertical > deg90 || vertical < -deg90) { + horizontal += deg180; + vertical = deg180 - vertical; + twist += deg180; + } + + this->swing = DirectionOf(horizontal, vertical); + this->twist = twist; +} + +template +SwingTwistOf SwingTwistOf::Degrees(float horizontal, + float vertical, + float twist) { + SwingTwistOf orientation = SwingTwistOf(AngleOf::Degrees(horizontal), + -AngleOf::Degrees(vertical), + AngleOf::Degrees(twist)); + // DirectionOf swing = DirectionOf::Degrees(horizontal, vertical); + // AngleOf twistAngle = AngleOf::Degrees(twist); + // SwingTwistOf orientation = SwingTwistOf(swing, twistAngle); + return orientation; +} + +template +Quaternion SwingTwistOf::ToQuaternion() const { + Quaternion q = Quaternion::Euler(-this->swing.vertical.InDegrees(), + this->swing.horizontal.InDegrees(), + this->twist.InDegrees()); + return q; +} + +template +SwingTwistOf SwingTwistOf::FromQuaternion(Quaternion q) { + Vector3 angles = Quaternion::ToAngles(q); + SwingTwistOf r = + SwingTwistOf::Degrees(angles.Up(), angles.Right(), angles.Forward()); + r.Normalize(); + return r; +} + +template +SphericalOf SwingTwistOf::ToAngleAxis() const { + Quaternion q = this->ToQuaternion(); + float angle; + Vector3 axis; + q.ToAngleAxis(&angle, &axis); + DirectionOf direction = DirectionOf::FromVector3(axis); + + SphericalOf aa = SphericalOf(angle, direction); + return aa; +} + +template +SwingTwistOf SwingTwistOf::FromAngleAxis(SphericalOf aa) { + Vector3 vectorAxis = aa.direction.ToVector3(); + Quaternion q = Quaternion::AngleAxis(aa.distance, vectorAxis); + return SwingTwistOf(); +} + +template +bool SwingTwistOf::operator==(const SwingTwistOf s) const { + return (this->swing == s.swing) && (this->twist == s.twist); +} + +template +const SwingTwistOf SwingTwistOf::identity = SwingTwistOf(); + +template +SphericalOf SwingTwistOf::operator*(const SphericalOf& vector) const { + SphericalOf v = SphericalOf( + vector.distance, vector.direction.horizontal + this->swing.horizontal, + vector.direction.vertical + this->swing.vertical); + return v; +} + +template +SwingTwistOf SwingTwistOf::operator*( + const SwingTwistOf& rotation) const { + SwingTwistOf r = + SwingTwistOf(this->swing.horizontal + rotation.swing.horizontal, + this->swing.vertical + rotation.swing.vertical, + this->twist + rotation.twist); + return r; +} + +template +SwingTwistOf SwingTwistOf::operator*=(const SwingTwistOf& rotation) { + this->swing.horizontal += rotation.swing.horizontal; + this->swing.vertical += rotation.swing.vertical; + this->twist += rotation.twist; + return *this; +} + +template +SwingTwistOf SwingTwistOf::Inverse(SwingTwistOf rotation) { + SwingTwistOf r = SwingTwistOf( + -rotation.swing.horizontal, -rotation.swing.vertical, -rotation.twist); + return r; +} + +template +SwingTwistOf SwingTwistOf::AngleAxis(float angle, + const DirectionOf& axis) { + Vector3 axis_vector = axis.ToVector3(); + Quaternion q = Quaternion::AngleAxis(angle, axis_vector); + SwingTwistOf r = SwingTwistOf::FromQuaternion(q); + return r; +} + +template +AngleOf SwingTwistOf::Angle(const SwingTwistOf& r1, + const SwingTwistOf& r2) { + Quaternion q1 = r1.ToQuaternion(); + Quaternion q2 = r2.ToQuaternion(); + float angle = Quaternion::Angle(q1, q2); + return AngleOf::Degrees(angle); +} + +template +void SwingTwistOf::Normalize() { + AngleOf deg90 = AngleOf::Degrees(90); + AngleOf deg180 = AngleOf::Degrees(180); + + if (this->swing.vertical > deg90 || this->swing.vertical < -deg90) { + this->swing.horizontal += deg180; + this->swing.vertical = deg180 - this->swing.vertical; + this->twist += deg180; + } +} + +template class SwingTwistOf; +template class SwingTwistOf; + +} \ No newline at end of file diff --git a/SwingTwist.h b/SwingTwist.h new file mode 100644 index 0000000..1800e43 --- /dev/null +++ b/SwingTwist.h @@ -0,0 +1,85 @@ +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0.If a copy of the MPL was not distributed with this +// file, You can obtain one at https ://mozilla.org/MPL/2.0/. + +#ifndef SWINGTWIST_H +#define SWINGTWIST_H + +#include "Angle.h" +#include "Direction.h" +#include "Quaternion.h" +#include "Spherical.h" + +namespace LinearAlgebra { + +/// @brief An orientation using swing and twist angles in various +/// representations +/// @tparam T The implmentation type used for the representation of the angles +template +class SwingTwistOf { + public: + DirectionOf swing; + AngleOf twist; + + SwingTwistOf(); + SwingTwistOf(DirectionOf swing, AngleOf twist); + SwingTwistOf(AngleOf horizontal, AngleOf vertical, AngleOf twist); + + static SwingTwistOf Degrees(float horizontal, + float vertical = 0, + float twist = 0); + + Quaternion ToQuaternion() const; + static SwingTwistOf FromQuaternion(Quaternion q); + + SphericalOf ToAngleAxis() const; + static SwingTwistOf FromAngleAxis(SphericalOf aa); + + const static SwingTwistOf identity; + + bool operator==(const SwingTwistOf d) const; + + /// + /// Rotate a vector using this rotation + /// + /// The vector to rotate + /// The rotated vector + SphericalOf operator*(const SphericalOf& vector) const; + /// + /// Multiply this rotation with another rotation + /// + /// The swing/twist rotation to multiply with + /// The resulting swing/twist rotation + /// The result will be this rotation rotated according to + /// the give rotation. + SwingTwistOf operator*(const SwingTwistOf& rotation) const; + SwingTwistOf operator*=(const SwingTwistOf& rotation); + + static SwingTwistOf Inverse(SwingTwistOf rotation); + + /// + /// Convert an angle/axis representation to a swingt + /// + /// The angle + /// The axis + /// The resulting quaternion + static SwingTwistOf AngleAxis(float angle, const DirectionOf& axis); + + static AngleOf Angle(const SwingTwistOf& r1, const SwingTwistOf& r2); + + void Normalize(); +}; + +using SwingTwistSingle = SwingTwistOf; +using SwingTwist16 = SwingTwistOf; + +#if defined(ARDUINO) +using SwingTwist = SwingTwist16; +#else +using SwingTwist = SwingTwistSingle; +#endif + +} // namespace LinearAlgebra +using namespace LinearAlgebra; + +#endif diff --git a/Vector2.cpp b/Vector2.cpp index 150b12f..1e34a99 100644 --- a/Vector2.cpp +++ b/Vector2.cpp @@ -5,26 +5,37 @@ #include "Vector2.h" #include "Angle.h" #include "FloatSingle.h" +#include "Vector3.h" -#if defined(AVR) -#include -#else +// #if defined(AVR) +// #include +// #else #include -#endif +// #endif Vector2::Vector2() { x = 0; y = 0; } - Vector2::Vector2(float _x, float _y) { x = _x; y = _y; } +// Vector2::Vector2(Vec2 v) { +// x = v.x; +// y = v.y; +// } +Vector2::Vector2(Vector3 v) { + x = v.Right(); // x; + y = v.Forward(); // z; +} +Vector2::Vector2(PolarSingle p) { + float horizontalRad = p.angle.InDegrees() * Deg2Rad; + float cosHorizontal = cosf(horizontalRad); + float sinHorizontal = sinf(horizontalRad); -Vector2::Vector2(Vec2 v) { - x = v.x; - y = v.y; + x = p.distance * sinHorizontal; + y = p.distance * cosHorizontal; } Vector2::~Vector2() {} @@ -38,15 +49,24 @@ const Vector2 Vector2::down = Vector2(0, -1); const Vector2 Vector2::forward = Vector2(0, 1); const Vector2 Vector2::back = Vector2(0, -1); -float Vector2::Magnitude(const Vector2 &a) { - return sqrtf(a.x * a.x + a.y * a.y); +bool Vector2::operator==(const Vector2& v) { + return (this->x == v.x && this->y == v.y); } -float Vector2::magnitude() const { return (float)sqrtf(x * x + y * y); } -float Vector2::SqrMagnitude(const Vector2 &a) { return a.x * a.x + a.y * a.y; } -float Vector2::sqrMagnitude() const { return (x * x + y * y); } +float Vector2::Magnitude(const Vector2& v) { + return sqrtf(v.x * v.x + v.y * v.y); +} +float Vector2::magnitude() const { + return (float)sqrtf(x * x + y * y); +} +float Vector2::SqrMagnitude(const Vector2& v) { + return v.x * v.x + v.y * v.y; +} +float Vector2::sqrMagnitude() const { + return (x * x + y * y); +} -Vector2 Vector2::Normalize(Vector2 v) { +Vector2 Vector2::Normalize(const Vector2& v) { float num = Vector2::Magnitude(v); Vector2 result = Vector2::zero; if (num > Float::epsilon) { @@ -63,56 +83,67 @@ Vector2 Vector2::normalized() const { return result; } -Vector2 Vector2::ClampMagnitude(const Vector2 &v, float magnitude) { - float length = Vector2::Magnitude(v); - Vector2 r = v; - if (length > magnitude) - r = v / (length * magnitude); - - return r; +Vector2 Vector2::operator-() { + return Vector2(-this->x, -this->y); } -Vector2 Vector2::operator-(const Vector2 &v2) const { - return Vector2(this->x - v2.x, this->y - v2.y); +Vector2 Vector2::operator-(const Vector2& v) const { + return Vector2(this->x - v.x, this->y - v.y); +} +Vector2 Vector2::operator-=(const Vector2& v) { + this->x -= v.x; + this->y -= v.y; + return *this; +} +Vector2 Vector2::operator+(const Vector2& v) const { + return Vector2(this->x + v.x, this->y + v.y); +} +Vector2 Vector2::operator+=(const Vector2& v) { + this->x += v.x; + this->y += v.y; + return *this; } -Vector2 Vector2::operator-() { return Vector2(-this->x, -this->y); } - -Vector2 Vector2::operator+(const Vector2 &v2) const { - return Vector2(this->x + v2.x, this->y + v2.y); +Vector2 Vector2::Scale(const Vector2& v1, const Vector2& v2) { + return Vector2(v1.x * v2.x, v1.y * v2.y); +} +// Vector2 Passer::LinearAlgebra::operator*(const Vector2 &v, float f) { +// return Vector2(v.x * f, v.y * f); +// } +// Vector2 Passer::LinearAlgebra::operator*(float f, const Vector2 &v) { +// return Vector2(v.x * f, v.y * f); +// } +Vector2 Vector2::operator*=(float f) { + this->x *= f; + this->y *= f; + return *this; +} +// Vector2 Passer::LinearAlgebra::operator/(const Vector2 &v, float f) { +// return Vector2(v.x / f, v.y / f); +// } +// Vector2 Passer::LinearAlgebra::operator/(float f, const Vector2 &v) { +// return Vector2(v.x / f, v.y / f); +// } +Vector2 Vector2::operator/=(float f) { + this->x /= f; + this->y /= f; + return *this; } -Vector2 Vector2::Scale(const Vector2 &p1, const Vector2 &p2) { - return Vector2(p1.x * p2.x, p1.y * p2.y); -} - -Vector2 Vector2::operator*(float f) const { - return Vector2(this->x * f, this->y * f); -} - -Vector2 Vector2::operator/(const float &d) const { - return Vector2(this->x / d, this->y / d); -} - -float Vector2::Dot(const Vector2 &v1, const Vector2 &v2) { +float Vector2::Dot(const Vector2& v1, const Vector2& v2) { return v1.x * v2.x + v1.y * v2.y; } -bool Vector2::operator==(const Vector2 &v) { - return (this->x == v.x && this->y == v.y); +float Vector2::Distance(const Vector2& v1, const Vector2& v2) { + return Magnitude(v1 - v2); } -float Vector2::Distance(const Vector2 &p1, const Vector2 &p2) { - return Magnitude(p1 - p2); +float Vector2::Angle(const Vector2& v1, const Vector2& v2) { + return (float)fabs(SignedAngle(v1, v2)); } - -float Vector2::Angle(Vector2 from, Vector2 to) { - return (float)fabs(SignedAngle(from, to)); -} - -float Vector2::SignedAngle(Vector2 from, Vector2 to) { - float sqrMagFrom = from.sqrMagnitude(); - float sqrMagTo = to.sqrMagnitude(); +float Vector2::SignedAngle(const Vector2& v1, const Vector2& v2) { + float sqrMagFrom = v1.sqrMagnitude(); + float sqrMagTo = v2.sqrMagnitude(); if (sqrMagFrom == 0 || sqrMagTo == 0) return 0; @@ -123,16 +154,16 @@ float Vector2::SignedAngle(Vector2 from, Vector2 to) { return nanf(""); #endif - float angleFrom = atan2(from.y, from.x); - float angleTo = atan2(to.y, to.x); - return (angleTo - angleFrom) * Angle::Rad2Deg; + float angleFrom = atan2f(v1.y, v1.x); + float angleTo = atan2f(v2.y, v2.x); + return -(angleTo - angleFrom) * Rad2Deg; } -Vector2 Vector2::Rotate(Vector2 v, float angle) { - float angleRad = angle * Angle::Deg2Rad; +Vector2 Vector2::Rotate(const Vector2& v, AngleSingle a) { + float angleRad = a.InDegrees() * Deg2Rad; #if defined(AVR) float sinValue = sin(angleRad); - float cosValue = cos(angleRad); // * Angle::Deg2Rad); + float cosValue = cos(angleRad); // * Angle::Deg2Rad); #else float sinValue = (float)sinf(angleRad); float cosValue = (float)cosf(angleRad); @@ -140,16 +171,12 @@ Vector2 Vector2::Rotate(Vector2 v, float angle) { float tx = v.x; float ty = v.y; - v.x = (cosValue * tx) - (sinValue * ty); - v.y = (sinValue * tx) + (cosValue * ty); - return v; + Vector2 r = Vector2((cosValue * tx) - (sinValue * ty), + (sinValue * tx) + (cosValue * ty)); + return r; } -Vector2 Vector2::Lerp(Vector2 from, Vector2 to, float f) { - Vector2 v = from + (to - from) * f; +Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) { + Vector2 v = v1 + (v2 - v1) * f; return v; } - -float Vector2::ToFactor(Vector2 a, Vector2 b) { - return (1 - Vector2::Dot(a, b)) / 2; -} diff --git a/Vector2.h b/Vector2.h index c2b0d11..04fa669 100644 --- a/Vector2.h +++ b/Vector2.h @@ -5,9 +5,11 @@ #ifndef VECTOR2_H #define VECTOR2_H +#include "Angle.h" + extern "C" { /// -/// 2-dimensional Vector representation +/// 2-dimensional Vector representation (C-style) /// /// This is a C-style implementation /// This uses the right-handed coordinate system. @@ -24,215 +26,184 @@ typedef struct Vec2 { } Vec2; } -/// -/// A 2-dimensional vector -/// -/// This uses the right-handed coordinate system. -struct Vector2 : Vec2 { -public: - /// - /// Create a new 2-dimensinal zero vector - /// - Vector2(); - /// - /// Create a new 2-dimensional vector - /// - /// x axis value - /// y axis value - Vector2(float x, float y); - /// - /// Create a vector from C-style Vec2 - /// - /// The C-style Vec - Vector2(Vec2 v); +namespace LinearAlgebra { +struct Vector3; +template +class PolarOf; + +/// @brief A 2-dimensional vector +/// @remark This uses the right=handed carthesian coordinate system. +/// @note This implementation intentionally avoids the use of x and y +struct Vector2 : Vec2 { + friend struct Vec2; + + public: + /// @brief A new 2-dimensional zero vector + Vector2(); + /// @brief A new 2-dimensional vector + /// @param right The distance in the right direction in meters + /// @param forward The distance in the forward direction in meters + Vector2(float right, float forward); + /// @brief Convert a Vector3 to a Vector2 + /// @param v The 3D vector + /// @note This will project the vector to the horizontal plane + Vector2(Vector3 v); + /// @brief Convert a Polar vector to a 2-dimensional vector + /// @param v The vector in polar coordinates + Vector2(PolarOf v); + + /// @brief Vector2 destructor ~Vector2(); - /// - /// A vector with zero for all axis - /// + /// @brief A vector with zero for all axis const static Vector2 zero; - /// - /// A vector with values (1, 1) - /// + /// @brief A vector with one for all axis const static Vector2 one; - /// - /// A vector with values (1, 0) - /// - /// - const static Vector2 right; - /// - /// A vector3 with values (-1, 0) - /// - const static Vector2 left; - /// - /// A vector with values (0, 1) - /// - const static Vector2 up; - /// - /// A vector with values (0, -1) - /// - const static Vector2 down; - /// - /// A vector with values (0, 1) - /// + /// @brief A normalized forward-oriented vector const static Vector2 forward; - /// - /// A vector with values (0, -1) - /// + /// @brief A normalized back-oriented vector const static Vector2 back; + /// @brief A normalized right-oriented vector + const static Vector2 right; + /// @brief A normalized left-oriented vector + const static Vector2 left; + /// @brief A normalized up-oriented vector + /// @note This is a convenience function which is equal to Vector2::forward + const static Vector2 up; + /// @brief A normalized down-oriented vector + /// @note This is a convenience function which is equal to Vector2::down + const static Vector2 down; - /// - /// The length of a vector - /// - /// The vector for which you need the length - /// The length of the given vector - static float Magnitude(const Vector2 &vector); - /// - /// The length of this vector - /// - /// The length of this vector + /// @brief Check if this vector to the given vector + /// @param v The vector to check against + /// @return true if it is identical to the given vector + /// @note This uses float comparison to check equality which may have strange + /// effects. Equality on floats should be avoided. + bool operator==(const Vector2& v); + + /// @brief The vector length + /// @param v The vector for which you need the length + /// @return The vector length + static float Magnitude(const Vector2& v); + /// @brief The vector length + /// @return The vector length float magnitude() const; - /// - /// The squared length of a vector - /// - /// The vector for which you need the squared - /// length The squatred length The squared length - /// is computationally simpler than the real length. Think of Pythagoras A^2 + - /// B^2 = C^2. This leaves out the calculation of the squared root of C. - static float SqrMagnitude(const Vector2 &vector); - /// - /// The squared length of this vector - /// - /// The squared length - /// The squared length is computationally simpler than the real length. - /// Think of Pythagoras A^2 + B^2 = C^2. - /// This leaves out the calculation of the squared root of C. + /// @brief The squared vector length + /// @param v The vector for which you need the squared length + /// @return The squared vector length + /// @remark The squared length is computationally simpler than the real + /// length. Think of Pythagoras A^2 + B^2 = C^2. This prevents the calculation + /// of the squared root of C. + static float SqrMagnitude(const Vector2& v); + /// @brief The squared vector length + /// @return The squared vector length + /// @remark The squared length is computationally simpler than the real + /// length. Think of Pythagoras A^2 + B^2 = C^2. This prevents the calculation + /// of the squared root of C. float sqrMagnitude() const; - /// - /// Connvert a vector to a length of 1 - /// - /// The vector to convert - /// The vector with length 1 - static Vector2 Normalize(Vector2 vector); - /// - /// Convert the vector to a length of a - /// - /// The vector with length 1 + + /// @brief Convert the vector to a length of 1 + /// @param v The vector to convert + /// @return The vector normalized to a length of 1 + static Vector2 Normalize(const Vector2& v); + /// @brief Convert the vector to a length 1 + /// @return The vector normalized to a length of 1 Vector2 normalized() const; - static Vector2 ClampMagnitude(const Vector2 &v, float magnitude); - - /// - /// Negate the vector - /// - /// The negated vector - /// This will result in a vector pointing in the opposite direction + /// @brief Negate the vector such that it points in the opposite direction + /// @return The negated vector Vector2 operator-(); - /// - /// Subtract a vector from this vector - /// - /// The vector to subtract from this vector - /// The result of the subtraction - Vector2 operator-(const Vector2 &vector) const; - /// - /// Add another vector to this vector - /// - /// The vector to add - /// The result of adding the vector - Vector2 operator+(const Vector2 &vector2) const; + /// @brief Subtract a vector from this vector + /// @param v The vector to subtract from this vector + /// @return The result of the subtraction + Vector2 operator-(const Vector2& v) const; + Vector2 operator-=(const Vector2& v); + /// @brief Add a vector to this vector + /// @param v The vector to add to this vector + /// @return The result of the addition + Vector2 operator+(const Vector2& v) const; + Vector2 operator+=(const Vector2& v); - /// - /// Scale a vector using another vector - /// - /// The vector to scale - /// A vector with scaling factors - /// The scaled vector - /// Each component of the vector v1 will be multiplied with the - /// component from the scaling vector v2. - static Vector2 Scale(const Vector2 &vector1, const Vector2 &vector2); - /// - /// Scale a vector uniformly up - /// - /// The scaling factor - /// The scaled vector - /// Each component of the vector will be multipled with the same factor. - Vector2 operator*(float factor) const; - /// - /// Scale a vector uniformy down - /// - /// The scaling factor - /// The scaled vector - /// Each componet of the vector will be divided by the same factor. - Vector2 operator/(const float &factor) const; + /// @brief Scale the vector using another vector + /// @param v1 The vector to scale + /// @param v2 A vector with the scaling factors + /// @return The scaled vector + /// @remark Each component of the vector v1 will be multiplied with the + /// matching component from the scaling vector v2. + static Vector2 Scale(const Vector2& v1, const Vector2& v2); + /// @brief Scale the vector uniformly up + /// @param f The scaling factor + /// @return The scaled vector + /// @remark Each component of the vector will be multipled with the same + /// factor f. + friend Vector2 operator*(const Vector2& v, float f) { + return Vector2(v.x * f, v.y * f); + } + friend Vector2 operator*(float f, const Vector2& v) { + return Vector2(v.x * f, v.y * f); + // return Vector2(f * v.x, f * v.y); + } + Vector2 operator*=(float f); + /// @brief Scale the vector uniformly down + /// @param f The scaling factor + /// @return The scaled vector + /// @remark Each componet of the vector will be divided by the same factor. + friend Vector2 operator/(const Vector2& v, float f) { + return Vector2(v.x / f, v.y / f); + } + friend Vector2 operator/(float f, const Vector2& v) { + return Vector2(f / v.x, f / v.y); + } + Vector2 operator/=(float f); - /// - /// The dot product of two vectors - /// - /// The first vector - /// The second vector - /// The dot product of the two vectors - static float Dot(const Vector2 &vector1, const Vector2 &vector2); + /// @brief The dot product of two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The dot product of the two vectors + static float Dot(const Vector2& v1, const Vector2& v2); - /// - /// Check is this vector is equal to the given vector - /// - /// The vector to check against - /// True if it is identical to the given vector - /// Note this uses float comparison to check equality which - /// may have strange effects. Equality on float should be avoided. - bool operator==(const Vector2 &vector); + /// @brief The distance between two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The distance between the two vectors + static float Distance(const Vector2& v1, const Vector2& v2); - /// - /// The distance between two vectors - /// - /// The first vector - /// The second vectors - /// The distance between the two vectors - static float Distance(const Vector2 &vector1, const Vector2 &vector2); + /// @brief The angle between two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The angle between the two vectors + /// @remark This reterns an unsigned angle which is the shortest distance + /// between the two vectors. Use Vector2::SignedAngle if a signed angle is + /// needed. + static float Angle(const Vector2& v1, const Vector2& v2); + /// @brief The signed angle between two vectors + /// @param v1 The starting vector + /// @param v2 The ending vector + /// @return The signed angle between the two vectors + static float SignedAngle(const Vector2& v1, const Vector2& v2); - /// - /// Calculate the angle between two vectors - /// - /// The first vector - /// The second vector - /// The angle - /// This reterns an unsigned angle which is the shortest distance - /// between the two vectors. Use Vector3::SignedAngle if a - /// signed angle is needed. - static float Angle(Vector2 vector1, Vector2 vector2); + /// @brief Rotate the vector + /// @param v The vector to rotate + /// @param a The angle in degrees to rotate + /// @return The rotated vector + static Vector2 Rotate(const Vector2& v, AngleSingle a); - /// - /// Calculate the angle between two vectors rotation around an axis. - /// - /// The starting vector - /// The ending vector - /// The axis to rotate around - /// The signed angle - static float SignedAngle(Vector2 from, Vector2 to); - - /// - /// Rotate the vector - /// - /// The vector to rotate - /// Angle in radias to rotate - /// The rotated vector - static Vector2 Rotate(Vector2 v, float angle); - - /// - /// Lerp between two vectors - /// - /// The from vector - /// The to vector - /// The interpolation distance (0..1) - /// The lerped vector - /// The factor f is unclamped. Value 0 matches the *from* vector, Value 1 - /// matches the *to* vector Value -1 is *from* vector minus the difference - /// between *from* and *to* etc. - static Vector2 Lerp(Vector2 from, Vector2 to, float f); - - static float ToFactor(Vector2 a, Vector2 b); + /// @brief Lerp (linear interpolation) between two vectors + /// @param v1 The starting vector + /// @param v2 The end vector + /// @param f The interpolation distance + /// @return The lerped vector + /// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value + /// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference + /// between *v1* and *v2* etc. + static Vector2 Lerp(const Vector2& v1, const Vector2& v2, float f); }; +} // namespace LinearAlgebra +using namespace LinearAlgebra; + +#include "Polar.h" + #endif \ No newline at end of file diff --git a/Vector3.cpp b/Vector3.cpp index c08c1c0..7ec6a95 100644 --- a/Vector3.cpp +++ b/Vector3.cpp @@ -3,6 +3,9 @@ // file, You can obtain one at https ://mozilla.org/MPL/2.0/. #include "Vector3.h" +#include "Angle.h" +#include "Spherical.h" + #include const float Deg2Rad = 0.0174532924F; @@ -10,21 +13,38 @@ const float Rad2Deg = 57.29578F; const float epsilon = 1E-05f; Vector3::Vector3() { - x = 0; - y = 0; - z = 0; + this->x = 0; + this->y = 0; + this->z = 0; } -Vector3::Vector3(float _x, float _y, float _z) { - x = _x; - y = _y; - z = _z; +Vector3::Vector3(float right, float up, float forward) { + this->x = right; + this->y = up; + this->z = forward; } -Vector3::Vector3(Vec3 v) { - x = v.x; - y = v.y; - z = v.z; +Vector3::Vector3(Vector2 v) { + this->x = v.x; + this->y = 0.0f; + this->z = v.y; +} + +Vector3::Vector3(SphericalOf s) { + float verticalRad = (90.0f - s.direction.vertical.InDegrees()) * Deg2Rad; + float horizontalRad = s.direction.horizontal.InDegrees() * Deg2Rad; + float cosVertical = cosf(verticalRad); + float sinVertical = sinf(verticalRad); + float cosHorizontal = cosf(horizontalRad); + float sinHorizontal = sinf(horizontalRad); + + x = s.distance * sinVertical * sinHorizontal; + y = s.distance * cosVertical; + z = s.distance * sinVertical * cosHorizontal; + // Vector3 v = Vector3(s.distance * sinVertical * sinHorizontal, + // s.distance * cosVertical, + // ); + // return v; } Vector3::~Vector3() {} @@ -41,21 +61,25 @@ const Vector3 Vector3::back = Vector3(0, 0, -1); // inline float Vector3::Forward() { return z; } // inline float Vector3::Up() { return y; } // inline float Vector3::Right() { return x; } -Vector3 Vector3::FromHorizontal(const Vector2 &v) { - return Vector3(v.x, 0, v.y); +// Vector3 Vector3::FromHorizontal(const Vector2 &v) { +// return Vector3(v.x, 0, v.y); +// } + +float Vector3::Magnitude(const Vector3& v) { + return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z); +} +float Vector3::magnitude() const { + return (float)sqrtf(x * x + y * y + z * z); } -float Vector3::Magnitude(const Vector3 &a) { - return sqrtf(a.x * a.x + a.y * a.y + a.z * a.z); +float Vector3::SqrMagnitude(const Vector3& v) { + return v.x * v.x + v.y * v.y + v.z * v.z; } -float Vector3::magnitude() const { return (float)sqrtf(x * x + y * y + z * z); } - -float Vector3::SqrMagnitude(const Vector3 &a) { - return a.x * a.x + a.y * a.y + a.z * a.z; +float Vector3::sqrMagnitude() const { + return (x * x + y * y + z * z); } -float Vector3::sqrMagnitude() const { return (x * x + y * y + z * z); } -Vector3 Vector3::Normalize(const Vector3 &v) { +Vector3 Vector3::Normalize(const Vector3& v) { float num = Vector3::Magnitude(v); Vector3 result = Vector3::zero; if (num > epsilon) { @@ -72,64 +96,87 @@ Vector3 Vector3::normalized() const { return result; } -Vector3 Vector3::operator-(const Vector3 &v2) const { - return Vector3(this->x - v2.x, this->y - v2.y, this->z - v2.z); +Vector3 Vector3::operator-() const { + return Vector3(-this->x, -this->y, -this->z); } -Vector3 Vector3::operator-() { return Vector3(-this->x, -this->y, -this->z); } - -Vector3 Vector3::operator+(const Vector3 &v2) const { - return Vector3(this->x + v2.x, this->y + v2.y, this->z + v2.z); +Vector3 Vector3::operator-(const Vector3& v) const { + return Vector3(this->x - v.x, this->y - v.y, this->z - v.z); +} +Vector3 Vector3::operator-=(const Vector3& v) { + this->x -= v.x; + this->y -= v.y; + this->z -= v.z; + return *this; +} +Vector3 Vector3::operator+(const Vector3& v) const { + return Vector3(this->x + v.x, this->y + v.y, this->z + v.z); +} +Vector3 Vector3::operator+=(const Vector3& v) { + this->x += v.x; + this->y += v.y; + this->z += v.z; + return *this; } -Vector3 Vector3::Scale(const Vector3 &p1, const Vector3 &p2) { - return Vector3(p1.x * p2.x, p1.y * p2.y, p1.z * p2.z); +Vector3 Vector3::Scale(const Vector3& v1, const Vector3& v2) { + return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z); +} +// Vector3 Passer::LinearAlgebra::operator*(const Vector3 &v, float f) { +// return Vector3(v.x * f, v.y * f, v.z * f); +// } +// Vector3 Passer::LinearAlgebra::operator*(float f, const Vector3 &v) { +// return Vector3(v.x * f, v.y * f, v.z * f); +// } +Vector3 Vector3::operator*=(float f) { + this->x *= f; + this->y *= f; + this->z *= f; + return *this; +} +// Vector3 Passer::LinearAlgebra::operator/(const Vector3 &v, float f) { +// return Vector3(v.x / f, v.y / f, v.z / f); +// } +// Vector3 Passer::LinearAlgebra::operator/(float f, const Vector3 &v) { +// return Vector3(v.x / f, v.y / f, v.z / f); +// } +Vector3 Vector3::operator/=(float f) { + this->x /= f; + this->y /= f; + this->z /= f; + return *this; } -Vector3 Vector3::operator*(float f) const { - return Vector3(this->x * f, this->y * f, this->z * f); -} - -Vector3 Vector3::operator/(float d) const { - return Vector3(this->x / d, this->y / d, this->z / d); -} - -float Vector3::Dot(const Vector3 &v1, const Vector3 &v2) { +float Vector3::Dot(const Vector3& v1, const Vector3& v2) { return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; } -bool Vector3::operator==(const Vector3 &v) { +bool Vector3::operator==(const Vector3& v) const { return (this->x == v.x && this->y == v.y && this->z == v.z); } -float Vector3::Distance(const Vector3 &p1, const Vector3 &p2) { - return Magnitude(p1 - p2); +float Vector3::Distance(const Vector3& v1, const Vector3& v2) { + return Magnitude(v1 - v2); } -Vector3 Vector3::Cross(const Vector3 &v1, const Vector3 &v2) { +Vector3 Vector3::Cross(const Vector3& v1, const Vector3& v2) { return Vector3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x); } -Vector3 Vector3::Project(const Vector3 &vector, const Vector3 &onNormal) { - float sqrMagnitude = Dot(onNormal, onNormal); +Vector3 Vector3::Project(const Vector3& v, const Vector3& n) { + float sqrMagnitude = Dot(n, n); if (sqrMagnitude < epsilon) return Vector3::zero; else { - float dot = Dot(vector, onNormal); - Vector3 r = onNormal * dot / sqrMagnitude; + float dot = Dot(v, n); + Vector3 r = n * dot / sqrMagnitude; return r; } } -Vector3 Vector3::ProjectOnPlane(const Vector3 &vector, - const Vector3 &planeNormal) { - Vector3 r = vector - Project(vector, planeNormal); - return r; -} - -Vector2 Vector3::ProjectHorizontalPlane(const Vector3 &vector) { - Vector2 r = Vector2(vector.x, vector.z); +Vector3 Vector3::ProjectOnPlane(const Vector3& v, const Vector3& n) { + Vector3 r = v - Project(v, n); return r; } @@ -139,37 +186,39 @@ float clamp(float x, float lower, float upper) { return upperClamp; } -float Vector3::Angle(const Vector3 &from, const Vector3 &to) { - float denominator = sqrtf(from.sqrMagnitude() * to.sqrMagnitude()); +AngleOf Vector3::Angle(const Vector3& v1, const Vector3& v2) { + float denominator = sqrtf(v1.sqrMagnitude() * v2.sqrMagnitude()); if (denominator < epsilon) - return 0; + return AngleOf(); - float dot = Vector3::Dot(from, to); + float dot = Vector3::Dot(v1, v2); float fraction = dot / denominator; if (isnan(fraction)) - return fraction; // short cut to returning NaN universally + return AngleOf::Degrees( + fraction); // short cut to returning NaN universally float cdot = clamp(fraction, -1.0, 1.0); - float r = ((float)acos(cdot)) * Rad2Deg; - return r; + float r = ((float)acos(cdot)); + return AngleOf::Radians(r); } -float Vector3::SignedAngle(const Vector3 &from, const Vector3 &to, - const Vector3 &axis) { +AngleOf Vector3::SignedAngle(const Vector3& v1, + const Vector3& v2, + const Vector3& axis) { // angle in [0,180] - float angle = Vector3::Angle(from, to); + AngleOf angle = Vector3::Angle(v1, v2); - Vector3 cross = Vector3::Cross(from, to); + Vector3 cross = Vector3::Cross(v1, v2); float b = Vector3::Dot(axis, cross); float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F); // angle in [-179,180] - float signed_angle = angle * signd; + AngleOf signed_angle = angle * signd; - return signed_angle; + return AngleOf(signed_angle); } -Vector3 Vector3::Lerp(const Vector3 &from, const Vector3 &to, float f) { - Vector3 v = from + (to - from) * f; +Vector3 Vector3::Lerp(const Vector3& v1, const Vector3& v2, float f) { + Vector3 v = v1 + (v2 - v1) * f; return v; -} \ No newline at end of file +} diff --git a/Vector3.h b/Vector3.h index 50fc1f3..914d72a 100644 --- a/Vector3.h +++ b/Vector3.h @@ -9,11 +9,12 @@ extern "C" { /// -/// 3-dimensional Vector representation +/// 3-dimensional Vector representation (C-style) /// /// This is a C-style implementation /// This uses the right-handed coordinate system. typedef struct Vec3 { + public: /// /// The right axis of the vector /// @@ -30,242 +31,203 @@ typedef struct Vec3 { } Vec3; } -/// -/// A 3-dimensional vector -/// -/// This uses the right-handed coordinate system. +namespace LinearAlgebra { + +template +class SphericalOf; + +/// @brief A 3-dimensional vector +/// @remark This uses a right-handed carthesian coordinate system. +/// @note This implementation intentionally avoids the use of x, y and z values. struct Vector3 : Vec3 { -public: - /// - /// Create a new 3-dimensinal zero vector - /// + friend struct Vec3; + + public: + /// @brief A new 3-dimensional zero vector Vector3(); - /// - /// Create a new 3-dimensional vector - /// - /// x axis value - /// y axis value - /// z axis value - Vector3(float x, float y, float z); - /// - /// Create a vector from C-style Vec3 - /// - /// The C-style Vec - Vector3(Vec3 v); + /// @brief A new 3-dimensional vector + /// @param right The distance in the right direction in meters + /// @param up The distance in the upward direction in meters + /// @param forward The distance in the forward direction in meters + Vector3(float right, float up, float forward); + /// @brief Convert a 2-dimenstional vector to a 3-dimensional vector + /// @param v The vector to convert + Vector3(Vector2 v); + /// @brief Convert vector in spherical coordinates to 3d carthesian + /// coordinates + /// @param v The vector to convert + Vector3(SphericalOf v); + + /// @brief Vector3 destructor ~Vector3(); - /// - /// A vector with zero for all axis - /// + /// @brief A vector with zero for all axis const static Vector3 zero; - /// - /// A vector with one for all axis - /// + /// @brief A vector with one for all axis const static Vector3 one; - /// - /// A normalized vector pointing in the right direction - /// - const static Vector3 right; - /// - /// A normalized vector pointing in the left direction - /// - const static Vector3 left; - /// - /// A normalized vector pointing in the upward direction - /// - const static Vector3 up; - /// - /// A normalized vector pointing in the downward direcion - /// - const static Vector3 down; - /// - /// A normalized vector pointing in the forward direction - /// + /// @brief A normalized forward-oriented vector const static Vector3 forward; - /// - /// A normalized vector pointing in the backward direction - /// + /// @brief A normalized back-oriented vector const static Vector3 back; + /// @brief A normalized right-oriented vector + const static Vector3 right; + /// @brief A normalized left-oriented vector + const static Vector3 left; + /// @brief A normalized up-oriented vector + const static Vector3 up; + /// @brief A normalized down-oriented vector + const static Vector3 down; - // Experimental Access functions which are intended to replace the use of XYZ - inline float Forward() { return z; }; - inline float Up() { return y; }; - inline float Right() { return x; }; - static Vector3 FromHorizontal(const Vector2 &vector); + // Access functions which are intended to replace the use of XYZ + inline float Forward() const { return z; }; + inline float Up() const { return y; }; + inline float Right() const { return x; }; - /// - /// The length of a vector - /// - /// The vector for which you need the length - /// The length of the given vector - static float Magnitude(const Vector3 &vector); - /// - /// The length of this vector - /// - /// The length of this vector + /// @brief Check if this vector to the given vector + /// @param v The vector to check against + /// @return true if it is identical to the given vector + /// @note This uses float comparison to check equality which may have strange + /// effects. Equality on floats should be avoided. + bool operator==(const Vector3& v) const; + + /// @brief The vector length + /// @param v The vector for which you need the length + /// @return The vector length + static float Magnitude(const Vector3& v); + /// @brief The vector length + /// @return The vector length float magnitude() const; - /// - /// The squared length of a vector - /// - /// The vector for which you need the squared - /// length The squatred length The squared length - /// is computationally simpler than the real length. Think of Pythagoras A^2 + - /// B^2 = C^2. This leaves out the calculation of the squared root of C. - static float SqrMagnitude(const Vector3 &vector); - /// - /// The squared length of this vector - /// - /// The squared length - /// The squared length is computationally simpler than the real length. - /// Think of Pythagoras A^2 + B^2 = C^2. - /// This leaves out the calculation of the squared root of C. + /// @brief The squared vector length + /// @param v The vector for which you need the length + /// @return The squared vector length + /// @remark The squared length is computationally simpler than the real + /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the + /// calculation of the squared root of C. + static float SqrMagnitude(const Vector3& v); + /// @brief The squared vector length + /// @return The squared vector length + /// @remark The squared length is computationally simpler than the real + /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the + /// calculation of the squared root of C. float sqrMagnitude() const; - /// - /// Connvert a vector to a length of 1 - /// - /// The vector to convert - /// The vector with length 1 - static Vector3 Normalize(const Vector3 &vector); - /// - /// Convert the vector to a length of a - /// - /// The vector with length 1 + + /// @brief Convert the vector to a length of 1 + /// @param v The vector to convert + /// @return The vector normalized to a length of 1 + static Vector3 Normalize(const Vector3& v); + /// @brief Convert the vector to a length of 1 + /// @return The vector normalized to a length of 1 Vector3 normalized() const; - /// - /// Negate the vector - /// - /// The negated vector - /// This will result in a vector pointing in the opposite direction - Vector3 operator-(); - /// - /// Subtract a vector from this vector - /// - /// The vector to subtract from this vector - /// The result of the subtraction - Vector3 operator-(const Vector3 &vector) const; + /// @brief Negate te vector such that it points in the opposite direction + /// @return The negated vector + Vector3 operator-() const; - /// - /// Add another vector to this vector - /// - /// The vector to add - /// The result of adding the vector - Vector3 operator+(const Vector3 &vector2) const; + /// @brief Subtract a vector from this vector + /// @param v The vector to subtract from this vector + /// @return The result of this subtraction + Vector3 operator-(const Vector3& v) const; + Vector3 operator-=(const Vector3& v); + /// @brief Add a vector to this vector + /// @param v The vector to add to this vector + /// @return The result of the addition + Vector3 operator+(const Vector3& v) const; + Vector3 operator+=(const Vector3& v); - /// - /// Scale a vector using another vector - /// - /// The vector to scale - /// A vector with scaling factors - /// The scaled vector - /// Each component of the vector v1 will be multiplied with the - /// component from the scaling vector v2. - static Vector3 Scale(const Vector3 &vector1, const Vector3 &vector2); - /// - /// Scale a vector uniformly up - /// - /// The scaling factor - /// The scaled vector - /// Each component of the vector will be multipled with the same factor. - Vector3 operator*(const float factor) const; - /// - /// Scale a vector uniformy down - /// - /// The scaling factor - /// The scaled vector - /// Each componet of the vector will be divided by the same factor. - Vector3 operator/(const float factor) const; + /// @brief Scale the vector using another vector + /// @param v1 The vector to scale + /// @param v2 A vector with the scaling factors + /// @return The scaled vector + /// @remark Each component of the vector v1 will be multiplied with the + /// matching component from the scaling vector v2. + static Vector3 Scale(const Vector3& v1, const Vector3& v2); + /// @brief Scale the vector uniformly up + /// @param f The scaling factor + /// @return The scaled vector + /// @remark Each component of the vector will be multipled with the same + /// factor f. + friend Vector3 operator*(const Vector3& v, float f) { + return Vector3(v.x * f, v.y * f, v.z * f); + } + friend Vector3 operator*(float f, const Vector3& v) { + // return Vector3(f * v.x, f * v.y, f * v.z); + return Vector3(v.x * f, v.y * f, v.z * f); + } + Vector3 operator*=(float f); + /// @brief Scale the vector uniformly down + /// @param f The scaling factor + /// @return The scaled vector + /// @remark Each componet of the vector will be divided by the same factor. + friend Vector3 operator/(const Vector3& v, float f) { + return Vector3(v.x / f, v.y / f, v.z / f); + } + friend Vector3 operator/(float f, const Vector3& v) { + // return Vector3(f / v.x, f / v.y, f / v.z); + return Vector3(v.x / f, v.y / f, v.z / f); + } + Vector3 operator/=(float f); - /// - /// The dot product of two vectors - /// - /// The first vector - /// The second vector - /// The dot product of the two vectors - static float Dot(const Vector3 &vector1, const Vector3 &vector2); + /// @brief The distance between two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The distance between the two vectors + static float Distance(const Vector3& v1, const Vector3& v2); - /// - /// Check is this vector is equal to the given vector - /// - /// The vector to check against - /// True if it is identical to the given vector - /// Note this uses float comparison to check equality which - /// may have strange effects. Equality on float should be avoided. - bool operator==(const Vector3 &vector); + /// @brief The dot product of two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The dot product of the two vectors + static float Dot(const Vector3& v1, const Vector3& v2); - /// - /// The distance between two vectors - /// - /// The first vector - /// The second vectors - /// The distance between the two vectors - static float Distance(const Vector3 &vector1, const Vector3 &vector2); + /// @brief The cross product of two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The cross product of the two vectors + static Vector3 Cross(const Vector3& v1, const Vector3& v2); - /// - /// The cross product of two vectors - /// - /// The first vector - /// The second vector - /// The cross product of the two vectors - static Vector3 Cross(const Vector3 &vector1, const Vector3 &vector2); - - /// - /// Project a vector on another vector - /// - /// The vector to project - /// The normal vector to project on - /// The projected vector - static Vector3 Project(const Vector3 &vector, const Vector3 &onNormal); - /// - /// Projects a vector onto a plane defined by a normal orthogonal to the + /// @brief Project the vector on another vector + /// @param v The vector to project + /// @param n The normal vecto to project on + /// @return The projected vector + static Vector3 Project(const Vector3& v, const Vector3& n); + /// @brief Project the vector on a plane defined by a normal orthogonal to the /// plane. - /// - /// The vector to project - /// The normal of the plane to project on - /// - static Vector3 ProjectOnPlane(const Vector3 &vector, - const Vector3 &planeNormal); + /// @param v The vector to project + /// @param n The normal of the plane to project on + /// @return Teh projected vector + static Vector3 ProjectOnPlane(const Vector3& v, const Vector3& n); - /// - /// Projects a vector onto the horizontal plane. - /// - /// The vector to project - /// A 2D carthesian vector with the coordinates in the horizontal - /// plane. - static Vector2 ProjectHorizontalPlane(const Vector3 &vector); + /// @brief The angle between two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The angle between the two vectors + /// @remark This reterns an unsigned angle which is the shortest distance + /// between the two vectors. Use Vector3::SignedAngle if a signed angle is + /// needed. + static AngleOf Angle(const Vector3& v1, const Vector3& v2); + /// @brief The signed angle between two vectors + /// @param v1 The starting vector + /// @param v2 The ending vector + /// @param axis The axis to rotate around + /// @return The signed angle between the two vectors + static AngleOf SignedAngle(const Vector3& v1, + const Vector3& v2, + const Vector3& axis); - /// - /// Calculate the angle between two vectors - /// - /// The first vector - /// The second vector - /// - /// This reterns an unsigned angle which is the shortest distance - /// between the two vectors. Use Vector3::SignedAngle if a - /// signed angle is needed. - static float Angle(const Vector3 &vector1, const Vector3 &vector2); - - /// - /// Calculate the angle between two vectors rotation around an axis. - /// - /// The starting vector - /// The ending vector - /// The axis to rotate around - /// The signed angle - static float SignedAngle(const Vector3 &from, const Vector3 &to, - const Vector3 &axis); - - /// - /// Lerp between two vectors - /// - /// The from vector - /// The to vector - /// The interpolation distance (0..1) - /// The lerped vector - /// The factor f is unclamped. Value 0 matches the *from* vector, Value 1 - /// matches the *to* vector Value -1 is *from* vector minus the difference - /// between *from* and *to* etc. - static Vector3 Lerp(const Vector3 &from, const Vector3 &to, float f); + /// @brief Lerp (linear interpolation) between two vectors + /// @param v1 The starting vector + /// @param v2 The ending vector + /// @param f The interpolation distance + /// @return The lerped vector + /// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value + /// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference + /// between *v1* and *v2* etc. + static Vector3 Lerp(const Vector3& v1, const Vector3& v2, float f); }; +} // namespace LinearAlgebra +using namespace LinearAlgebra; + +#include "Spherical.h" + #endif \ No newline at end of file diff --git a/float16.cpp b/float16.cpp new file mode 100644 index 0000000..041b3d3 --- /dev/null +++ b/float16.cpp @@ -0,0 +1,250 @@ +// +// FILE: float16.cpp +// AUTHOR: Rob Tillaart +// VERSION: 0.1.8 +// PURPOSE: library for Float16s for Arduino +// URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format + +#include "float16.h" +// #include +#include + +// CONSTRUCTOR +float16::float16(float f) { _value = f32tof16(f); } + +// PRINTING +// size_t float16::printTo(Print& p) const +// { +// double d = this->f16tof32(_value); +// return p.print(d, _decimals); +// } + +float float16::toFloat() const { return f16tof32(_value); } + +////////////////////////////////////////////////////////// +// +// EQUALITIES +// +bool float16::operator==(const float16 &f) { return (_value == f._value); } + +bool float16::operator!=(const float16 &f) { return (_value != f._value); } + +bool float16::operator>(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value < f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value > f._value; +} + +bool float16::operator>=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value <= f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value >= f._value; +} + +bool float16::operator<(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value > f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value < f._value; +} + +bool float16::operator<=(const float16 &f) { + if ((_value & (uint16_t)0x8000) && (f._value & (uint16_t)0x8000)) + return _value >= f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value <= f._value; +} + +////////////////////////////////////////////////////////// +// +// NEGATION +// +float16 float16::operator-() { + float16 f16; + f16.setBinary(_value ^ 0x8000); + return f16; +} + +////////////////////////////////////////////////////////// +// +// MATH +// +float16 float16::operator+(const float16 &f) { + return float16(this->toFloat() + f.toFloat()); +} + +float16 float16::operator-(const float16 &f) { + return float16(this->toFloat() - f.toFloat()); +} + +float16 float16::operator*(const float16 &f) { + return float16(this->toFloat() * f.toFloat()); +} + +float16 float16::operator/(const float16 &f) { + return float16(this->toFloat() / f.toFloat()); +} + +float16 &float16::operator+=(const float16 &f) { + *this = this->toFloat() + f.toFloat(); + return *this; +} + +float16 &float16::operator-=(const float16 &f) { + *this = this->toFloat() - f.toFloat(); + return *this; +} + +float16 &float16::operator*=(const float16 &f) { + *this = this->toFloat() * f.toFloat(); + return *this; +} + +float16 &float16::operator/=(const float16 &f) { + *this = this->toFloat() / f.toFloat(); + return *this; +} + +////////////////////////////////////////////////////////// +// +// MATH HELPER FUNCTIONS +// +int float16::sign() { + if (_value & 0x8000) + return -1; + if (_value & 0xFFFF) + return 1; + return 0; +} + +bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); } + +bool float16::isNaN() { + if ((_value & 0x7C00) != 0x7C00) + return false; + if ((_value & 0x03FF) == 0x0000) + return false; + return true; +} + +bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); } + +////////////////////////////////////////////////////////// +// +// CORE CONVERSION +// +float float16::f16tof32(uint16_t _value) const { + uint16_t sgn, man; + int exp; + float f; + + sgn = (_value & 0x8000) > 0; + exp = (_value & 0x7C00) >> 10; + man = (_value & 0x03FF); + + // ZERO + if ((_value & 0x7FFF) == 0) { + return sgn ? -0.0f : 0.0f; + } + // NAN & INF + if (exp == 0x001F) { + if (man == 0) + return sgn ? -INFINITY : INFINITY; + else + return NAN; + } + + // SUBNORMAL/NORMAL + if (exp == 0) + f = 0; + else + f = 1; + + // PROCESS MANTISSE + for (int i = 9; i >= 0; i--) { + f *= 2; + if (man & (1 << i)) + f = f + 1; + } + f = f * powf(2.0f, (float)(exp - 25)); + if (exp == 0) { + f = f * powf(2.0f, -13); // 5.96046447754e-8; + } + return sgn ? -f : f; +} + +uint16_t float16::f32tof16(float f) const { + // untested code, but will avoid strict aliasing warning + // union { + // float f; + // uint32_t t; + // } u; + // u.f = f; + // uint32_t t = u.t; + uint32_t t = *(uint32_t *)&f; + // man bits = 10; but we keep 11 for rounding + uint16_t man = (t & 0x007FFFFF) >> 12; + int16_t exp = (t & 0x7F800000) >> 23; + bool sgn = (t & 0x80000000); + + // handle 0 + if ((t & 0x7FFFFFFF) == 0) { + return sgn ? 0x8000 : 0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) { + return sgn ? 0x8000 : 0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) { + if (man) + return 0xFE00; // NAN + return sgn ? 0xFC00 : 0x7C00; // -INF : INF + } + + // normal numbers + exp = exp - 127 + 15; + // overflow does not fit => INF + if (exp > 30) { + return sgn ? 0xFC00 : 0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) { + return sgn ? 0x8000 : 0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return 0x8000 | man; + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + if (sgn) + return 0x8000 | exp | man; + return exp | man; +} + +// -- END OF FILE -- diff --git a/float16.h b/float16.h new file mode 100644 index 0000000..0a95346 --- /dev/null +++ b/float16.h @@ -0,0 +1,74 @@ +#pragma once +// +// FILE: float16.h +// AUTHOR: Rob Tillaart +// VERSION: 0.1.8 +// PURPOSE: Arduino library to implement float16 data type. +// half-precision floating point format, +// used for efficient storage and transport. +// URL: https://github.com/RobTillaart/float16 + +#include + +#define FLOAT16_LIB_VERSION (F("0.1.8")) + +// typedef uint16_t _fp16; + +class float16 { +public: + // Constructors + float16(void) { _value = 0x0000; }; + float16(float f); + float16(const float16 &f) { _value = f._value; }; + + // Conversion + float toFloat(void) const; + // access the 2 byte representation. + uint16_t getBinary() { return _value; }; + void setBinary(uint16_t u) { _value = u; }; + + // Printable + // size_t printTo(Print &p) const; + void setDecimals(uint8_t d) { _decimals = d; }; + uint8_t getDecimals() { return _decimals; }; + + // equalities + bool operator==(const float16 &f); + bool operator!=(const float16 &f); + + bool operator>(const float16 &f); + bool operator>=(const float16 &f); + bool operator<(const float16 &f); + bool operator<=(const float16 &f); + + // negation + float16 operator-(); + + // basic math + float16 operator+(const float16 &f); + float16 operator-(const float16 &f); + float16 operator*(const float16 &f); + float16 operator/(const float16 &f); + + float16 &operator+=(const float16 &f); + float16 &operator-=(const float16 &f); + float16 &operator*=(const float16 &f); + float16 &operator/=(const float16 &f); + + // math helper functions + int sign(); // 1 = positive 0 = zero -1 = negative. + bool isZero(); + bool isNaN(); + bool isInf(); + + // CORE CONVERSION + // should be private but for testing... + float f16tof32(uint16_t) const; + uint16_t f32tof16(float) const; + +private: + uint8_t _decimals = 4; + uint16_t _value; +}; + +// -- END OF FILE -- diff --git a/test/Angle16_test.cc b/test/Angle16_test.cc new file mode 100644 index 0000000..f4eca3b --- /dev/null +++ b/test/Angle16_test.cc @@ -0,0 +1,241 @@ +#if GTEST +#include "gtest/gtest.h" + +#include +#include + +#include "Angle.h" + +using namespace LinearAlgebra; + +#define FLOAT_INFINITY std::numeric_limits::infinity() + +TEST(Angle16, Construct) { + float angle = 0.0F; + Angle16 a = Angle16::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = -180.0F; + a = Angle16::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = 270.0F; + a = Angle16::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), -90); +} + +TEST(Angle16, Negate) { + float angle = 0; + Angle16 a = Angle16::Degrees(angle); + a = -a; + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = 90.0F; + a = Angle16::Degrees(angle); + a = -a; + EXPECT_FLOAT_EQ(a.InDegrees(), -angle); +} + +TEST(Angle16, Subtract) { + Angle16 a = Angle16::Degrees(0); + Angle16 b = Angle16::Degrees(45.0F); + Angle16 r = a - b; + EXPECT_FLOAT_EQ(r.InDegrees(), -45); +} + +TEST(Angle16, Add) { + Angle16 a = Angle16::Degrees(-45); + Angle16 b = Angle16::Degrees(45.0F); + Angle16 r = a + b; + EXPECT_FLOAT_EQ(r.InDegrees(), 0); +} + +TEST(Angle16, Compare) { + Angle16 a = Angle16::Degrees(45); + bool r = false; + + r = a > Angle16::Degrees(0); + EXPECT_TRUE(r) << "45 > 0"; + + r = a > Angle16::Degrees(90); + EXPECT_FALSE(r) << "45 > 90"; + + r = a > Angle16::Degrees(-90); + EXPECT_TRUE(r) << "45 > -90"; +} + +TEST(Angle16, Normalize) { + Angle16 r = Angle16(); + + r = Angle16::Normalize(Angle16::Degrees(90.0f)); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90"; + + r = Angle16::Normalize(Angle16::Degrees(-90)); + EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90"; + + r = Angle16::Normalize(Angle16::Degrees(270)); + EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270"; + + r = Angle16::Normalize(Angle16::Degrees(270 + 360)); + EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360"; + + r = Angle16::Normalize(Angle16::Degrees(-270)); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270"; + + r = Angle16::Normalize(Angle16::Degrees(-270 - 360)); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360"; + + r = Angle16::Normalize(Angle16::Degrees(0)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0"; + + if (false) { // std::numeric_limits::is_iec559) { + // Infinites are not supported + r = Angle16::Normalize(Angle16::Degrees(FLOAT_INFINITY)); + EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY"; + + r = Angle16::Normalize(Angle16::Degrees(-FLOAT_INFINITY)); + EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY"; + } +} + +TEST(Angle16, Clamp) { + Angle16 r = Angle16(); + + // Clamp(1, 0, 2) will fail because Angle16 does not have enough resolution + // for this. Instead we use Clamp(10, 0, 20) etc. + r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0), + Angle16::Degrees(20)); + EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 10 0 20"; + + r = Angle16::Clamp(Angle16::Degrees(-10), Angle16::Degrees(0), + Angle16::Degrees(20)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -10 0 20"; + + r = Angle16::Clamp(Angle16::Degrees(30), Angle16::Degrees(0), + Angle16::Degrees(20)); + EXPECT_NEAR(r.InDegrees(), 20, 1.0e-2) << "Clamp 30 0 20"; + + r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0), + Angle16::Degrees(0)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 10 0 0"; + + r = Angle16::Clamp(Angle16::Degrees(0), Angle16::Degrees(0), + Angle16::Degrees(0)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0"; + + r = Angle16::Clamp(Angle16::Degrees(0), Angle16::Degrees(10), + Angle16::Degrees(-10)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10"; + + if (false) { // std::numeric_limits::is_iec559) { + // Infinites are not supported + r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0), + Angle16::Degrees(FLOAT_INFINITY)); + EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 1 0 INFINITY"; + + r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(-FLOAT_INFINITY), + Angle16::Degrees(10)); + EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 1 -INFINITY 1"; + } +} + +// TEST(Angle16, Difference) { +// Angle16 r = 0; + +// r = Angle16::Difference(0, 90); +// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90"; + +// r = Angle16::Difference(0, -90); +// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90"; + +// r = Angle16::Difference(0, 270); +// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270"; + +// r = Angle16::Difference(0, -270); +// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270"; + +// r = Angle16::Difference(90, 0); +// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0"; + +// r = Angle16::Difference(-90, 0); +// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0"; + +// r = Angle16::Difference(0, 0); +// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0"; + +// r = Angle16::Difference(90, 90); +// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90"; + +// if (std::numeric_limits::is_iec559) { +// r = Angle16::Difference(0, INFINITY); +// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY"; + +// r = Angle16::Difference(0, -INFINITY); +// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY"; + +// r = Angle16::Difference(-INFINITY, INFINITY); +// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY +// INFINITY"; +// } +// } + +TEST(Angle16, MoveTowards) { + Angle16 r = Angle16(); + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 30); + EXPECT_NEAR(r.InDegrees(), 30, 1.0e-2) << "MoveTowards 0 90 30"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 90); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), 180); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -180"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 270); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), -30); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -30); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -90); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -180); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -270); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 0); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(0), 0); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(0), 30); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30"; + + if (false) { // std::numeric_limits::is_iec559) { + // infinites are not supported + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), + FLOAT_INFINITY); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), + Angle16::Degrees(FLOAT_INFINITY), 30); + EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), + -FLOAT_INFINITY); + EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) + << "MoveTowards 0 -90 -FLOAT_INFINITY"; + + r = Angle16::MoveTowards(Angle16::Degrees(0), + Angle16::Degrees(-FLOAT_INFINITY), -30); + EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30"; + } +} +#endif \ No newline at end of file diff --git a/test/Angle8_test.cc b/test/Angle8_test.cc new file mode 100644 index 0000000..0917f17 --- /dev/null +++ b/test/Angle8_test.cc @@ -0,0 +1,241 @@ +#if GTEST +#include + +#include +#include + +#include "Angle.h" + +using namespace LinearAlgebra; + +#define FLOAT_INFINITY std::numeric_limits::infinity() + +TEST(Angle8, Construct) { + float angle = 0.0F; + Angle8 a = Angle8::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = -180.0F; + a = Angle8::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = 270.0F; + a = Angle8::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), -90); +} + +TEST(Angle8, Negate) { + float angle = 0; + Angle8 a = Angle8::Degrees(angle); + a = -a; + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = 90.0F; + a = Angle8::Degrees(angle); + a = -a; + EXPECT_FLOAT_EQ(a.InDegrees(), -angle); +} + +TEST(Angle8, Add) { + Angle8 a = Angle8::Degrees(-45); + Angle8 b = Angle8::Degrees(45.0F); + Angle8 r = a + b; + EXPECT_FLOAT_EQ(r.InDegrees(), 0); +} + +TEST(Angle8, Subtract) { + Angle8 a = Angle8::Degrees(0); + Angle8 b = Angle8::Degrees(45.0F); + Angle8 r = a - b; + EXPECT_FLOAT_EQ(r.InDegrees(), -45); +} + +TEST(Angle8, Compare) { + Angle8 a = Angle8::Degrees(45); + bool r = false; + + r = a > Angle8::Degrees(0); + EXPECT_TRUE(r) << "45 > 0"; + + r = a > Angle8::Degrees(90); + EXPECT_FALSE(r) << "45 > 90"; + + r = a > Angle8::Degrees(-90); + EXPECT_TRUE(r) << "45 > -90"; +} + +TEST(Angle8, Normalize) { + Angle8 r = Angle8(); + + r = Angle8::Normalize(Angle8::Degrees(90.0f)); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90"; + + r = Angle8::Normalize(Angle8::Degrees(-90)); + EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90"; + + r = Angle8::Normalize(Angle8::Degrees(270)); + EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270"; + + r = Angle8::Normalize(Angle8::Degrees(270 + 360)); + EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360"; + + r = Angle8::Normalize(Angle8::Degrees(-270)); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270"; + + r = Angle8::Normalize(Angle8::Degrees(-270 - 360)); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360"; + + r = Angle8::Normalize(Angle8::Degrees(0)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0"; + + if (false) { // std::numeric_limits::is_iec559) { + // Infinites are not supported + r = Angle8::Normalize(Angle8::Degrees(FLOAT_INFINITY)); + EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY"; + + r = Angle8::Normalize(Angle8::Degrees(-FLOAT_INFINITY)); + EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY"; + } +} + +TEST(Angle8, Clamp) { + Angle8 r = Angle8(); + + // Clamp(1, 0, 2) will fail because Angle8 does not have enough resolution for + // this. Instead we use Clamp(10, 0, 20) etc. + r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0), + Angle8::Degrees(20)); + EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 10 0 20"; + + r = Angle8::Clamp(Angle8::Degrees(-10), Angle8::Degrees(0), + Angle8::Degrees(20)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -10 0 20"; + + r = Angle8::Clamp(Angle8::Degrees(30), Angle8::Degrees(0), + Angle8::Degrees(20)); + EXPECT_NEAR(r.InDegrees(), 20, 1.0e-0) << "Clamp 30 0 20"; + + r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0), + Angle8::Degrees(0)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 10 0 0"; + + r = Angle8::Clamp(Angle8::Degrees(0), Angle8::Degrees(0), Angle8::Degrees(0)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0"; + + r = Angle8::Clamp(Angle8::Degrees(0), Angle8::Degrees(10), + Angle8::Degrees(-10)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10"; + + if (false) { // std::numeric_limits::is_iec559) { + // Infinites are not supported + r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0), + Angle8::Degrees(FLOAT_INFINITY)); + EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 1 0 INFINITY"; + + r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(-FLOAT_INFINITY), + Angle8::Degrees(10)); + EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 1 -INFINITY 1"; + } +} + +// TEST(Angle8, Difference) { +// Angle8 r = 0; + +// r = Angle8::Difference(0, 90); +// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90"; + +// r = Angle8::Difference(0, -90); +// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90"; + +// r = Angle8::Difference(0, 270); +// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270"; + +// r = Angle8::Difference(0, -270); +// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270"; + +// r = Angle8::Difference(90, 0); +// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0"; + +// r = Angle8::Difference(-90, 0); +// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0"; + +// r = Angle8::Difference(0, 0); +// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0"; + +// r = Angle8::Difference(90, 90); +// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90"; + +// if (std::numeric_limits::is_iec559) { +// r = Angle8::Difference(0, INFINITY); +// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY"; + +// r = Angle8::Difference(0, -INFINITY); +// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY"; + +// r = Angle8::Difference(-INFINITY, INFINITY); +// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY +// INFINITY"; +// } +// } + +TEST(Angle8, MoveTowards) { + Angle8 r = Angle8(); + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 30); + EXPECT_NEAR(r.InDegrees(), 30, 1.0e-0) << "MoveTowards 0 90 30"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 90); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), 180); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -180"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 270); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), -30); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -30); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -90); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -180); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -270); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 0); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(0), 0); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(0), 30); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30"; + + if (false) { // std::numeric_limits::is_iec559) { + // infinites are not supported + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), + FLOAT_INFINITY); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(FLOAT_INFINITY), + 30); + EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), + -FLOAT_INFINITY); + EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) + << "MoveTowards 0 -90 -FLOAT_INFINITY"; + + r = Angle8::MoveTowards(Angle8::Degrees(0), + Angle8::Degrees(-FLOAT_INFINITY), -30); + EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30"; + } +} + +#endif \ No newline at end of file diff --git a/test/AngleSingle_test.cc b/test/AngleSingle_test.cc new file mode 100644 index 0000000..c4dab51 --- /dev/null +++ b/test/AngleSingle_test.cc @@ -0,0 +1,249 @@ +#if GTEST +#include + +#include +#include + +#include "Angle.h" + +using namespace LinearAlgebra; + +#define FLOAT_INFINITY std::numeric_limits::infinity() + +TEST(AngleSingle, Construct) { + float angle = 0.0F; + AngleSingle a = AngleSingle::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = -180.0F; + a = AngleSingle::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = 270.0F; + a = AngleSingle::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), -90); +} + +TEST(AngleSingle, Negate) { + float angle = 0; + AngleSingle a = AngleSingle::Degrees(angle); + a = -a; + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = 90.0F; + a = AngleSingle::Degrees(angle); + a = -a; + EXPECT_FLOAT_EQ(a.InDegrees(), -angle); +} + +TEST(AngleSingle, Add) { + AngleSingle a = AngleSingle::Degrees(-45); + AngleSingle b = AngleSingle::Degrees(45.0F); + AngleSingle r = a + b; + EXPECT_FLOAT_EQ(r.InDegrees(), 0); +} + +TEST(AngleSingle, Subtract) { + AngleSingle a = AngleSingle::Degrees(0); + AngleSingle b = AngleSingle::Degrees(45.0F); + AngleSingle r = a - b; + EXPECT_FLOAT_EQ(r.InDegrees(), -45); +} + +TEST(AngleSingle, Compare) { + AngleSingle a = AngleSingle::Degrees(45); + bool r = false; + + r = a > AngleSingle::Degrees(0); + EXPECT_TRUE(r) << "45 > 0"; + + r = a > AngleSingle::Degrees(90); + EXPECT_FALSE(r) << "45 > 90"; + + r = a > AngleSingle::Degrees(-90); + EXPECT_TRUE(r) << "45 > -90"; +} + +TEST(AngleSingle, Normalize) { + AngleSingle r = AngleSingle(); + + r = AngleSingle::Normalize(AngleSingle::Degrees(90.0f)); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90"; + + r = AngleSingle::Normalize(AngleSingle::Degrees(-90)); + EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90"; + + r = AngleSingle::Normalize(AngleSingle::Degrees(270)); + EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270"; + + r = AngleSingle::Normalize(AngleSingle::Degrees(270 + 360)); + EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360"; + + r = AngleSingle::Normalize(AngleSingle::Degrees(-270)); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270"; + + r = AngleSingle::Normalize(AngleSingle::Degrees(-270 - 360)); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360"; + + r = AngleSingle::Normalize(AngleSingle::Degrees(0)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0"; + + if (std::numeric_limits::is_iec559) { + r = AngleSingle::Normalize(AngleSingle::Degrees(FLOAT_INFINITY)); + EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY"; + + r = AngleSingle::Normalize(AngleSingle::Degrees(-FLOAT_INFINITY)); + EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY"; + } +} + +TEST(AngleSingle, Clamp) { + AngleSingle r = AngleSingle(); + + r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0), + AngleSingle::Degrees(2)); + EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 2"; + + r = AngleSingle::Clamp(AngleSingle::Degrees(-1), AngleSingle::Degrees(0), + AngleSingle::Degrees(2)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -1 0 2"; + + r = AngleSingle::Clamp(AngleSingle::Degrees(3), AngleSingle::Degrees(0), + AngleSingle::Degrees(2)); + EXPECT_FLOAT_EQ(r.InDegrees(), 2) << "Clamp 3 0 2"; + + r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0), + AngleSingle::Degrees(0)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 1 0 0"; + + r = AngleSingle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(0), + AngleSingle::Degrees(0)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0"; + + r = AngleSingle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(1), + AngleSingle::Degrees(-1)); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 1 -1"; + + if (std::numeric_limits::is_iec559) { + r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0), + AngleSingle::Degrees(FLOAT_INFINITY)); + EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 INFINITY"; + + r = AngleSingle::Clamp(AngleSingle::Degrees(1), + AngleSingle::Degrees(-FLOAT_INFINITY), + AngleSingle::Degrees(1)); + EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 -INFINITY 1"; + } +} + +// TEST(AngleSingle, Difference) { +// AngleSingle r = 0; + +// r = AngleSingle::Difference(0, 90); +// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90"; + +// r = AngleSingle::Difference(0, -90); +// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90"; + +// r = AngleSingle::Difference(0, 270); +// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270"; + +// r = AngleSingle::Difference(0, -270); +// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270"; + +// r = AngleSingle::Difference(90, 0); +// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0"; + +// r = AngleSingle::Difference(-90, 0); +// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0"; + +// r = AngleSingle::Difference(0, 0); +// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0"; + +// r = AngleSingle::Difference(90, 90); +// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90"; + +// if (std::numeric_limits::is_iec559) { +// r = AngleSingle::Difference(0, INFINITY); +// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY"; + +// r = AngleSingle::Difference(0, -INFINITY); +// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY"; + +// r = AngleSingle::Difference(-INFINITY, INFINITY); +// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY +// INFINITY"; +// } +// } + +TEST(AngleSingle, MoveTowards) { + AngleSingle r = AngleSingle(); + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(90), 30); + EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 90 30"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(90), 90); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(90), 180); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 180"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(90), 270); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(90), -30); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(-90), -30); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(-90), -90); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(-90), -180); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(-90), -270); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(90), 0); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), AngleSingle::Degrees(0), + 0); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), AngleSingle::Degrees(0), + 30); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30"; + + if (std::numeric_limits::is_iec559) { + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(90), FLOAT_INFINITY); + EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(FLOAT_INFINITY), 30); + EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(-90), -FLOAT_INFINITY); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -FLOAT_INFINITY"; + + r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), + AngleSingle::Degrees(-FLOAT_INFINITY), -30); + EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -FLOAT_INFINITY -30"; + } +} + +#endif \ No newline at end of file diff --git a/test/Angle_test.cc b/test/Angle_test.cc deleted file mode 100644 index 77ec3b6..0000000 --- a/test/Angle_test.cc +++ /dev/null @@ -1,167 +0,0 @@ -#if GTEST -#include - -#include -#include - -#include "Angle.h" - -#define FLOAT_INFINITY std::numeric_limits::infinity() - -TEST(Angle, Normalize) { - float r = 0; - - r = Angle::Normalize(90); - EXPECT_FLOAT_EQ(r, 90) << "Normalize 90"; - - r = Angle::Normalize(-90); - EXPECT_FLOAT_EQ(r, -90) << "Normalize -90"; - - r = Angle::Normalize(270); - EXPECT_FLOAT_EQ(r, -90) << "Normalize 270"; - - r = Angle::Normalize(270+360); - EXPECT_FLOAT_EQ(r, -90) << "Normalize 270+360"; - - r = Angle::Normalize(-270); - EXPECT_FLOAT_EQ(r, 90) << "Normalize -270"; - - r = Angle::Normalize(-270 - 360); - EXPECT_FLOAT_EQ(r, 90) << "Normalize -270-360"; - - r = Angle::Normalize(0); - EXPECT_FLOAT_EQ(r, 0) << "Normalize 0"; - - if (std::numeric_limits::is_iec559) { - r = Angle::Normalize(FLOAT_INFINITY); - EXPECT_FLOAT_EQ(r, FLOAT_INFINITY) << "Normalize INFINITY"; - - r = Angle::Normalize(-FLOAT_INFINITY); - EXPECT_FLOAT_EQ(r, -FLOAT_INFINITY) << "Normalize INFINITY"; - } -} - -TEST(Angle, Clamp) { - float r = 0; - - r = Angle::Clamp(1, 0, 2); - EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 2"; - - r = Angle::Clamp(-1, 0, 2); - EXPECT_FLOAT_EQ(r, 0) << "Clamp -1 0 2"; - - r = Angle::Clamp(3, 0, 2); - EXPECT_FLOAT_EQ(r, 2) << "Clamp 3 0 2"; - - r = Angle::Clamp(1, 0, 0); - EXPECT_FLOAT_EQ(r, 0) << "Clamp 1 0 0"; - - r = Angle::Clamp(0, 0, 0); - EXPECT_FLOAT_EQ(r, 0) << "Clamp 0 0 0"; - - r = Angle::Clamp(0, 1, -1); - EXPECT_FLOAT_EQ(r, 1) << "Clamp 0 1 -1"; - - if (std::numeric_limits::is_iec559) { - r = Angle::Clamp(1, 0, FLOAT_INFINITY); - EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 INFINITY"; - - r = Angle::Clamp(1, -FLOAT_INFINITY, 1); - EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 -INFINITY 1"; - } -} - -TEST(Angle, Difference) { - float r = 0; - - r = Angle::Difference(0, 90); - EXPECT_FLOAT_EQ(r, 90) << "Difference 0 90"; - - r = Angle::Difference(0, -90); - EXPECT_FLOAT_EQ(r, -90) << "Difference 0 -90"; - - r = Angle::Difference(0, 270); - EXPECT_FLOAT_EQ(r, -90) << "Difference 0 270"; - - r = Angle::Difference(0, -270); - EXPECT_FLOAT_EQ(r, 90) << "Difference 0 -270"; - - r = Angle::Difference(90, 0); - EXPECT_FLOAT_EQ(r, -90) << "Difference 90 0"; - - r = Angle::Difference(-90, 0); - EXPECT_FLOAT_EQ(r, 90) << "Difference -90 0"; - - r = Angle::Difference(0, 0); - EXPECT_FLOAT_EQ(r, 0) << "Difference 0 0"; - - r = Angle::Difference(90, 90); - EXPECT_FLOAT_EQ(r, 0) << "Difference 90 90"; - - if (std::numeric_limits::is_iec559) { - r = Angle::Difference(0, INFINITY); - EXPECT_FLOAT_EQ(r, INFINITY) << "Difference 0 INFINITY"; - - r = Angle::Difference(0, -INFINITY); - EXPECT_FLOAT_EQ(r, -INFINITY) << "Difference 0 -INFINITY"; - - r = Angle::Difference(-INFINITY, INFINITY); - EXPECT_FLOAT_EQ(r, INFINITY) << "Difference -INFINITY INFINITY"; - } -} - -TEST(Angle, MoveTowards) { - float r = 0; - - r = Angle::MoveTowards(0, 90, 30); - EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 90 30"; - - r = Angle::MoveTowards(0, 90, 90); - EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 90"; - - r = Angle::MoveTowards(0, 90, 180); - EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 180"; - - r = Angle::MoveTowards(0, 90, 270); - EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 270"; - - r = Angle::MoveTowards(0, 90, -30); - EXPECT_FLOAT_EQ(r, -30) << "MoveTowards 0 90 -30"; - - r = Angle::MoveTowards(0, -90, -30); - EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 -90 -30"; - - r = Angle::MoveTowards(0, -90, -90); - EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 -90 -90"; - - r = Angle::MoveTowards(0, -90, -180); - EXPECT_FLOAT_EQ(r, 180) << "MoveTowards 0 -90 -180"; - - r = Angle::MoveTowards(0, -90, -270); - EXPECT_FLOAT_EQ(r, 270) << "MoveTowards 0 -90 -270"; - - r = Angle::MoveTowards(0, 90, 0); - EXPECT_FLOAT_EQ(r, 0) << "MoveTowards 0 90 0"; - - r = Angle::MoveTowards(0, 0, 0); - EXPECT_FLOAT_EQ(r, 0) << "MoveTowards 0 0 0"; - - r = Angle::MoveTowards(0, 0, 30); - EXPECT_FLOAT_EQ(r, 0) << "MoveTowards 0 0 30"; - - if (std::numeric_limits::is_iec559) { - r = Angle::MoveTowards(0, 90, FLOAT_INFINITY); - EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 FLOAT_INFINITY"; - - r = Angle::MoveTowards(0, FLOAT_INFINITY, 30); - EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 FLOAT_INFINITY 30"; - - r = Angle::MoveTowards(0, -90, -FLOAT_INFINITY); - EXPECT_FLOAT_EQ(r, FLOAT_INFINITY) << "MoveTowards 0 -90 -FLOAT_INFINITY"; - - r = Angle::MoveTowards(0, -FLOAT_INFINITY, -30); - EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 -FLOAT_INFINITY -30"; - } -} - -#endif \ No newline at end of file diff --git a/test/Direction_test.cc b/test/Direction_test.cc new file mode 100644 index 0000000..6489caa --- /dev/null +++ b/test/Direction_test.cc @@ -0,0 +1,58 @@ +#if GTEST +#include + +#include +#include + +#include "Direction.h" + +using namespace LinearAlgebra; + +#define FLOAT_INFINITY std::numeric_limits::infinity() + +TEST(Direction16, Compare) { + Direction16 d = Direction16::Degrees(45, 135); + bool r; + r = (d == Direction16(Angle16::Degrees(45), Angle16::Degrees(135))); + EXPECT_TRUE(r) << "45,135 == 45, 135"; + + r = (d == + Direction16(Angle16::Degrees(45 + 360), Angle16::Degrees(135 - 360))); + EXPECT_TRUE(r) << "45+360, 135-360 == 45, 135"; +} + +TEST(Direction16, Inverse) { + Direction16 d; + Direction16 r; + + d = Direction16::Degrees(45, 135); + r = -d; + EXPECT_EQ(r, Direction16::Degrees(-135, -135)) << "-(45, 135)"; + + d = Direction16::Degrees(-45, -135); + r = -d; + EXPECT_EQ(r, Direction16::Degrees(135, 135)) << "-(-45, -135)"; + + d = Direction16::Degrees(0, 0); + r = -d; + EXPECT_EQ(r, Direction16::Degrees(180, 0)) << "-(0, 0)"; + + d = Direction16::Degrees(0, 45); + r = -d; + EXPECT_EQ(r, Direction16::Degrees(180, -45)) << "-(0, 45)"; +} + +TEST(Direction16, Equality) { + Direction16 d; + d = Direction16::Degrees(135, 45); + EXPECT_EQ(d, Direction16::Degrees(135, 45)) << "(135, 45) == (135, 45)"; + EXPECT_EQ(d, Direction16::Degrees(135 + 360, 45)) + << "(135, 45) == (135 + 360, 45) "; + EXPECT_EQ(d, Direction16::Degrees(135 - 360, 45)) + << "(135, 135) == (135 - 360, 45) "; + + d = Direction16::Degrees(0, 45 + 180); + EXPECT_EQ(d, Direction16::Degrees(180, -45)) << "(0, 45+180) == (180, -45)"; +} + +#endif \ No newline at end of file diff --git a/test/DiscreteAngle_test.cc b/test/DiscreteAngle_test.cc new file mode 100644 index 0000000..91a7cc8 --- /dev/null +++ b/test/DiscreteAngle_test.cc @@ -0,0 +1,82 @@ +/* +#if GTEST +#include + +#include +#include + +#include "Angle.h" +// #include "Angle16.h" +// #include "Angle8.h" + +#define FLOAT_INFINITY std::numeric_limits::infinity() + +TEST(Angle8, Construct) { + float angle = 0.0F; + Angle8 a = Angle8::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = -180.0F; + a = Angle8::Degrees(angle); + EXPECT_FLOAT_EQ(a.InDegrees(), angle); +} + +TEST(Angle8, Negate) { + float angle = 0; + Angle8 a = Angle8::Degrees(angle); + a = -a; + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = 90.0F; + a = Angle8::Degrees(angle); + a = -a; + EXPECT_FLOAT_EQ(a.InDegrees(), -angle); +} + +TEST(Angle8, Add) { + Angle8 a = Angle8::Degrees(-45); + Angle8 b = Angle8::Degrees(45.0F); + Angle8 r = a + b; + EXPECT_FLOAT_EQ(r.InDegrees(), 0); +} + +TEST(Angle8, Subtract) { + Angle8 a = Angle8::Degrees(0); + Angle8 b = Angle8::Degrees(45.0F); + Angle8 r = a - b; + EXPECT_FLOAT_EQ(r.InDegrees(), -45); +} + +TEST(Angle16, Construct) { + Angle16 a = Angle16::Degrees(0.0F); + EXPECT_FLOAT_EQ(a.InDegrees(), 0); +} + +TEST(Angle16, Negate) { + float angle = 0; + Angle16 a = Angle16::Degrees(angle); + a = -a; + EXPECT_FLOAT_EQ(a.InDegrees(), angle); + + angle = 90.0F; + a = Angle16::Degrees(angle); + a = -a; + EXPECT_FLOAT_EQ(a.InDegrees(), -angle); +} + +TEST(Angle16, Subtract) { + Angle16 a = Angle16::Degrees(0); + Angle16 b = Angle16::Degrees(45.0F); + Angle16 r = a - b; + EXPECT_FLOAT_EQ(r.InDegrees(), -45); +} + +TEST(Angle16, Add) { + Angle16 a = Angle16::Degrees(-45); + Angle16 b = Angle16::Degrees(45.0F); + Angle16 r = a + b; + EXPECT_FLOAT_EQ(r.InDegrees(), 0); +} + +#endif +*/ \ No newline at end of file diff --git a/test/FloatSingle_test.cc b/test/FloatSingle_test.cc index 3083985..9673ade 100644 --- a/test/FloatSingle_test.cc +++ b/test/FloatSingle_test.cc @@ -1,41 +1,41 @@ #if GTEST #include -#include #include +#include #include "FloatSingle.h" #define FLOAT_INFINITY std::numeric_limits::infinity() TEST(FloatC, Clamp) { - float r = 0; + float r = 0; - r = Float::Clamp(1, 0, 2); - EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 2"; + r = Float::Clamp(1, 0, 2); + EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 2"; - r = Float::Clamp(-1, 0, 2); - EXPECT_FLOAT_EQ(r, 0) << "Clamp -1 0 2"; + r = Float::Clamp(-1, 0, 2); + EXPECT_FLOAT_EQ(r, 0) << "Clamp -1 0 2"; - r = Float::Clamp(3, 0, 2); - EXPECT_FLOAT_EQ(r, 2) << "Clamp 3 0 2"; + r = Float::Clamp(3, 0, 2); + EXPECT_FLOAT_EQ(r, 2) << "Clamp 3 0 2"; - r = Float::Clamp(1, 0, 0); - EXPECT_FLOAT_EQ(r, 0) << "Clamp 1 0 0"; + r = Float::Clamp(1, 0, 0); + EXPECT_FLOAT_EQ(r, 0) << "Clamp 1 0 0"; - r = Float::Clamp(0, 0, 0); - EXPECT_FLOAT_EQ(r, 0) << "Clamp 0 0 0"; + r = Float::Clamp(0, 0, 0); + EXPECT_FLOAT_EQ(r, 0) << "Clamp 0 0 0"; - r = Float::Clamp(0, 1, -1); - EXPECT_FLOAT_EQ(r, 1) << "Clamp 0 1 -1"; + r = Float::Clamp(0, 1, -1); + EXPECT_FLOAT_EQ(r, 0) << "Clamp 0 1 -1"; - if (std::numeric_limits::is_iec559) { - r = Float::Clamp(1, 0, FLOAT_INFINITY); - EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 INFINITY"; + if (std::numeric_limits::is_iec559) { + r = Float::Clamp(1, 0, FLOAT_INFINITY); + EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 INFINITY"; - r = Float::Clamp(1, -FLOAT_INFINITY, 1); - EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 -INFINITY 1"; - } + r = Float::Clamp(1, -FLOAT_INFINITY, 1); + EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 -INFINITY 1"; + } } #endif \ No newline at end of file diff --git a/test/Matrix_test.cc b/test/Matrix_test.cc index dd358f9..4cb66b8 100644 --- a/test/Matrix_test.cc +++ b/test/Matrix_test.cc @@ -1,10 +1,90 @@ #if GTEST #include -#include #include +#include #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 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 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); + + 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"; + + + // 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; + + 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; + + 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; + + 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); @@ -18,7 +98,8 @@ TEST(MatrixSingle, Init) { MatrixOf m2 = MatrixOf(2, 2, data2); // negative - MatrixOf m_1 = MatrixOf(-1, -1); + // MatrixOf m_1 = MatrixOf(-1, -1); + // parameters are unsigned } TEST(MatrixSingle, Transpose) { diff --git a/test/Polar_test.cc b/test/Polar_test.cc new file mode 100644 index 0000000..ba65946 --- /dev/null +++ b/test/Polar_test.cc @@ -0,0 +1,233 @@ +#if GTEST +#include +#include +#include +#include + +#include "Polar.h" +#include "Spherical.h" + +#define FLOAT_INFINITY std::numeric_limits::infinity() + +TEST(Polar, FromVector2) { + Vector2 v = Vector2(0, 1); + PolarSingle p = PolarSingle::FromVector2(v); + + EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance 0 1"; + EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "s.angle 0 0 1"; + + v = Vector2(1, 0); + p = PolarSingle::FromVector2(v); + + EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance 1 0"; + EXPECT_FLOAT_EQ(p.angle.InDegrees(), 90.0F) << "s.angle 1 0"; + + v = Vector2(-1, 1); + p = PolarSingle::FromVector2(v); + + EXPECT_FLOAT_EQ(p.distance, sqrt(2.0F)) << "p.distance -1 1"; + EXPECT_NEAR(p.angle.InDegrees(), -45.0F, 1.0e-05) << "s.angle -1 1"; +} + +TEST(Polar, FromSpherical) { + SphericalSingle s; + PolarSingle p; + + s = SphericalSingle(1, DirectionSingle::forward); + p = PolarSingle::FromSpherical(s); + + EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 0 0)"; + EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(1 0 0)"; + + s = SphericalSingle(1, AngleSingle::Degrees(45), AngleSingle::Degrees(0)); + p = PolarSingle::FromSpherical(s); + + EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 45 0)"; + EXPECT_FLOAT_EQ(p.angle.InDegrees(), 45.0F) + << "p.angle FromSpherical(1 45 0)"; + + s = SphericalSingle(1, AngleSingle::Degrees(-45), AngleSingle::Degrees(0)); + p = PolarSingle::FromSpherical(s); + + EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 -45 0)"; + EXPECT_FLOAT_EQ(p.angle.InDegrees(), -45.0F) + << "p.angle FromSpherical(1 -45 0)"; + + s = SphericalSingle(0, AngleSingle::Degrees(0), AngleSingle::Degrees(0)); + p = PolarSingle::FromSpherical(s); + + EXPECT_FLOAT_EQ(p.distance, 0.0F) << "p.distance FromSpherical(0 0 0)"; + EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(0 0 0)"; + + s = SphericalSingle(-1, AngleSingle::Degrees(0), AngleSingle::Degrees(0)); + p = PolarSingle::FromSpherical(s); + + EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(-1 0 0)"; + EXPECT_FLOAT_EQ(p.angle.InDegrees(), -180.0F) + << "p.angle FromSpherical(-1 0 0)"; + + s = SphericalSingle(0, AngleSingle::Degrees(0), AngleSingle::Degrees(90)); + p = PolarSingle::FromSpherical(s); + + EXPECT_FLOAT_EQ(p.distance, 0.0F) << "p.distance FromSpherical(0 0 90)"; + EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(0 0 90)"; +} + +TEST(Polar, Negation) { + PolarSingle v = PolarSingle(2, AngleSingle::Degrees(45)); + PolarSingle r = PolarSingle::zero; + + r = -v; + EXPECT_FLOAT_EQ(r.distance, 2); + EXPECT_FLOAT_EQ(r.angle.InDegrees(), -135); + EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(-135))) + << "Negate(2 45)"; + + v = PolarSingle::Deg(2, -45); + r = -v; + EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(135))) + << "Negate(2 -45)"; + + v = PolarSingle::Degrees(2, 0); + r = -v; + EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(180))) << "Negate(2 0)"; + + v = PolarSingle(0, AngleSingle::Degrees(0)); + r = -v; + EXPECT_FLOAT_EQ(r.distance, 0.0f); + EXPECT_FLOAT_EQ(r.angle.InDegrees(), 0.0f); + EXPECT_TRUE(r == PolarSingle(0, AngleSingle::Degrees(0))) << "Negate(0 0)"; +} + +TEST(Polar, Subtraction) { + PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45)); + PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90)); + PolarSingle r = PolarSingle::zero; + + r = v1 - v2; + // don't know what to expect yet + + v2 = PolarSingle::zero; + r = v1 - v2; + EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Subtraction(0 0)"; +} + +TEST(Polar, Addition) { + PolarSingle v1 = PolarSingle(1, AngleSingle::Degrees(45)); + PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90)); + PolarSingle r = PolarSingle::zero; + + r = v1 - v2; + // don't know what to expect yet + + v2 = PolarSingle::zero; + r = v1 + v2; + EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)"; + + r = v1; + r += v2; + EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)"; + + v2 = PolarSingle(1, AngleSingle::Degrees(-45)); + r = v1 + v2; + EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(0 0 0)"; + EXPECT_FLOAT_EQ(r.angle.InDegrees(), 0) << "Addition(0 0 0)"; +} + +TEST(Polar, Scale_Multiply) { + PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45)); + PolarSingle r = PolarSingle::zero; + + r = v1 * 2.0f; + EXPECT_FLOAT_EQ(r.distance, v1.distance * 2) << "ScaleMult(4 45, 2)"; + EXPECT_FLOAT_EQ(r.angle.InDegrees(), v1.angle.InDegrees()) + << "ScaleMult(4 45, 2)"; +} + +TEST(Polar, Scale_Divide) { + PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45)); + PolarSingle r = PolarSingle::zero; + + r = v1 / 2.0f; + EXPECT_FLOAT_EQ(r.distance, v1.distance / 2) << "ScaleDiv(4 45, 2)"; + EXPECT_FLOAT_EQ(r.angle.InDegrees(), v1.angle.InDegrees()) + << "ScaleDiv(4 45, 2)"; +} + +TEST(Polar, Distance) { + PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45)); + PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90)); + float d = 0; + + d = PolarSingle::Distance(v1, v2); + // don't know what to expect yet + + v2 = PolarSingle::zero; + d = PolarSingle::Distance(v1, v2); + EXPECT_FLOAT_EQ(d, v1.distance) << "Distance(4 45, zero)"; +} + +TEST(Polar, Rotate) { + PolarSingle v = PolarSingle(4, AngleSingle::Degrees(45)); + PolarSingle r = PolarSingle::zero; + + r = PolarSingle::Rotate(v, AngleSingle::Degrees(45)); + EXPECT_FLOAT_EQ(r.distance, v.distance) << "Rotate(4 45, 45)"; + EXPECT_FLOAT_EQ(r.angle.InDegrees(), 90.0f) << "Rotate(4 45, 45)"; +} + +// Performance Test +TEST(PolarOfTest, PerformanceTest) { + const int numIterations = 1000000; // Number of instances to test + std::vector> polarObjects; + + // Measure time for creating a large number of PolarOf objects + auto start = std::chrono::high_resolution_clock::now(); + + for (int i = 0; i < numIterations; ++i) { + float distance = + static_cast(rand() % 100); // Random distance from 0 to 100 + AngleOf angle = AngleOf::Degrees( + static_cast(rand() % 360)); // Random angle from 0 to 360 degrees + PolarOf p = PolarOf(distance, angle); + polarObjects.emplace_back(p); // Create and store the object + } + + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration duration = end - start; + std::cout << "Time to construct " << numIterations + << " PolarOf objects: " << duration.count() << " seconds." + << std::endl; + + // Test completion with a message + ASSERT_GE(duration.count(), 0); // Ensure duration is non-negative + + // Assert that the duration is less than or equal to 1 second + ASSERT_LE(duration.count(), 1.0) + << "Performance test failed: Construction took longer than 1 second."; +} + +// Edge Case 1: Testing with distance = 0 and angle = 45 +TEST(PolarOfTest, TestDistanceZero) { + PolarOf p1(0.0f, AngleOf::Degrees(45.0f)); + EXPECT_EQ(p1.distance, 0.0f); // Ensure distance is 0 + EXPECT_EQ(p1.angle.InDegrees(), 0.0f); // Ensure angle is 0 when distance is 0 +} + +// Edge Case 2: Testing with negative distance, angle should be adjusted +TEST(PolarOfTest, TestNegativeDistance) { + PolarOf p2(-10.0f, AngleOf::Degrees(90.0f)); + EXPECT_EQ(p2.distance, 10.0f); // Ensure distance is positive + EXPECT_NEAR(p2.angle.InDegrees(), -90.0f, + 0.0001f); // Ensure angle is normalized to 270 degrees (180 + 90) +} + +// Edge Case 3: Testing with positive distance and angle = 180 +TEST(PolarOfTest, TestPositiveDistance) { + PolarOf p3(100.0f, AngleOf::Degrees(180.0f)); + EXPECT_EQ(p3.distance, 100.0f); // Ensure distance is correct + EXPECT_NEAR(p3.angle.InDegrees(), -180.0f, + 0.0001f); // Ensure angle is correct +} + +#endif \ No newline at end of file diff --git a/test/Quaternion_test.cc b/test/Quaternion_test.cc index 84cefcf..e71bce4 100644 --- a/test/Quaternion_test.cc +++ b/test/Quaternion_test.cc @@ -36,7 +36,8 @@ TEST(Quaternion, ToAngles) { q1 = Quaternion(1, 0, 0, 0); v = Quaternion::ToAngles(q1); r = v == Vector3(180, 0, 0); - EXPECT_TRUE(r) << "Quaternion::ToAngles 1 0 0 0"; + // EXPECT_TRUE(r) << "Quaternion::ToAngles 1 0 0 0"; + // fails on MacOS? } TEST(Quaternion, Multiplication) { diff --git a/test/Spherical16_test.cc b/test/Spherical16_test.cc new file mode 100644 index 0000000..8eea700 --- /dev/null +++ b/test/Spherical16_test.cc @@ -0,0 +1,223 @@ +#if GTEST +#include +#include +#include +#include + +#include "Spherical.h" +#include "Vector3.h" + +#define FLOAT_INFINITY std::numeric_limits::infinity() + +TEST(Spherical16, FromVector3) { + Vector3 v = Vector3(0, 0, 1); + Spherical16 s = Spherical16::FromVector3(v); + + EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1"; + EXPECT_FLOAT_EQ((float)s.direction.horizontal.InDegrees(), 0.0F) + << "s.hor 0 0 1"; + EXPECT_FLOAT_EQ((float)s.direction.vertical.InDegrees(), 0.0F) + << "s.vert 0 0 1"; + + v = Vector3(0, 1, 0); + s = Spherical16::FromVector3(v); + + EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0"; + EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0"; + EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0"; + + v = Vector3(1, 0, 0); + s = Spherical16::FromVector3(v); + + EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0"; + EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0"; + EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0"; +} + +TEST(Spherical16, Vector3) { + Vector3 v = Vector3(1, 2, 3); + Spherical16 rd = Spherical16::FromVector3(v); + Vector3 rv = rd.ToVector3(); + EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical"; + + v = Vector3(1, 2, -3); + rd = Spherical16::FromVector3(v); + rv = rd.ToVector3(); + EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical"; +} + +// TEST(Spherical16, FromPolar) { +// Polar p = Polar(1, 0); +// Spherical16 s = Spherical16::FromPolar(p); + +// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)"; +// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 0.0F) << "s.hor Polar(1 0)"; +// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 0)"; + +// p = Polar(1, 45); +// s = Spherical16::FromPolar(p); + +// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)"; +// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 45.0F) << "s.hor Polar(1 45)"; +// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 45)"; + +// p = Polar(1, -45); +// s = Spherical16::FromPolar(p); + +// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)"; +// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), -45.0F) << "s.hor Polar(1 -45)"; +// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 -45)"; + +// p = Polar(0, 0); +// s = Spherical16::FromPolar(p); + +// EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)"; +// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 0.0F) << "s.hor Polar(0 0)"; +// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(0 0)"; + +// p = Polar(-1, 0); +// s = Spherical16::FromPolar(p); + +// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)"; +// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), -180.0F) << "s.hor Polar(-1 0)"; +// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(-1 0)"; +// } + +TEST(Spherical16, Incident1) { + Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f); + Spherical16 s = Spherical16::FromVector3(v); + + Spherical16 sr = + Spherical16(2.49F, Angle16::Degrees(98.18f), Angle16::Degrees(24.4F)); + EXPECT_NEAR(s.distance, sr.distance, 1.0e-01); + EXPECT_NEAR(s.direction.horizontal.InDegrees(), + sr.direction.horizontal.InDegrees(), 1.0e-02); + EXPECT_NEAR(s.direction.vertical.InDegrees(), + sr.direction.vertical.InDegrees(), 1.0e-02); + + Vector3 r = + Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical) + .ToVector3(); + EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0"; + EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0"; + EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0"; +} + +TEST(Spherical16, Incident2) { + Vector3 v = Vector3(1.0f, 0.0f, 1.0f); + Spherical16 s = Spherical16::FromVector3(v); + + Spherical16 sr = Spherical16(1.4142135623F, Angle16::Degrees(45.0f), + Angle16::Degrees(0.0F)); + EXPECT_NEAR(s.distance, sr.distance, 1.0e-05); + EXPECT_NEAR(s.direction.horizontal.InDegrees(), + sr.direction.horizontal.InDegrees(), 1.0e-05); + EXPECT_NEAR(s.direction.vertical.InDegrees(), + sr.direction.vertical.InDegrees(), 1.0e-05); + + Vector3 r = + Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical) + .ToVector3(); + EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); + EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06); + EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06); + + v = Vector3(0.0f, 1.0f, 1.0f); + s = Spherical16::FromVector3(v); + + sr = Spherical16(1.4142135623F, Angle16::Degrees(0), Angle16::Degrees(45)); + EXPECT_NEAR(s.distance, sr.distance, 1.0e-05); + EXPECT_NEAR(s.direction.horizontal.InDegrees(), + sr.direction.horizontal.InDegrees(), 1.0e-05); + EXPECT_NEAR(s.direction.vertical.InDegrees(), + sr.direction.vertical.InDegrees(), 1.0e-05); + + r = Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical) + .ToVector3(); + EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); + EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06); + EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06); + + v = Vector3(1.0f, 1.0f, 1.0f); + s = Spherical16::FromVector3(v); + r = Spherical16(s.distance, s.direction.horizontal, s.direction.vertical) + .ToVector3(); + + EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02); + EXPECT_NEAR(s.direction.horizontal.InDegrees(), 45.0F, 1.0e-02); + EXPECT_NEAR(s.direction.vertical.InDegrees(), 35.26F, 1.0e-02); + + EXPECT_NEAR(r.Right(), v.Right(), 1.0e-04); + EXPECT_NEAR(r.Up(), v.Up(), 1.0e-04); + EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-04); + + // s = Spherical16(10, 45, 45); + // r = s.ToVector3(); + // EXPECT_NEAR(r.x, 5, 1.0e-06); + // EXPECT_NEAR(r.y, 7.07, 1.0e-06); + // EXPECT_NEAR(r.z, 5, 1.0e-06); +} + +TEST(Spherical16, Addition) { + Spherical16 v1 = Spherical16(1, Angle16::Degrees(45), Angle16::Degrees(0)); + Spherical16 v2 = Spherical16::zero; + Spherical16 r = Spherical16::zero; + + r = v1 + v2; + EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)"; + + r = v1; + r += v2; + EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)"; + + v2 = Spherical16(1, Angle16::Degrees(-45), Angle16::Degrees(0)); + r = v1 + v2; + EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)"; + EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)"; + EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)"; + + v2 = Spherical16(1, Angle16::Degrees(0), Angle16::Degrees(90)); + r = v1 + v2; + EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)"; + EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)"; + EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 45) << "Addition(1 0 90)"; +} + +TEST(Spherical16, AdditionPerformance) { + const int numIterations = 1000000; // Number of additions to test + std::vector sphericalObjects; + + // Populate the vector with random SphericalOf objects + for (int i = 0; i < numIterations; ++i) { + float distance = (float)(rand() % 100); + float horizontal = (float)(rand() % 180); + float vertical = (float)(rand() % 360); + Spherical16 s = Spherical16::Deg(distance, horizontal, vertical); + sphericalObjects.push_back(s); + } + + // Measure the time to perform multiple additions + auto start = std::chrono::high_resolution_clock::now(); + + Spherical16 result = Spherical16::zero; // Start with a + // zero-initialized object + + for (int i = 0; i < numIterations - 1; ++i) { + result = result + sphericalObjects[i]; // Add objects + // together + } + + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration duration = end - start; + std::cout << "Time to perform " << numIterations - 1 + << " additions: " << duration.count() << " seconds." << std::endl; + + // Assert that the time taken is less than + // 1 second (or any other performance + // requirement) + ASSERT_LE(duration.count(), 2.0) << "Performance test failed: " + "Additions took longer than 1 " + "second."; +} + +#endif \ No newline at end of file diff --git a/test/SphericalSingle_test.cc b/test/SphericalSingle_test.cc new file mode 100644 index 0000000..3dedb31 --- /dev/null +++ b/test/SphericalSingle_test.cc @@ -0,0 +1,214 @@ +#if GTEST +#include +#include +#include +#include + +#include "Spherical.h" + +#define FLOAT_INFINITY std::numeric_limits::infinity() + +TEST(SphericalSingle, FromVector3) { + Vector3 v = Vector3(0, 0, 1); + SphericalSingle s = SphericalSingle ::FromVector3(v); + + EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1"; + EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 0 1"; + EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 0 0 1"; + + v = Vector3(0, 1, 0); + s = SphericalSingle ::FromVector3(v); + + EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0"; + EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0"; + EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0"; + + v = Vector3(1, 0, 0); + s = SphericalSingle ::FromVector3(v); + + EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0"; + EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0"; + EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0"; +} + +TEST(SphericalSingle, FromPolar) { + PolarSingle p = PolarSingle(1, AngleSingle::Degrees(0)); + SphericalSingle s = SphericalSingle ::FromPolar(p); + + EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)"; + EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) + << "s.hor Polar(1 0)"; + EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) + << "s.vert Polar(1 0)"; + + p = PolarSingle(1, AngleSingle::Degrees(45)); + s = SphericalSingle ::FromPolar(p); + + EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)"; + EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 45.0F) + << "s.hor Polar(1 45)"; + EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) + << "s.vert Polar(1 45)"; + + p = PolarSingle(1, AngleSingle::Degrees(-45)); + s = SphericalSingle ::FromPolar(p); + + EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)"; + EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -45.0F) + << "s.hor Polar(1 -45)"; + EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) + << "s.vert Polar(1 -45)"; + + p = PolarSingle(0, AngleSingle::Degrees(0)); + s = SphericalSingle ::FromPolar(p); + + EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)"; + EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) + << "s.hor Polar(0 0)"; + EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) + << "s.vert Polar(0 0)"; + + p = PolarSingle(-1, AngleSingle::Degrees(0)); + s = SphericalSingle ::FromPolar(p); + + EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)"; + EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -180.0F) + << "s.hor Polar(-1 0)"; + EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) + << "s.vert Polar(-1 0)"; +} + +TEST(SphericalSingle, Incident1) { + Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f); + SphericalSingle s = SphericalSingle ::FromVector3(v); + + SphericalSingle sr = SphericalSingle(2.49F, AngleSingle::Degrees(98.18f), + AngleSingle::Degrees(24.4F)); + EXPECT_NEAR(s.distance, sr.distance, 1.0e-01); + EXPECT_NEAR(s.direction.horizontal.InDegrees(), + sr.direction.horizontal.InDegrees(), 1.0e-02); + EXPECT_NEAR(s.direction.vertical.InDegrees(), + sr.direction.vertical.InDegrees(), 1.0e-02); + + Vector3 r = Vector3(sr); + EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0"; + EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0"; + EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0"; +} + +TEST(SphericalSingle, Incident2) { + Vector3 v = Vector3(1.0f, 0.0f, 1.0f); + SphericalSingle s = SphericalSingle ::FromVector3(v); + + SphericalSingle sr = SphericalSingle( + 1.4142135623F, AngleSingle::Degrees(45.0f), AngleSingle::Degrees(0.0F)); + EXPECT_NEAR(s.distance, sr.distance, 1.0e-05); + EXPECT_NEAR(s.direction.horizontal.InDegrees(), + sr.direction.horizontal.InDegrees(), 1.0e-05); + EXPECT_NEAR(s.direction.vertical.InDegrees(), + sr.direction.vertical.InDegrees(), 1.0e-05); + + Vector3 r = Vector3(sr); + EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); + EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06); + EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06); + + v = Vector3(0.0f, 1.0f, 1.0f); + s = SphericalSingle ::FromVector3(v); + + sr = SphericalSingle(1.4142135623F, AngleSingle::Degrees(0.0f), + AngleSingle::Degrees(45.0F)); + EXPECT_NEAR(s.distance, sr.distance, 1.0e-05); + EXPECT_NEAR(s.direction.horizontal.InDegrees(), + sr.direction.horizontal.InDegrees(), 1.0e-05); + EXPECT_NEAR(s.direction.vertical.InDegrees(), + sr.direction.vertical.InDegrees(), 1.0e-05); + + r = Vector3(sr); + EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); + EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06); + EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06); + + v = Vector3(1.0f, 1.0f, 1.0f); + s = SphericalSingle ::FromVector3(v); + r = Vector3(s); + + EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02); + EXPECT_NEAR(s.direction.horizontal.InDegrees(), 45.0F, 1.0e-02); + EXPECT_NEAR(s.direction.vertical.InDegrees(), 35.26F, 1.0e-02); + + EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); + EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06); + EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06); + + // s = SphericalSingle(10, 45, 45); + // r = s.ToVector3(); + // EXPECT_NEAR(r.x, 5, 1.0e-06); + // EXPECT_NEAR(r.y, 7.07, 1.0e-06); + // EXPECT_NEAR(r.z, 5, 1.0e-06); +} + +TEST(SphericalSingle, Addition) { + SphericalSingle v1 = + SphericalSingle(1, AngleSingle::Degrees(45), AngleSingle::Degrees(0)); + SphericalSingle v2 = SphericalSingle ::zero; + SphericalSingle r = SphericalSingle ::zero; + + r = v1 + v2; + EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)"; + + r = v1; + r += v2; + EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)"; + + v2 = SphericalSingle(1, AngleSingle::Degrees(-45), AngleSingle::Degrees(0)); + r = v1 + v2; + EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)"; + EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)"; + EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)"; + + v2 = SphericalSingle(1, AngleSingle::Degrees(0), AngleSingle::Degrees(90)); + r = v1 + v2; + EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)"; + EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)"; + EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 45) << "Addition(1 0 90)"; +} + +TEST(SphericalSingle, AdditionPerformance) { + const int numIterations = 1000000; // Number of additions to test + std::vector sphericalObjects; + + // Populate the vector with random SphericalOf objects + for (int i = 0; i < numIterations; ++i) { + float distance = (float)(rand() % 100); + float horizontal = (float)(rand() % 180); + float vertical = (float)(rand() % 360); + SphericalSingle s = SphericalSingle::Deg(distance, horizontal, vertical); + sphericalObjects.push_back(s); + } + + // Measure the time to perform multiple additions + auto start = std::chrono::high_resolution_clock::now(); + + SphericalSingle result = SphericalSingle::zero; // Start with a + // zero-initialized object + + for (int i = 0; i < numIterations - 1; ++i) { + result = result + sphericalObjects[i]; // Add objects + // together + } + + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration duration = end - start; + std::cout << "Time to perform " << numIterations - 1 + << " additions: " << duration.count() << " seconds." << std::endl; + + // Assert that the time taken is less than + // 1 second (or any other performance + // requirement) + ASSERT_LE(duration.count(), 1.0) << "Performance test failed: " + "Additions took longer than 1 " + "second."; +} + +#endif \ No newline at end of file diff --git a/test/SwingTwistSingle_test.cc b/test/SwingTwistSingle_test.cc new file mode 100644 index 0000000..8885ae4 --- /dev/null +++ b/test/SwingTwistSingle_test.cc @@ -0,0 +1,131 @@ +#if GTEST +#include +#include +#include + +#include "SwingTwist.h" + +#define FLOAT_INFINITY std::numeric_limits::infinity() + +TEST(SwingTwistSingle, Quaternion) { + Quaternion q; + SwingTwistSingle s; + Quaternion rq; + + q = Quaternion::identity; + s = SwingTwistSingle::FromQuaternion(q); + rq = s.ToQuaternion(); + EXPECT_EQ(q, rq) << " 0 0 0 1 <-> SwingTwist"; + + q = Quaternion::Euler(90, 0, 0); + s = SwingTwistSingle::FromQuaternion(q); + rq = s.ToQuaternion(); + EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 90 0 0 <-> SwingTwist"; + + q = Quaternion::Euler(0, 90, 0); + s = SwingTwistSingle::FromQuaternion(q); + rq = s.ToQuaternion(); + EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist"; + + q = Quaternion::Euler(0, 0, 90); + s = SwingTwistSingle::FromQuaternion(q); + rq = s.ToQuaternion(); + EXPECT_EQ(q, rq) << " Euler 0 0 90 <-> SwingTwist"; + + q = Quaternion::Euler(0, 180, 0); // ==> spherical S(180 0)T0 + s = SwingTwistSingle::FromQuaternion(q); + rq = s.ToQuaternion(); + EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist"; + + q = Quaternion::Euler(0, 135, 0); // ==> spherical S(180 45)T0 + s = SwingTwistSingle::FromQuaternion(q); + rq = s.ToQuaternion(); + EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist"; +} + +TEST(SwingTwistSingle, AngleAxis) { + SwingTwistSingle s; + SwingTwistSingle r; + + s = SwingTwistSingle::AngleAxis(0, DirectionSingle::up); + EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 up"; + + r = SwingTwistSingle::AngleAxis(90, DirectionSingle::up); + s = SwingTwistSingle::Degrees(90, 0, 0); + EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f)) + << "90 up"; + + r = SwingTwistSingle::AngleAxis(180, DirectionSingle::up); + s = SwingTwistSingle::Degrees(180, 0, 0); + EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f)) + << "180 up"; + + r = SwingTwistSingle::AngleAxis(270, DirectionSingle::up); + s = SwingTwistSingle::Degrees(-90, 0, 0); + EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f)) + << "270 up"; + + r = SwingTwistSingle::AngleAxis(90, DirectionSingle::right); + s = SwingTwistSingle::Degrees(0, 90, 0); + EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f)) + << "90 right"; + + r = SwingTwistSingle::AngleAxis(180, DirectionSingle::right); + s = SwingTwistSingle::Degrees(0, 180, 0); + EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f)) + << "180 right"; + r = SwingTwistSingle::AngleAxis(270, DirectionSingle::right); + s = SwingTwistSingle::Degrees(0, -90, 0); + EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f)) + << "270 right"; + + r = SwingTwistSingle::AngleAxis(90, DirectionSingle::forward); + s = SwingTwistSingle::Degrees(0, 0, 90); + EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f)) + << "90 up"; + + r = SwingTwistSingle::AngleAxis(180, DirectionSingle::forward); + s = SwingTwistSingle::Degrees(0, 0, 180); + EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f)) + << "180 up"; + + r = SwingTwistSingle::AngleAxis(270, DirectionSingle::forward); + s = SwingTwistSingle::Degrees(0, 0, -90); + EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f)) + << "270 up"; + + auto r16 = SwingTwist16::AngleAxis(13, Direction16::down); + auto s16 = SwingTwist16::Degrees(-13, 0, 0); + EXPECT_LT(SwingTwist16::Angle(r16, s16), Angle16::Degrees(10e-2f)) + << "270 up"; +} + +TEST(SwingTwistSingle, Normalize) { + SwingTwistSingle s; + + s = SwingTwistSingle::Degrees(0, 0, 0); + EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 0 0 Normalized"; + + s = SwingTwistSingle::Degrees(0, 180, 0); + EXPECT_EQ(s, SwingTwistSingle::Degrees(180, 0, 180)) << "0 180 0 Normalized"; + + s = SwingTwistSingle::Degrees(0, 180, 180); + EXPECT_EQ(s, SwingTwistSingle::Degrees(180, 0, 0)) << "0 180 180 Normalized"; + + s = SwingTwistSingle::Degrees(270, 90, 0); + EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, 90, 0)) << "270 90 0 Normalized"; + + s = SwingTwistSingle::Degrees(270, 270, 0); + EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, -90, 0)) + << "270 270 0 Normalized"; + + s = SwingTwistSingle::Degrees(270, 225, 0); + EXPECT_EQ(s, SwingTwistSingle::Degrees(90, -45, -180)) + << "270 225 0 Normalized"; + + s = SwingTwistSingle::Degrees(270, 0, 225); + EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, 0, -135)) + << "270 0 225 Normalized"; +} + +#endif \ No newline at end of file diff --git a/test/Vector2_test.cc b/test/Vector2_test.cc index 83c52ce..3afeb4b 100644 --- a/test/Vector2_test.cc +++ b/test/Vector2_test.cc @@ -7,6 +7,33 @@ #define FLOAT_INFINITY std::numeric_limits::infinity() +TEST(Vector2, FromPolar) { + Vector2 v; + PolarSingle p; + Vector2 r; + + v = Vector2(0, 1); + p = PolarSingle::FromVector2(v); + r = Vector2(p); + + EXPECT_FLOAT_EQ(r.x, 0.0F) << "FromPolar(0 1)"; + EXPECT_FLOAT_EQ(r.y, 1.0F) << "FromPolar(0 1)"; + + v = Vector2(1, 0); + p = PolarSingle::FromVector2(v); + r = Vector2(p); + + EXPECT_FLOAT_EQ(r.x, 1.0F) << "FromPolar(1 0)"; + EXPECT_NEAR(r.y, 0.0F, 1.0e-07) << "FromPolar(1 0)"; + + v = Vector2(0, 0); + p = PolarSingle::FromVector2(v); + r = Vector2(p); + + EXPECT_FLOAT_EQ(r.x, 0.0F) << "FromPolar(0 0)"; + EXPECT_FLOAT_EQ(r.y, 0.0F) << "FromPolar(0 0)"; +} + TEST(Vector2, Magnitude) { Vector2 v = Vector2(1, 2); float m = 0; @@ -99,8 +126,6 @@ TEST(Vector2, Normalize) { } TEST(Vector2, Negate) { - bool r = false; - Vector2 v1 = Vector2(4, 5); Vector2 v = Vector2::zero; @@ -129,8 +154,6 @@ TEST(Vector2, Negate) { } TEST(Vector2, Subtract) { - bool r = false; - Vector2 v1 = Vector2(4, 5); Vector2 v2 = Vector2(1, 2); Vector2 v = Vector2::zero; @@ -145,11 +168,17 @@ TEST(Vector2, Subtract) { v2 = Vector2(4, 5); v = v1 - v2; EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 - 4 5"; + v = v1; + v -= v2; + EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 - 4 5"; v2 = Vector2(0, 0); v = v1 - v2; EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 - 0 0"; + v -= v2; + EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 - 0 0"; + if (std::numeric_limits::is_iec559) { v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY); v = v1 - v2; @@ -164,11 +193,9 @@ TEST(Vector2, Subtract) { } TEST(Vector2, Addition) { - Vector2 v1 = Vector2(4, 5); Vector2 v2 = Vector2(1, 2); Vector2 v = Vector2::zero; - bool r = false; v = v1 + v2; EXPECT_TRUE(v == Vector2(5, 7)) << "4 5 + 1 2"; @@ -176,10 +203,15 @@ TEST(Vector2, Addition) { v2 = Vector2(-1, -2); v = v1 + v2; EXPECT_TRUE(v == Vector2(3, 3)) << "4 5 + -1 -2"; + v = v1; + v += v2; + EXPECT_TRUE(v == Vector2(3, 3)) << "4 5 + -1 -2"; v2 = Vector2(0, 0); v = v1 + v2; EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 + 0 0"; + v += v2; + EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 + 0 0"; if (std::numeric_limits::is_iec559) { v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY); @@ -195,8 +227,6 @@ TEST(Vector2, Addition) { } TEST(Vector2, Scale) { - bool r = false; - Vector2 v1 = Vector2(4, 5); Vector2 v2 = Vector2(1, 2); Vector2 v = Vector2::zero; @@ -226,8 +256,6 @@ TEST(Vector2, Scale) { } TEST(Vector2, Multiply) { - bool r = false; - Vector2 v1 = Vector2(4, 5); float f = 3; Vector2 v = Vector2::zero; @@ -257,8 +285,6 @@ TEST(Vector2, Multiply) { } TEST(Vector2, Divide) { - bool r = false; - Vector2 v1 = Vector2(4, 5); float f = 2; Vector2 v = Vector2::zero; @@ -399,11 +425,11 @@ TEST(Vector2, SignedAngle) { bool r = false; f = Vector2::SignedAngle(v1, v2); - EXPECT_FLOAT_EQ(f, 12.09476F) << "SignedAngle(4 5, 1 2)"; + EXPECT_FLOAT_EQ(f, -12.09476F) << "SignedAngle(4 5, 1 2)"; v2 = Vector2(-1, -2); f = Vector2::SignedAngle(v1, v2); - EXPECT_FLOAT_EQ(f, -167.9052F) << "SignedAngle(4 5, -1 -2)"; + EXPECT_FLOAT_EQ(f, 167.9052F) << "SignedAngle(4 5, -1 -2)"; v2 = Vector2(0, 0); f = Vector2::SignedAngle(v1, v2); @@ -420,22 +446,32 @@ TEST(Vector2, SignedAngle) { r = isnan(f); EXPECT_TRUE(r) << "SignedAngle(4 5, -INFINITY -INFINITY)"; } + + v1 = Vector2(0, 1); + v2 = Vector2(1, 0); + f = Vector2::SignedAngle(v1, v2); + EXPECT_FLOAT_EQ(f, 90.0F) << "SignedAngle(0 1, 1 0)"; + + v1 = Vector2(0, 1); + v2 = Vector2(0, -1); + f = Vector2::SignedAngle(v1, v2); + EXPECT_FLOAT_EQ(f, 180.0F) << "SignedAngle(0 1, 1 0)"; } TEST(Vector2, Rotate) { Vector2 v1 = Vector2(1, 2); Vector2 r = Vector2(0, 0); - r = Vector2::Rotate(v1, 0); + r = Vector2::Rotate(v1, AngleSingle::Degrees(0)); EXPECT_FLOAT_EQ(Vector2::Distance(r, v1), 0); - r = Vector2::Rotate(v1, 180); + r = Vector2::Rotate(v1, AngleSingle::Degrees(180)); EXPECT_NEAR(Vector2::Distance(r, Vector2(-1, -2)), 0, 1.0e-06); - r = Vector2::Rotate(v1, -90); + r = Vector2::Rotate(v1, AngleSingle::Degrees(-90)); EXPECT_NEAR(Vector2::Distance(r, Vector2(2, -1)), 0, 1.0e-06); - r = Vector2::Rotate(v1, 270); + r = Vector2::Rotate(v1, AngleSingle::Degrees(270)); EXPECT_NEAR(Vector2::Distance(r, Vector2(2, -1)), 0, 1.0e-06); } @@ -460,6 +496,4 @@ TEST(Vector2, Lerp) { EXPECT_FLOAT_EQ(Vector2::Distance(r, Vector2(-2.0, -1.0f)), 0); } -TEST(Vector2, DISABLED_ToFactor) {} - #endif \ No newline at end of file diff --git a/test/Vector3_test.cc b/test/Vector3_test.cc index e8f61f8..82e57e9 100644 --- a/test/Vector3_test.cc +++ b/test/Vector3_test.cc @@ -7,6 +7,32 @@ #define FLOAT_INFINITY std::numeric_limits::infinity() +TEST(Vector3, FromSpherical) { + Vector3 v = Vector3(0, 0, 1); + SphericalOf s = SphericalOf::FromVector3(v); + Vector3 r = Vector3(s); + + EXPECT_FLOAT_EQ(r.Right(), 0.0F) << "toVector3.x 0 0 1"; + EXPECT_NEAR(r.Up(), 0.0F, 1.0e-06) << "toVector3.y 0 0 1"; + EXPECT_FLOAT_EQ(r.Forward(), 1.0F) << "toVector3.z 0 0 1"; + + v = Vector3(0, 1, 0); + s = SphericalOf::FromVector3(v); + r = Vector3(s); + + EXPECT_FLOAT_EQ(r.Right(), 0.0F) << "toVector3.x 0 1 0"; + EXPECT_FLOAT_EQ(r.Up(), 1.0F) << "toVector3.y 0 1 0"; + EXPECT_NEAR(r.Forward(), 0.0F, 1.0e-06) << "toVector3.z 0 1 0"; + + v = Vector3(1, 0, 0); + s = SphericalOf::FromVector3(v); + r = Vector3(s); + + EXPECT_FLOAT_EQ(r.Right(), 1.0F) << "toVector3.x 1 0 0"; + EXPECT_NEAR(r.Up(), 0.0F, 1.0e-06) << "toVector3.y 1 0 0"; + EXPECT_NEAR(r.Forward(), 0.0F, 1.0e-06) << "toVector3.z 1 0 0"; +} + TEST(Vector3, Magnitude) { Vector3 v = Vector3(1, 2, 3); float m = 0; @@ -92,19 +118,17 @@ TEST(Vector3, Normalize) { if (std::numeric_limits::is_iec559) { v1 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY); v = v1.normalized(); - r = isnan(v.x) && isnan(v.y) && isnan(v.z); + r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward()); EXPECT_TRUE(r) << "v.normalized INFINITY INFINITY INFINITY"; v1 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY); v = v1.normalized(); - r = isnan(v.x) && isnan(v.y) && isnan(v.z); + r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward()); EXPECT_TRUE(r) << "v.normalized -INFINITY -INFINITY -INFINITY"; } } TEST(Vector3, Negate) { - bool r = false; - Vector3 v1 = Vector3(4, 5, 6); Vector3 v = Vector3::zero; @@ -133,8 +157,6 @@ TEST(Vector3, Negate) { } TEST(Vector3, Subtract) { - bool r = false; - Vector3 v1 = Vector3(4, 5, 6); Vector3 v2 = Vector3(1, 2, 3); Vector3 v = Vector3::zero; @@ -168,11 +190,9 @@ TEST(Vector3, Subtract) { } TEST(Vector3, Addition) { - Vector3 v1 = Vector3(4, 5, 6); Vector3 v2 = Vector3(1, 2, 3); Vector3 v = Vector3::zero; - bool r = false; v = v1 + v2; EXPECT_TRUE(v == Vector3(5, 7, 9)) << "4 5 6 + 1 2 3"; @@ -199,8 +219,6 @@ TEST(Vector3, Addition) { } TEST(Vector3, Scale) { - bool r = false; - Vector3 v1 = Vector3(4, 5, 6); Vector3 v2 = Vector3(1, 2, 3); Vector3 v = Vector3::zero; @@ -230,8 +248,6 @@ TEST(Vector3, Scale) { } TEST(Vector3, Multiply) { - bool r = false; - Vector3 v1 = Vector3(4, 5, 6); float f = 3; Vector3 v = Vector3::zero; @@ -261,8 +277,6 @@ TEST(Vector3, Multiply) { } TEST(Vector3, Divide) { - bool r = false; - Vector3 v1 = Vector3(4, 5, 6); float f = 2; Vector3 v = Vector3::zero; @@ -395,12 +409,12 @@ TEST(Vector3, Cross) { if (std::numeric_limits::is_iec559) { v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY); v = Vector3::Cross(v1, v2); - r = isnan(v.x) && isnan(v.y) && isnan(v.z); + r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward()); EXPECT_TRUE(r) << "Cross(4 5 6, INFINITY INFINITY INFINITY)"; v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY); v = Vector3::Cross(v1, v2); - r = isnan(v.x) && isnan(v.y) && isnan(v.z); + r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward()); EXPECT_TRUE(r) << "Cross(4 5 6, -INFINITY -INFINITY -INFINITY)"; } } @@ -428,12 +442,12 @@ TEST(Vector3, Project) { if (std::numeric_limits::is_iec559) { v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY); v = Vector3::Project(v1, v2); - r = isnan(v.x) && isnan(v.y) && isnan(v.z); + r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward()); EXPECT_TRUE(r) << "Project(4 5 6, INFINITY INFINITY INFINITY)"; v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY); v = Vector3::Project(v1, v2); - r = isnan(v.x) && isnan(v.y) && isnan(v.z); + r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward()); EXPECT_TRUE(r) << "Project(4 5 6, -INFINITY -INFINITY -INFINITY)"; } } @@ -461,12 +475,12 @@ TEST(Vector3, ProjectOnPlane) { if (std::numeric_limits::is_iec559) { v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY); v = Vector3::ProjectOnPlane(v1, v2); - r = isnan(v.x) && isnan(v.y) && isnan(v.z); + r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward()); EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, INFINITY INFINITY INFINITY)"; v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY); v = Vector3::ProjectOnPlane(v1, v2); - r = isnan(v.x) && isnan(v.y) && isnan(v.z); + r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward()); EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, -INFINITY -INFINITY -INFINITY)"; } } @@ -474,29 +488,29 @@ TEST(Vector3, ProjectOnPlane) { TEST(Vector3, Angle) { Vector3 v1 = Vector3(4, 5, 6); Vector3 v2 = Vector3(1, 2, 3); - float f = 0; + AngleOf f = AngleOf::Degrees(0); bool r = false; f = Vector3::Angle(v1, v2); - EXPECT_FLOAT_EQ(f, 12.9331388F) << "Angle(4 5 6, 1 2 3)"; + EXPECT_FLOAT_EQ(f.InDegrees(), 12.9331388F) << "Angle(4 5 6, 1 2 3)"; v2 = Vector3(-1, -2, -3); f = Vector3::Angle(v1, v2); - EXPECT_FLOAT_EQ(f, 167.066864F) << "Angle(4 5 6, -1 -2 -3)"; + EXPECT_FLOAT_EQ(f.InDegrees(), 167.066864F) << "Angle(4 5 6, -1 -2 -3)"; v2 = Vector3(0, 0, 0); f = Vector3::Angle(v1, v2); - EXPECT_FLOAT_EQ(f, 0) << "Angle(4 5 6, 0 0 0)"; + EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "Angle(4 5 6, 0 0 0)"; if (std::numeric_limits::is_iec559) { v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY); f = Vector3::Angle(v1, v2); - r = isnan(f); + r = isnan(f.InDegrees()); EXPECT_TRUE(r) << "Angle(4 5 6, INFINITY INFINITY INFINITY)"; v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY); f = Vector3::Angle(v1, v2); - r = isnan(f); + r = isnan(f.InDegrees()); EXPECT_TRUE(r) << "Angle(4 5 6, -INFINITY -INFINITY -INFINITY)"; } } @@ -505,39 +519,42 @@ TEST(Vector3, SignedAngle) { Vector3 v1 = Vector3(4, 5, 6); Vector3 v2 = Vector3(1, 2, 3); Vector3 v3 = Vector3(7, 8, -9); - float f = 0; + AngleOf f = AngleOf::Degrees(0); bool r = false; f = Vector3::SignedAngle(v1, v2, v3); - EXPECT_FLOAT_EQ(f, -12.9331388F) << "SignedAngle(4 5 6, 1 2 3, 7 8 -9)"; + EXPECT_FLOAT_EQ(f.InDegrees(), -12.9331388F) + << "SignedAngle(4 5 6, 1 2 3, 7 8 -9)"; v2 = Vector3(-1, -2, -3); f = Vector3::SignedAngle(v1, v2, v3); - EXPECT_FLOAT_EQ(f, 167.066864F) << "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)"; + EXPECT_FLOAT_EQ(f.InDegrees(), 167.066864F) + << "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)"; v2 = Vector3(0, 0, 0); f = Vector3::SignedAngle(v1, v2, v3); - EXPECT_FLOAT_EQ(f, 0) << "SignedAngle(4 5 6, 0 0 0, 7 8 -9 )"; + EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "SignedAngle(4 5 6, 0 0 0, 7 8 -9 )"; v2 = Vector3(1, 2, 3); v3 = Vector3(-7, -8, 9); f = Vector3::SignedAngle(v1, v2, v3); - EXPECT_FLOAT_EQ(f, 12.9331388F) << "SignedAngle(4 5 6, 1 2 3, -7 -8 9)"; + EXPECT_FLOAT_EQ(f.InDegrees(), 12.9331388F) + << "SignedAngle(4 5 6, 1 2 3, -7 -8 9)"; v3 = Vector3(0, 0, 0); f = Vector3::SignedAngle(v1, v2, v3); - EXPECT_FLOAT_EQ(f, 0) << "SignedAngle(4 5 6, 1 2 3, 0 0 0)"; + EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "SignedAngle(4 5 6, 1 2 3, 0 0 0)"; if (std::numeric_limits::is_iec559) { v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY); f = Vector3::SignedAngle(v1, v2, v3); - r = isnan(f); + r = isnan(f.InDegrees()); EXPECT_TRUE(r) << "SignedAngle(4 5 6, INFINITY INFINITY INFINITY)"; v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY); f = Vector3::SignedAngle(v1, v2, v3); - r = isnan(f); + r = isnan(f.InDegrees()); EXPECT_TRUE(r) << "SignedAngle(4 5 6, -INFINITY -INFINITY -INFINITY)"; } } @@ -562,4 +579,5 @@ TEST(Vector3, Lerp) { r = Vector3::Lerp(v1, v2, 2); EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(-2.0, -1.0f, 0.0f)), 0); } + #endif \ No newline at end of file From 2883f5e0fa7cfa56ccb6c843d4b2d38412407fed Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 2 Jun 2025 12:30:21 +0200 Subject: [PATCH 36/56] thing tytpe enum->const --- Participant.cpp | 2 +- Thing.cpp | 502 ++++++++++++++++++++++++++---------------------- Thing.h | 36 ++-- 3 files changed, 293 insertions(+), 247 deletions(-) diff --git a/Participant.cpp b/Participant.cpp index 3f11839..549f78a 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -16,7 +16,7 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) { } Participant::Participant() { - std::cout << "P\n"; + //std::cout << "P\n"; //this->root.name = "Isolated"; this->root = new Thing(this); this->root->name = "Root"; diff --git a/Thing.cpp b/Thing.cpp index 1ec24b3..5fc4c72 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -16,300 +16,344 @@ #include #endif -namespace RoboidControl { +namespace RoboidControl +{ #pragma region Init -Thing* Thing::LocalRoot() { - Participant* p = Participant::LocalParticipant; - Thing* localRoot = p->root; - return localRoot; -} + Thing *Thing::LocalRoot() + { + Participant *p = Participant::LocalParticipant; + Thing *localRoot = p->root; + return localRoot; + } -// Only use this for root things -Thing::Thing(Participant* owner) { - this->type = Type::Roboid; // should become root + // Only use this for root things + Thing::Thing(Participant *owner) + { + this->type = Type::Root; // should become root - this->position = Spherical::zero; - this->positionUpdated = true; - this->orientation = SwingTwist::identity; - this->orientationUpdated = true; - this->hierarchyChanged = true; + this->position = Spherical::zero; + this->positionUpdated = true; + this->orientation = SwingTwist::identity; + this->orientationUpdated = true; + this->hierarchyChanged = true; - this->linearVelocity = Spherical::zero; - this->angularVelocity = Spherical::zero; + this->linearVelocity = Spherical::zero; + this->angularVelocity = Spherical::zero; - this->owner = owner; - //this->owner->Add(this, true); - std::cout << this->owner->name << ": New root thing " << std::endl; -} + this->owner = owner; + // this->owner->Add(this, true); + //std::cout << this->owner->name << ": New root thing " << std::endl; + } -Thing::Thing(unsigned char thingType, Thing* parent) { - this->type = thingType; + Thing::Thing(unsigned char thingType, Thing *parent) + { + this->type = thingType; - this->position = Spherical::zero; - this->positionUpdated = true; - this->orientation = SwingTwist::identity; - this->orientationUpdated = true; - this->hierarchyChanged = true; + this->position = Spherical::zero; + this->positionUpdated = true; + this->orientation = SwingTwist::identity; + this->orientationUpdated = true; + this->hierarchyChanged = true; - this->linearVelocity = Spherical::zero; - this->angularVelocity = Spherical::zero; + this->linearVelocity = Spherical::zero; + this->angularVelocity = Spherical::zero; - this->owner = parent->owner; - this->owner->Add(this, true); - this->SetParent(parent); + this->owner = parent->owner; + this->owner->Add(this, true); + this->SetParent(parent); - std::cout << this->owner->name << ": New thing for " << parent->name - << std::endl; -} + std::cout << this->owner->name << ": New thing for " << parent->name + << std::endl; + } -Thing::~Thing() { - std::cout << "Destroy thing " << this->name << std::endl; -} + Thing::~Thing() + { + std::cout << "Destroy thing " << this->name << std::endl; + } -// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, -// unsigned char thingId) { -// Thing thing = Thing(owner, thingType); -// thing.id = thingId; -// return thing; -// } + Thing Thing::Reconstruct(Participant *owner, unsigned char thingType, unsigned char thingId) + { + Thing thing = Thing(thingType, owner->root); + thing.id = thingId; + return thing; + } #pragma endregion Init -void Thing::SetName(const char* name) { - this->name = name; - this->nameChanged = true; -} + void Thing::SetName(const char *name) + { + this->name = name; + this->nameChanged = true; + } -const char* Thing::GetName() const { - return this->name; -} + const char *Thing::GetName() const + { + return this->name; + } -void Thing::SetModel(const char* url) { - this->modelUrl = url; -} + void Thing::SetModel(const char *url) + { + this->modelUrl = url; + } #pragma region Hierarchy -void Thing::SetParent(Thing* parent) { - if (parent == nullptr) { - Thing* parentThing = this->parent; - if (parentThing != nullptr) - parentThing->RemoveChild(this); - this->parent = nullptr; - } else - parent->AddChild(this); - this->hierarchyChanged = true; -} - -// void Thing::SetParent(Thing* parent) { -// parent->AddChild(this); -// this->hierarchyChanged = true; -// } - -// const Thing& Thing::GetParent() { -// return *this->parent; -// } - -bool Thing::IsRoot() const { - return this == LocalRoot() || this->parent == nullptr; //&Thing::Root; -} - -// void Thing::SetParent(Thing* root, const char* name) { -// Thing* thing = root->FindChild(name); -// if (thing != nullptr) -// this->SetParent(thing); -// } - -Thing* Thing::GetParent() { - return this->parent; -} - -Thing* Thing::GetChildByIndex(unsigned char ix) { - return this->children[ix]; -} - -void Thing::AddChild(Thing* child) { - unsigned char newChildCount = this->childCount + 1; - Thing** newChildren = new Thing*[newChildCount]; - - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - newChildren[childIx] = this->children[childIx]; - if (this->children[childIx] == child) { - // child is already present, stop copying do not update the children - delete[] newChildren; - return; + void Thing::SetParent(Thing *parent) + { + if (parent == nullptr) + { + Thing *parentThing = this->parent; + if (parentThing != nullptr) + parentThing->RemoveChild(this); + this->parent = nullptr; } + else + parent->AddChild(this); + this->hierarchyChanged = true; } - newChildren[this->childCount] = child; - child->parent = this; + // void Thing::SetParent(Thing* parent) { + // parent->AddChild(this); + // this->hierarchyChanged = true; + // } - if (this->children != nullptr) - delete[] this->children; + // const Thing& Thing::GetParent() { + // return *this->parent; + // } - this->children = newChildren; - this->childCount = newChildCount; -} + bool Thing::IsRoot() const + { + return this == LocalRoot() || this->parent == nullptr; //&Thing::Root; + } -Thing* Thing::RemoveChild(Thing* child) { - unsigned char newChildCount = this->childCount - 1; - Thing** newChildren = new Thing*[newChildCount]; + // void Thing::SetParent(Thing* root, const char* name) { + // Thing* thing = root->FindChild(name); + // if (thing != nullptr) + // this->SetParent(thing); + // } - unsigned char newChildIx = 0; - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - if (this->children[childIx] != child) { - if (newChildIx == newChildCount) { // We did not find the child - // stop copying and return nothing + Thing *Thing::GetParent() + { + return this->parent; + } + + Thing *Thing::GetChildByIndex(unsigned char ix) + { + return this->children[ix]; + } + + void Thing::AddChild(Thing *child) + { + unsigned char newChildCount = this->childCount + 1; + Thing **newChildren = new Thing *[newChildCount]; + + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + { + newChildren[childIx] = this->children[childIx]; + if (this->children[childIx] == child) + { + // child is already present, stop copying do not update the children delete[] newChildren; - return nullptr; - } else - newChildren[newChildIx++] = this->children[childIx]; + return; + } } + + newChildren[this->childCount] = child; + child->parent = this; + + if (this->children != nullptr) + delete[] this->children; + + this->children = newChildren; + this->childCount = newChildCount; } - child->parent = Thing::LocalRoot(); + Thing *Thing::RemoveChild(Thing *child) + { + unsigned char newChildCount = this->childCount - 1; + Thing **newChildren = new Thing *[newChildCount]; - delete[] this->children; - this->children = newChildren; - this->childCount = newChildCount; + unsigned char newChildIx = 0; + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + { + if (this->children[childIx] != child) + { + if (newChildIx == newChildCount) + { // We did not find the child + // stop copying and return nothing + delete[] newChildren; + return nullptr; + } + else + newChildren[newChildIx++] = this->children[childIx]; + } + } - return child; -} + child->parent = Thing::LocalRoot(); -Thing* Thing::GetChild(unsigned char id, bool recurse) { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing* child = this->children[childIx]; - if (child == nullptr) - continue; - if (child->id == id) - return child; + delete[] this->children; + this->children = newChildren; + this->childCount = newChildCount; - if (recurse) { - Thing* foundChild = child->GetChild(id, recurse); + return child; + } + + Thing *Thing::GetChild(unsigned char id, bool recurse) + { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + { + Thing *child = this->children[childIx]; + if (child == nullptr) + continue; + if (child->id == id) + return child; + + if (recurse) + { + Thing *foundChild = child->GetChild(id, recurse); + if (foundChild != nullptr) + return foundChild; + } + } + return nullptr; + } + + Thing *Thing::FindChild(const char *name, bool recurse) + { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + { + Thing *child = this->children[childIx]; + if (child == nullptr || child->name == nullptr) + continue; + + if (strcmp(child->name, name) == 0) + return child; + + Thing *foundChild = child->FindChild(name); if (foundChild != nullptr) return foundChild; } + return nullptr; } - return nullptr; -} - -Thing* Thing::FindChild(const char* name, bool recurse) { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing* child = this->children[childIx]; - if (child == nullptr || child->name == nullptr) - continue; - - if (strcmp(child->name, name) == 0) - return child; - - Thing* foundChild = child->FindChild(name); - if (foundChild != nullptr) - return foundChild; - } - return nullptr; -} #pragma endregion Hierarchy #pragma region Pose -void Thing::SetPosition(Spherical position) { - this->position = position; - this->positionUpdated = true; -} -Spherical Thing::GetPosition() { - return this->position; -} - -void Thing::SetOrientation(SwingTwist orientation) { - this->orientation = orientation; - this->orientationUpdated = true; -} - -SwingTwist Thing::GetOrientation() { - return this->orientation; -} - -void Thing::SetLinearVelocity(Spherical linearVelocity) { - if (this->linearVelocity.distance != linearVelocity.distance) { - this->linearVelocity = linearVelocity; - this->linearVelocityUpdated = true; + void Thing::SetPosition(Spherical position) + { + this->position = position; + this->positionUpdated = true; } -} - -Spherical Thing::GetLinearVelocity() { - return this->linearVelocity; -} - -void Thing::SetAngularVelocity(Spherical angularVelocity) { - if (this->angularVelocity.distance != angularVelocity.distance) { - this->angularVelocity = angularVelocity; - this->angularVelocityUpdated = true; + Spherical Thing::GetPosition() + { + return this->position; } -} -Spherical Thing::GetAngularVelocity() { - return this->angularVelocity; -} + void Thing::SetOrientation(SwingTwist orientation) + { + this->orientation = orientation; + this->orientationUpdated = true; + } + + SwingTwist Thing::GetOrientation() + { + return this->orientation; + } + + void Thing::SetLinearVelocity(Spherical linearVelocity) + { + if (this->linearVelocity.distance != linearVelocity.distance) + { + this->linearVelocity = linearVelocity; + this->linearVelocityUpdated = true; + } + } + + Spherical Thing::GetLinearVelocity() + { + return this->linearVelocity; + } + + void Thing::SetAngularVelocity(Spherical angularVelocity) + { + if (this->angularVelocity.distance != angularVelocity.distance) + { + this->angularVelocity = angularVelocity; + this->angularVelocityUpdated = true; + } + } + + Spherical Thing::GetAngularVelocity() + { + return this->angularVelocity; + } #pragma endregion Pose #pragma region Update -unsigned long Thing::GetTimeMs() { + unsigned long Thing::GetTimeMs() + { #if defined(ARDUINO) - unsigned long ms = millis(); - return ms; + unsigned long ms = millis(); + return ms; #else - auto now = std::chrono::steady_clock::now(); - auto ms = std::chrono::duration_cast( - now.time_since_epoch()); - return static_cast(ms.count()); + auto now = std::chrono::steady_clock::now(); + auto ms = std::chrono::duration_cast( + now.time_since_epoch()); + return static_cast(ms.count()); #endif -} + } -// void Thing::Update(bool recursive) { -// Update(GetTimeMs(), recursive); -// } + // void Thing::Update(bool recursive) { + // Update(GetTimeMs(), recursive); + // } -void Thing::PrepareForUpdate() {} + void Thing::PrepareForUpdate() {} -void Thing::Update(bool recursive) { - // if (this->positionUpdated || this->orientationUpdated) - // OnPoseChanged callback - this->positionUpdated = false; - this->orientationUpdated = false; - // this->linearVelocityUpdated = false; - // this->angularVelocityUpdated = false; - this->hierarchyChanged = false; - this->nameChanged = false; + void Thing::Update(bool recursive) + { + // if (this->positionUpdated || this->orientationUpdated) + // OnPoseChanged callback + this->positionUpdated = false; + this->orientationUpdated = false; + // this->linearVelocityUpdated = false; + // this->angularVelocityUpdated = false; + this->hierarchyChanged = false; + this->nameChanged = false; - if (recursive) { - // std::cout << "# children: " << (int)this->childCount << std::endl; - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing* child = this->children[childIx]; - if (child == nullptr) - continue; - child->Update(recursive); + if (recursive) + { + // std::cout << "# children: " << (int)this->childCount << std::endl; + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + { + Thing *child = this->children[childIx]; + if (child == nullptr) + continue; + child->Update(recursive); + } } } -} -void Thing::UpdateThings() { - IsolatedParticipant::Isolated()->Update(); -} + void Thing::UpdateThings() + { + IsolatedParticipant::Isolated()->Update(); + } #pragma endregion Update -int Thing::GenerateBinary(char* buffer, unsigned char* ix) { - (void)buffer; - (void)ix; - return 0; -} -void Thing::ProcessBinary(char* bytes) { - (void)bytes; -}; + int Thing::GenerateBinary(char *buffer, unsigned char *ix) + { + (void)buffer; + (void)ix; + return 0; + } + void Thing::ProcessBinary(char *bytes) + { + (void)bytes; + }; -} // namespace RoboidControl \ No newline at end of file +} // namespace RoboidControl \ No newline at end of file diff --git a/Thing.h b/Thing.h index 439606c..5e8a5a8 100644 --- a/Thing.h +++ b/Thing.h @@ -20,24 +20,26 @@ class ParticipantUDP; class Thing { public: /// @brief Predefined thing types - enum Type : unsigned char { - Undetermined, - // Sensor, - Switch, - DistanceSensor, - DirectionalSensor, - TemperatureSensor, - TouchSensor, - // Motor, - ControlledMotor, - UncontrolledMotor, - Servo, - IncrementalEncoder, + struct Type { + static const unsigned char Undetermined = 0x00; + // Sensor + static const unsigned char Switch = 0x01; + static const unsigned char DistanceSensor = 0x02; + static const unsigned char DirectionalSensor = 0x03; + static const unsigned char TemperatureSensor = 0x04; + static const unsigned char TouchSensor = 0x05; + // Motor + static const unsigned char ControlledMotor = 0x06; + static const unsigned char UncontrolledMotor = 0x07; + static const unsigned char Servo = 0x08; + static const unsigned char IncrementalEncoder = 0x19; // Other - Roboid, - Humanoid, - ExternalSensor, - DifferentialDrive + static const unsigned char Root = 0x10; + static const unsigned char Roboid = 0x09; + static const unsigned char Humanoid = 0x0A; + static const unsigned char ExternalSensor = 0x08; + static const unsigned char Animator = 0x0C; + static const unsigned char DifferentialDrive = 0x0D; }; #pragma region Init From 57cf14b4871320574487b0fea90c6aad48f7f774 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 2 Jun 2025 12:35:53 +0200 Subject: [PATCH 37/56] Removed reconstruct thing and thingtype in constructor --- Thing.cpp | 11 ++--------- Thing.h | 7 +------ Things/DifferentialDrive.cpp | 7 ++++--- Things/DigitalSensor.cpp | 15 +++------------ Things/Motor.cpp | 4 +++- Things/RelativeEncoder.cpp | 5 +++-- Things/TemperatureSensor.cpp | 12 +++--------- Things/TouchSensor.cpp | 3 ++- 8 files changed, 21 insertions(+), 43 deletions(-) diff --git a/Thing.cpp b/Thing.cpp index 5fc4c72..29e4f95 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -47,9 +47,9 @@ namespace RoboidControl //std::cout << this->owner->name << ": New root thing " << std::endl; } - Thing::Thing(unsigned char thingType, Thing *parent) + Thing::Thing(Thing *parent) { - this->type = thingType; + this->type = Type::Undetermined; this->position = Spherical::zero; this->positionUpdated = true; @@ -73,13 +73,6 @@ namespace RoboidControl std::cout << "Destroy thing " << this->name << std::endl; } - Thing Thing::Reconstruct(Participant *owner, unsigned char thingType, unsigned char thingId) - { - Thing thing = Thing(thingType, owner->root); - thing.id = thingId; - return thing; - } - #pragma endregion Init void Thing::SetName(const char *name) diff --git a/Thing.h b/Thing.h index 5e8a5a8..bce0053 100644 --- a/Thing.h +++ b/Thing.h @@ -58,8 +58,7 @@ class Thing { /// The owner will be the same as the owner of the parent thing, it will /// be Participant::LocalParticipant if the parent is not specified. A thing /// without a parent will be a root thing. - Thing(unsigned char thingType = Thing::Type::Undetermined, - Thing* parent = LocalRoot()); + Thing(Thing* parent = LocalRoot()); /// @brief Create a new child thing /// @param parent The parent thing @@ -70,10 +69,6 @@ class Thing { ~Thing(); - static Thing Reconstruct(Participant* owner, - unsigned char thingType, - unsigned char thingId); - #pragma endregion Init public: diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index 14967ae..875b654 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -4,8 +4,8 @@ namespace RoboidControl { -DifferentialDrive::DifferentialDrive(Thing* parent) - : Thing(Type::DifferentialDrive, parent) { +DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent) { + this->type = Type::DifferentialDrive; this->name = "Differential drive"; this->leftWheel = new Motor(this); @@ -18,7 +18,8 @@ DifferentialDrive::DifferentialDrive(Thing* parent) DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor, Thing* parent) - : Thing(Type::DifferentialDrive, parent) { + : Thing(parent) { + this->type = Type::DifferentialDrive; this->name = "Differential drive"; this->leftWheel = leftMotor; this->rightWheel = rightMotor; diff --git a/Things/DigitalSensor.cpp b/Things/DigitalSensor.cpp index ba0d859..35b9190 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -2,18 +2,9 @@ namespace RoboidControl { -//DigitalSensor::DigitalSensor() : Thing(Type::Switch) {} - -// DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId) -// : Thing(owner, Type::Switch, thingId) {} - -// DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId) -// : Thing(parent, Type::Switch) {} - -// DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {} - -// DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {} -DigitalSensor::DigitalSensor(Thing* parent) : Thing(Type::Switch, parent) {} +DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent) { + this->type = Type::Switch; +} int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) { bytes[(*ix)++] = state ? 1 : 0; diff --git a/Things/Motor.cpp b/Things/Motor.cpp index 12ab226..936d4ec 100644 --- a/Things/Motor.cpp +++ b/Things/Motor.cpp @@ -2,7 +2,9 @@ namespace RoboidControl { -Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {} +Motor::Motor(Thing* parent) : Thing(parent) { + this->type = Type::UncontrolledMotor; +} void Motor::SetTargetVelocity(float targetSpeed) { this->targetVelocity = targetSpeed; diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp index 6e6a19c..573f84b 100644 --- a/Things/RelativeEncoder.cpp +++ b/Things/RelativeEncoder.cpp @@ -2,8 +2,9 @@ namespace RoboidControl { -RelativeEncoder::RelativeEncoder(Thing* parent) - : Thing(Type::IncrementalEncoder, parent) {} +RelativeEncoder::RelativeEncoder(Thing* parent) : Thing(parent) { + this->type = Type::IncrementalEncoder; +} float RelativeEncoder::GetRotationSpeed() { return rotationSpeed; diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index d67800b..39ecc41 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -4,15 +4,9 @@ namespace RoboidControl { -// TemperatureSensor::TemperatureSensor(Participant* participant, -// unsigned char thingId) -// : Thing(participant, Type::TemperatureSensor, thingId) {} - -// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {} - -TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {} - -// TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {} +TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent) { + this->type = Type::TemperatureSensor; +} void TemperatureSensor::SetTemperature(float temp) { this->temperature = temp; diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index d1b9b41..85d9bbe 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -2,7 +2,8 @@ namespace RoboidControl { -TouchSensor::TouchSensor(Thing* parent) : Thing(Type::TouchSensor, parent) { +TouchSensor::TouchSensor(Thing* parent) : Thing(parent) { + this->type = Type::TouchSensor; this->name = "Touch sensor"; } From be95dbeedc0bef3d3c82afed8ecc32d89b2800d1 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 2 Jun 2025 12:39:47 +0200 Subject: [PATCH 38/56] Renamed thing type incremental to relative encoder --- Thing.h | 2 +- Things/RelativeEncoder.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Thing.h b/Thing.h index bce0053..cb63c4f 100644 --- a/Thing.h +++ b/Thing.h @@ -32,7 +32,7 @@ class Thing { static const unsigned char ControlledMotor = 0x06; static const unsigned char UncontrolledMotor = 0x07; static const unsigned char Servo = 0x08; - static const unsigned char IncrementalEncoder = 0x19; + static const unsigned char RelativeEncoder = 0x19; // Other static const unsigned char Root = 0x10; static const unsigned char Roboid = 0x09; diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp index 573f84b..4fbef8c 100644 --- a/Things/RelativeEncoder.cpp +++ b/Things/RelativeEncoder.cpp @@ -3,7 +3,7 @@ namespace RoboidControl { RelativeEncoder::RelativeEncoder(Thing* parent) : Thing(parent) { - this->type = Type::IncrementalEncoder; + this->type = Type::RelativeEncoder; } float RelativeEncoder::GetRotationSpeed() { From c24925a4da9fd62e993cb24783ebb1fc71072599 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 4 Jun 2025 10:58:05 +0200 Subject: [PATCH 39/56] Compatibility fixes --- Arduino/Things/DRV8833.cpp | 4 +++- Arduino/Things/DigitalInput.cpp | 5 +++-- Arduino/Things/UltrasonicSensor.cpp | 3 ++- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 59a657d..ed04d79 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -7,7 +7,9 @@ namespace Arduino { #pragma region DRV8833 -DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) { +DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) { + this->type = Type::Undetermined; + this->name = "DRV8833"; this->pinStandby = config.standby; if (pinStandby != 255) pinMode(pinStandby, OUTPUT); diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index ef41ad9..4f379cd 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -7,8 +7,9 @@ namespace Arduino { #pragma region Digital input -DigitalInput::DigitalInput(unsigned char pin, Thing* parent) - : Thing(Type::Undetermined, parent) { +DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(parent) { + this->type = Type::Switch; + this->name = "Digital Input"; this->pin = pin; pinMode(this->pin, INPUT); std::cout << "digital input start\n"; diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 2568070..1ec4db5 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -7,7 +7,8 @@ namespace RoboidControl { namespace Arduino { UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) - : Thing(Type::Undetermined, parent) { + : Thing(parent) { + this->type = Type::DistanceSensor; this->name = "Ultrasonic sensor"; this->pinTrigger = config.trigger; this->pinEcho = config.echo; From e36f3bb45dae560125e26e0b1d4021cbd61a23b7 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 4 Jun 2025 11:49:47 +0200 Subject: [PATCH 40/56] Alignment with C# --- Participant.cpp | 12 +- Participant.h | 55 ++++-- Thing.cpp | 470 +++++++++++++++++++++--------------------------- Thing.h | 53 +++--- 4 files changed, 276 insertions(+), 314 deletions(-) diff --git a/Participant.cpp b/Participant.cpp index 549f78a..7bbafa2 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -16,17 +16,13 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) { } Participant::Participant() { - //std::cout << "P\n"; - //this->root.name = "Isolated"; - this->root = new Thing(this); - this->root->name = "Root"; - this->Add(this->root); + Thing::CreateRoot(this); + //this->Add(this->root); } Participant::Participant(const char* ipAddress, int port) { - // Add the root thing to the list of things, because we could not do that - // earlier (I think) - this->Add(this->root); + Thing::CreateRoot(this); + //this->Add(this->root); // make a copy of the ip address string int addressLength = (int)strlen(ipAddress); diff --git a/Participant.h b/Participant.h index ea1cdcf..c653c67 100644 --- a/Participant.h +++ b/Participant.h @@ -59,30 +59,48 @@ class ParticipantRegistry { /// participant. It is used as a basis for the local participant, but also as a /// reference to remote participants. class Participant { +#pragma region Init + + public: + /// @brief Create a generic participant + Participant(); + /// @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 + Participant(const char* ipAddress, int port); + /// @brief Destructor for the participant + ~Participant(); + + /// @brief The local participant for this application + static Participant* LocalParticipant; + /// @brief Replace the local participant + /// @param newParticipant The new local Participant + static void ReplaceLocalParticipant(Participant& newParticipant); + +#pragma endregion Init + +#pragma region Properties + public: /// @brief The name of the participant const char* name = "Participant"; /// @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; /// @brief The network Id to identify the participant unsigned char networkId = 0; - Participant(); - /// @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 - Participant(const char* ipAddress, int port); - /// @brief Destructor for the participant - ~Participant(); - - static Participant* LocalParticipant; - static void ReplaceLocalParticipant(Participant& newParticipant); - - Thing* root = new Thing(this); + /// @brief The root thing for this participant + Thing* root = nullptr; public: #if defined(NO_STD) @@ -104,12 +122,23 @@ class Participant { /// @param thing The thing to remove void Remove(Thing* thing); +#pragma endregion Properties + +#pragma region Update + + public: /// @brief Update all things for this participant - /// @param currentTimeMs The current time in milliseconds (optional) virtual void Update(); +#pragma endregion Update + +#pragma region Participant Registry + public: static ParticipantRegistry registry; + +#pragma endregion Participant Registry + }; } // namespace RoboidControl diff --git a/Thing.cpp b/Thing.cpp index 29e4f95..5f9694c 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -16,337 +16,277 @@ #include #endif -namespace RoboidControl -{ +namespace RoboidControl { #pragma region Init - Thing *Thing::LocalRoot() - { - Participant *p = Participant::LocalParticipant; - Thing *localRoot = p->root; - return localRoot; - } +Thing* Thing::LocalRoot() { + Participant* p = Participant::LocalParticipant; + Thing* localRoot = p->root; + return localRoot; +} - // Only use this for root things - Thing::Thing(Participant *owner) - { - this->type = Type::Root; // should become root +// Only use this for root things +Thing::Thing(Participant* owner) { + this->type = Type::Root; + this->name = "Root"; - this->position = Spherical::zero; - this->positionUpdated = true; - this->orientation = SwingTwist::identity; - this->orientationUpdated = true; - this->hierarchyChanged = true; + this->position = Spherical::zero; + this->positionUpdated = true; + this->orientation = SwingTwist::identity; + this->orientationUpdated = true; + this->hierarchyChanged = true; - this->linearVelocity = Spherical::zero; - this->angularVelocity = Spherical::zero; + this->linearVelocity = Spherical::zero; + this->angularVelocity = Spherical::zero; - this->owner = owner; - // this->owner->Add(this, true); - //std::cout << this->owner->name << ": New root thing " << std::endl; - } + this->owner = owner; + this->owner->Add(this); + // std::cout << this->owner->name << ": New root thing " << std::endl; +} - Thing::Thing(Thing *parent) - { - this->type = Type::Undetermined; +void Thing::CreateRoot(Participant* owner) { + owner->root = new Thing(owner); +} - this->position = Spherical::zero; - this->positionUpdated = true; - this->orientation = SwingTwist::identity; - this->orientationUpdated = true; - this->hierarchyChanged = true; +Thing::Thing(Thing* parent) { + this->type = Type::Undetermined; - this->linearVelocity = Spherical::zero; - this->angularVelocity = Spherical::zero; + this->position = Spherical::zero; + this->positionUpdated = true; + this->orientation = SwingTwist::identity; + this->orientationUpdated = true; + this->hierarchyChanged = true; - this->owner = parent->owner; - this->owner->Add(this, true); - this->SetParent(parent); + this->linearVelocity = Spherical::zero; + this->angularVelocity = Spherical::zero; - std::cout << this->owner->name << ": New thing for " << parent->name - << std::endl; - } + this->owner = parent->owner; + this->owner->Add(this, true); + this->SetParent(parent); - Thing::~Thing() - { - std::cout << "Destroy thing " << this->name << std::endl; - } + std::cout << this->owner->name << ": New thing for " << parent->name + << std::endl; +} + +Thing::~Thing() { + std::cout << "Destroy thing " << this->name << std::endl; +} #pragma endregion Init - void Thing::SetName(const char *name) - { - this->name = name; - this->nameChanged = true; - } +void Thing::SetName(const char* name) { + this->name = name; + this->nameChanged = true; +} - const char *Thing::GetName() const - { - return this->name; - } +const char* Thing::GetName() const { + return this->name; +} - void Thing::SetModel(const char *url) - { - this->modelUrl = url; - } +void Thing::SetModel(const char* url) { + this->modelUrl = url; +} #pragma region Hierarchy - void Thing::SetParent(Thing *parent) - { - if (parent == nullptr) - { - Thing *parentThing = this->parent; - if (parentThing != nullptr) - parentThing->RemoveChild(this); - this->parent = nullptr; +void Thing::SetParent(Thing* parent) { + if (parent == nullptr) { + Thing* parentThing = this->parent; + if (parentThing != nullptr) + parentThing->RemoveChild(this); + this->parent = nullptr; + } else + parent->AddChild(this); + this->hierarchyChanged = true; +} + +bool Thing::IsRoot() const { + return this == LocalRoot() || this->parent == nullptr; +} + +Thing* Thing::GetParent() { + return this->parent; +} + +Thing* Thing::GetChildByIndex(unsigned char ix) { + return this->children[ix]; +} + +void Thing::AddChild(Thing* child) { + unsigned char newChildCount = this->childCount + 1; + Thing** newChildren = new Thing*[newChildCount]; + + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + newChildren[childIx] = this->children[childIx]; + if (this->children[childIx] == child) { + // child is already present, stop copying do not update the children + delete[] newChildren; + return; } - else - parent->AddChild(this); - this->hierarchyChanged = true; } - // void Thing::SetParent(Thing* parent) { - // parent->AddChild(this); - // this->hierarchyChanged = true; - // } - - // const Thing& Thing::GetParent() { - // return *this->parent; - // } - - bool Thing::IsRoot() const - { - return this == LocalRoot() || this->parent == nullptr; //&Thing::Root; - } - - // void Thing::SetParent(Thing* root, const char* name) { - // Thing* thing = root->FindChild(name); - // if (thing != nullptr) - // this->SetParent(thing); - // } - - Thing *Thing::GetParent() - { - return this->parent; - } - - Thing *Thing::GetChildByIndex(unsigned char ix) - { - return this->children[ix]; - } - - void Thing::AddChild(Thing *child) - { - unsigned char newChildCount = this->childCount + 1; - Thing **newChildren = new Thing *[newChildCount]; - - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) - { - newChildren[childIx] = this->children[childIx]; - if (this->children[childIx] == child) - { - // child is already present, stop copying do not update the children - delete[] newChildren; - return; - } - } - - newChildren[this->childCount] = child; - child->parent = this; - - if (this->children != nullptr) - delete[] this->children; - - this->children = newChildren; - this->childCount = newChildCount; - } - - Thing *Thing::RemoveChild(Thing *child) - { - unsigned char newChildCount = this->childCount - 1; - Thing **newChildren = new Thing *[newChildCount]; - - unsigned char newChildIx = 0; - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) - { - if (this->children[childIx] != child) - { - if (newChildIx == newChildCount) - { // We did not find the child - // stop copying and return nothing - delete[] newChildren; - return nullptr; - } - else - newChildren[newChildIx++] = this->children[childIx]; - } - } - - child->parent = Thing::LocalRoot(); + newChildren[this->childCount] = child; + child->parent = this; + if (this->children != nullptr) delete[] this->children; - this->children = newChildren; - this->childCount = newChildCount; - return child; - } + this->children = newChildren; + this->childCount = newChildCount; +} - Thing *Thing::GetChild(unsigned char id, bool recurse) - { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) - { - Thing *child = this->children[childIx]; - if (child == nullptr) - continue; - if (child->id == id) - return child; +Thing* Thing::RemoveChild(Thing* child) { + unsigned char newChildCount = this->childCount - 1; + Thing** newChildren = new Thing*[newChildCount]; - if (recurse) - { - Thing *foundChild = child->GetChild(id, recurse); - if (foundChild != nullptr) - return foundChild; - } + unsigned char newChildIx = 0; + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + if (this->children[childIx] != child) { + if (newChildIx == newChildCount) { // We did not find the child + // stop copying and return nothing + delete[] newChildren; + return nullptr; + } else + newChildren[newChildIx++] = this->children[childIx]; } - return nullptr; } - Thing *Thing::FindChild(const char *name, bool recurse) - { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) - { - Thing *child = this->children[childIx]; - if (child == nullptr || child->name == nullptr) - continue; + child->parent = Thing::LocalRoot(); - if (strcmp(child->name, name) == 0) - return child; + delete[] this->children; + this->children = newChildren; + this->childCount = newChildCount; - Thing *foundChild = child->FindChild(name); + return child; +} + +Thing* Thing::GetChild(unsigned char id, bool recurse) { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + Thing* child = this->children[childIx]; + if (child == nullptr) + continue; + if (child->id == id) + return child; + + if (recurse) { + Thing* foundChild = child->GetChild(id, recurse); if (foundChild != nullptr) return foundChild; } - return nullptr; } + return nullptr; +} + +Thing* Thing::FindChild(const char* name, bool recurse) { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + Thing* child = this->children[childIx]; + if (child == nullptr || child->name == nullptr) + continue; + + if (strcmp(child->name, name) == 0) + return child; + + Thing* foundChild = child->FindChild(name); + if (foundChild != nullptr) + return foundChild; + } + return nullptr; +} #pragma endregion Hierarchy #pragma region Pose - void Thing::SetPosition(Spherical position) - { - this->position = position; - this->positionUpdated = true; - } - Spherical Thing::GetPosition() - { - return this->position; - } +void Thing::SetPosition(Spherical position) { + this->position = position; + this->positionUpdated = true; +} +Spherical Thing::GetPosition() { + return this->position; +} - void Thing::SetOrientation(SwingTwist orientation) - { - this->orientation = orientation; - this->orientationUpdated = true; - } +void Thing::SetOrientation(SwingTwist orientation) { + this->orientation = orientation; + this->orientationUpdated = true; +} - SwingTwist Thing::GetOrientation() - { - return this->orientation; - } +SwingTwist Thing::GetOrientation() { + return this->orientation; +} - void Thing::SetLinearVelocity(Spherical linearVelocity) - { - if (this->linearVelocity.distance != linearVelocity.distance) - { - this->linearVelocity = linearVelocity; - this->linearVelocityUpdated = true; - } +void Thing::SetLinearVelocity(Spherical linearVelocity) { + if (this->linearVelocity.distance != linearVelocity.distance) { + this->linearVelocity = linearVelocity; + this->linearVelocityUpdated = true; } +} - Spherical Thing::GetLinearVelocity() - { - return this->linearVelocity; - } +Spherical Thing::GetLinearVelocity() { + return this->linearVelocity; +} - void Thing::SetAngularVelocity(Spherical angularVelocity) - { - if (this->angularVelocity.distance != angularVelocity.distance) - { - this->angularVelocity = angularVelocity; - this->angularVelocityUpdated = true; - } +void Thing::SetAngularVelocity(Spherical angularVelocity) { + if (this->angularVelocity.distance != angularVelocity.distance) { + this->angularVelocity = angularVelocity; + this->angularVelocityUpdated = true; } +} - Spherical Thing::GetAngularVelocity() - { - return this->angularVelocity; - } +Spherical Thing::GetAngularVelocity() { + return this->angularVelocity; +} #pragma endregion Pose #pragma region Update - unsigned long Thing::GetTimeMs() - { +unsigned long Thing::GetTimeMs() { #if defined(ARDUINO) - unsigned long ms = millis(); - return ms; + unsigned long ms = millis(); + return ms; #else - auto now = std::chrono::steady_clock::now(); - auto ms = std::chrono::duration_cast( - now.time_since_epoch()); - return static_cast(ms.count()); + auto now = std::chrono::steady_clock::now(); + auto ms = std::chrono::duration_cast( + now.time_since_epoch()); + return static_cast(ms.count()); #endif - } +} - // void Thing::Update(bool recursive) { - // Update(GetTimeMs(), recursive); - // } +// void Thing::Update(bool recursive) { +// Update(GetTimeMs(), recursive); +// } - void Thing::PrepareForUpdate() {} +void Thing::PrepareForUpdate() {} - void Thing::Update(bool recursive) - { - // if (this->positionUpdated || this->orientationUpdated) - // OnPoseChanged callback - this->positionUpdated = false; - this->orientationUpdated = false; - // this->linearVelocityUpdated = false; - // this->angularVelocityUpdated = false; - this->hierarchyChanged = false; - this->nameChanged = false; +void Thing::Update(bool recursive) { + this->positionUpdated = false; + this->orientationUpdated = false; + this->linearVelocityUpdated = false; + this->angularVelocityUpdated = false; + this->hierarchyChanged = false; + this->nameChanged = false; - if (recursive) - { - // std::cout << "# children: " << (int)this->childCount << std::endl; - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) - { - Thing *child = this->children[childIx]; - if (child == nullptr) - continue; - child->Update(recursive); - } + if (recursive) { + // std::cout << "# children: " << (int)this->childCount << std::endl; + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + Thing* child = this->children[childIx]; + if (child == nullptr) + continue; + child->Update(recursive); } } - - void Thing::UpdateThings() - { - IsolatedParticipant::Isolated()->Update(); - } +} #pragma endregion Update - int Thing::GenerateBinary(char *buffer, unsigned char *ix) - { - (void)buffer; - (void)ix; - return 0; - } - void Thing::ProcessBinary(char *bytes) - { - (void)bytes; - }; +int Thing::GenerateBinary(char* buffer, unsigned char* ix) { + (void)buffer; + (void)ix; + return 0; +} +void Thing::ProcessBinary(char* bytes) { + (void)bytes; +}; -} // namespace RoboidControl \ No newline at end of file +} // namespace RoboidControl \ No newline at end of file diff --git a/Thing.h b/Thing.h index cb63c4f..e425f9b 100644 --- a/Thing.h +++ b/Thing.h @@ -43,32 +43,35 @@ class Thing { }; #pragma region Init - static Thing* LocalRoot(); private: - // Special constructor to create a root thing - Thing(Participant* parent); - // Which can only be used by the Participant - friend class Participant; - public: - /// @brief Create a new thing - /// @param thingType The type of thing (can use Thing::Type) + /// @brief Create a new Thing /// @param parent (optional) The parent thing /// The owner will be the same as the owner of the parent thing, it will /// be Participant::LocalParticipant if the parent is not specified. A thing - /// without a parent will be a root thing. + /// without a parent will be connected to the root thing. Thing(Thing* parent = LocalRoot()); - /// @brief Create a new child thing - /// @param parent The parent thing - /// @param thingType The type of thing (can use Thing::Type) - /// @param thingId The ID of the thing, leave out or set to zero to generate - /// an ID - /// @note The owner will be the same as the owner of the parent thing + private: + /// @brief Constructor to create a root thing + /// @param owner The participant who will own this root thing + /// @remarks This function is private because CreateRoot() should be used + /// instead + Thing(Participant* owener); + public: + /// @brief Destructor for a Thing ~Thing(); + /// @brief Create a root thing for a participant + /// @param owner The participant who will own this root thing + static void CreateRoot(Participant* owner); + + /// @brief The root thing for the local participant + /// @return The root thing for the local participant + static Thing* LocalRoot(); + #pragma endregion Init public: @@ -77,9 +80,7 @@ class Thing { #pragma region Properties - /// @brief The participant managing this thing - Participant* owner = nullptr; - + public: /// @brief The ID of the thing unsigned char id = 0; @@ -87,10 +88,12 @@ class Thing { /// This can be either a Thing::Type of a byte value for custom types unsigned char type = Type::Undetermined; + /// @brief The participant owning this thing + Participant* owner = nullptr; + /// @brief The name of the thing const char* name = nullptr; - public: void SetName(const char* name); const char* GetName() const; bool nameChanged = false; @@ -99,14 +102,12 @@ class Thing { /// loaded from /// @param url The url of the model /// @remark Although the roboid implementation is not dependent on the model, - /// the only official supported model format is .obj + /// the only official supported model formats are .png (sprite), .gltf and .glb void SetModel(const char* url); /// @brief An URL pointing to the location where a model of the thing can be /// found const char* modelUrl = nullptr; - /// @brief The scale of the model (deprecated I think) - float modelScale = 1; #pragma endregion Properties @@ -114,13 +115,13 @@ class Thing { /// @brief Sets the parent of this Thing /// @param parent The Thing which should become the parent - // virtual void SetParent(Thing* parent); void SetParent(Thing* parent); /// @brief Gets the parent of this Thing /// @return The parent Thing - // Thing* GetParent(); Thing* GetParent(); + /// @brief Check if this is a root thing + /// @return True is this thing is a root bool IsRoot() const; /// @brief The number of children @@ -225,13 +226,9 @@ class Thing { virtual void PrepareForUpdate(); /// @brief Updates the state of the thing - /// @param currentTimeMs The current clock time in milliseconds; if this is - /// zero, the current time is retrieved automatically /// @param recurse When true, this will Update the descendants recursively virtual void Update(bool recurse = false); - static void UpdateThings(); - /// @brief Get the current time in milliseconds /// @return The current time in milliseconds static unsigned long GetTimeMs(); From 2a83dbe7ca944014eaa5c150c9f025a42dc88d66 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 4 Jun 2025 12:17:30 +0200 Subject: [PATCH 41/56] Alignment with C# --- Participant.cpp | 32 +++++++++ Participant.h | 11 ++- Participants/ParticipantUDP.cpp | 124 +++++++++++++++++++------------- Participants/ParticipantUDP.h | 20 +++--- Participants/SiteServer.cpp | 6 +- Participants/SiteServer.h | 10 +-- 6 files changed, 135 insertions(+), 68 deletions(-) diff --git a/Participant.cpp b/Participant.cpp index 7bbafa2..bd03908 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -1,6 +1,10 @@ #include "Participant.h" #include +#include "Arduino/ArduinoParticipant.h" +#include "EspIdf/EspIdfParticipant.h" +#include "Posix/PosixParticipant.h" +#include "Windows/WindowsParticipant.h" namespace RoboidControl { @@ -52,6 +56,34 @@ void Participant::Update() { } } +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 " << remoteParticipant->ipAddress << std::endl; + +#if defined(_WIN32) || defined(_WIN64) + Windows::ParticipantUDP* thisWindows = + static_cast(this); + return thisWindows->Send(remoteParticipant, bufferSize); +#elif defined(__unix__) || defined(__APPLE__) + Posix::ParticipantUDP* thisPosix = static_cast(this); + return thisPosix->Send(remoteParticipant, bufferSize); +#elif defined(ARDUINO) + Arduino::ParticipantUDP* thisArduino = + static_cast(this); + return thisArduino->Send(this, bufferSize); +#elif defined(IDF_VER) + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); + return thisEspIdf->Send(remoteParticipant, bufferSize); +#else + return false; +#endif +} + Thing* Participant::Get(unsigned char thingId) { for (Thing* thing : this->things) { if (thing->id == thingId) diff --git a/Participant.h b/Participant.h index c653c67..6e1217d 100644 --- a/Participant.h +++ b/Participant.h @@ -1,5 +1,6 @@ #pragma once +#include "Messages/IMessage.h" #include "Thing.h" namespace RoboidControl { @@ -132,13 +133,21 @@ class Participant { #pragma endregion Update +#pragma region Send + + public: + char buffer[1024]; + + virtual bool Send(IMessage* msg); + +#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 2ec2f88..89682ea 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -46,14 +46,6 @@ ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) Participant::ReplaceLocalParticipant(*this); } -static ParticipantUDP* isolatedParticipant = nullptr; - -ParticipantUDP* ParticipantUDP::Isolated() { - if (isolatedParticipant == nullptr) - isolatedParticipant = new ParticipantUDP(0); - return isolatedParticipant; -} - void ParticipantUDP::begin() { if (this->isIsolated || this->remoteSite == nullptr) return; @@ -105,7 +97,8 @@ void ParticipantUDP::Update() { if (this->remoteSite == nullptr) this->Publish(msg); else - this->Send(this->remoteSite, msg); + this->remoteSite->Send(msg); + delete msg; this->nextPublishMe = currentTimeMs + this->publishInterval; @@ -122,7 +115,7 @@ void ParticipantUDP::PrepMyThings() { for (Thing* thing : this->things) { if (thing == nullptr) continue; - + thing->PrepareForUpdate(); } } @@ -137,12 +130,12 @@ void ParticipantUDP::UpdateMyThings() { if (thing->hierarchyChanged) { if (!(this->isIsolated || this->networkId == 0)) { ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); - this->Send(this->remoteSite, thingMsg); + this->remoteSite->Send(thingMsg); delete thingMsg; if (thing->nameChanged) { NameMsg* nameMsg = new NameMsg(this->networkId, thing); - this->Send(this->remoteSite, nameMsg); + this->remoteSite->Send(nameMsg); delete nameMsg; } } @@ -159,20 +152,20 @@ void ParticipantUDP::UpdateMyThings() { if (!(this->isIsolated || this->networkId == 0)) { if (thing->terminate) { DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); - this->Send(this->remoteSite, destroyMsg); + this->remoteSite->Send(destroyMsg); delete destroyMsg; } else { // Send to remote site if (thing->nameChanged) { NameMsg* nameMsg = new NameMsg(this->networkId, thing); - this->Send(this->remoteSite, nameMsg); + this->remoteSite->Send(nameMsg); delete nameMsg; } PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - this->Send(this->remoteSite, poseMsg); + this->remoteSite->Send(poseMsg); delete poseMsg; BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); - this->Send(this->remoteSite, binaryMsg); + this->remoteSite->Send(binaryMsg); delete binaryMsg; } } @@ -203,10 +196,10 @@ void ParticipantUDP::UpdateOtherThings() { for (Thing* thing : participant->things) { PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing); - this->Send(participant, poseMsg); + participant->Send(poseMsg); delete poseMsg; BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing); - this->Send(participant, binaryMsg); + participant->Send(binaryMsg); delete binaryMsg; } } @@ -221,49 +214,50 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, Thing* thing) { // std::cout << "Send thing info [" << (int)thing->id << "] \n"; ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); - this->Send(remoteParticipant, thingMsg); + remoteParticipant->Send(thingMsg); delete thingMsg; NameMsg* nameMsg = new NameMsg(this->networkId, thing); - this->Send(remoteParticipant, nameMsg); + remoteParticipant->Send(nameMsg); delete nameMsg; ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing); - this->Send(remoteParticipant, modelMsg); + remoteParticipant->Send(modelMsg); delete modelMsg; PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true); - this->Send(remoteParticipant, poseMsg); + remoteParticipant->Send(poseMsg); delete poseMsg; - BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing); - this->Send(remoteParticipant, customMsg); - delete customMsg; + BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); + remoteParticipant->Send(binaryMsg); + delete binaryMsg; } -bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { - int bufferSize = msg->Serialize(this->buffer); - if (bufferSize <= 0) - return true; +// bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { +// int bufferSize = msg->Serialize(this->buffer); +// if (bufferSize <= 0) +// return true; - // std::cout << "send msg " << (static_cast(this->buffer[0]) & 0xff) - // << " to " << remoteParticipant->ipAddress << std::endl; +// // std::cout << "send msg " << (static_cast(this->buffer[0]) & 0xff) +// // << " to " << remoteParticipant->ipAddress << std::endl; -#if defined(_WIN32) || defined(_WIN64) - Windows::ParticipantUDP* thisWindows = - static_cast(this); - return thisWindows->Send(remoteParticipant, bufferSize); -#elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = static_cast(this); - return thisPosix->Send(remoteParticipant, bufferSize); -#elif defined(ARDUINO) - Arduino::ParticipantUDP* thisArduino = - static_cast(this); - return thisArduino->Send(remoteParticipant, bufferSize); -#elif defined(IDF_VER) - EspIdf::ParticipantUDP* thisEspIdf = - static_cast(this); - return thisEspIdf->Send(remoteParticipant, bufferSize); -#else - return false; -#endif -} +// #if defined(_WIN32) || defined(_WIN64) +// Windows::ParticipantUDP* thisWindows = +// static_cast(this); +// return thisWindows->Send(remoteParticipant, bufferSize); +// #elif defined(__unix__) || defined(__APPLE__) +// Posix::ParticipantUDP* thisPosix = +// static_cast(this); return +// thisPosix->Send(remoteParticipant, bufferSize); +// #elif defined(ARDUINO) +// Arduino::ParticipantUDP* thisArduino = +// static_cast(this); +// return thisArduino->Send(remoteParticipant, bufferSize); +// #elif defined(IDF_VER) +// EspIdf::ParticipantUDP* thisEspIdf = +// static_cast(this); +// return thisEspIdf->Send(remoteParticipant, bufferSize); +// #else +// return false; +// #endif +// } void ParticipantUDP::PublishThingInfo(Thing* thing) { // std::cout << "Publish thing info" << thing->networkId << "\n"; @@ -402,6 +396,18 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize, Process(sender, msg); delete msg; } break; + case TextMsg::id: { + TextMsg* msg = new TextMsg(this->buffer); + bufferSize -= msg->length + msg->textLength; + Process(sender, msg); + delete msg; + } break; + case DestroyMsg::id: { + DestroyMsg* msg = new DestroyMsg(this->buffer); + bufferSize -= msg->length; + Process(sender, msg); + delete msg; + } break; }; // Check if the buffer has been read completely @@ -532,6 +538,24 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { } } +void ParticipantUDP::Process(Participant* sender, TextMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " " + << (int)msg->text << "\n"; +#endif +} + +void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/" + << (int)msg->thingId << "]\n"; +#endif + + Thing* thing = sender->Get(msg->thingId); + if (thing != nullptr) + this->Remove(thing); +} + // Receive #pragma endregion diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index bf5e1a2..9d92251 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -9,6 +9,7 @@ #include "Messages/PoseMsg.h" #include "Messages/NetworkIdMsg.h" #include "Messages/ThingMsg.h" +#include "Messages/TextMsg.h" #include "Participant.h" #if !defined(NO_STD) @@ -43,6 +44,7 @@ constexpr int MAX_SENDER_COUNT = 256; /// RoboidControl::IsolatedParticipant::Isolated(). /// @sa RoboidControl::Thing::Thing() class ParticipantUDP : public Participant { + #pragma region Init public: @@ -58,19 +60,15 @@ class ParticipantUDP : public Participant { /// @param localPort The port used by the local participant ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681); - /// @brief Isolated participant is used when the application is run without - /// networking - /// @return A participant without networking support - static ParticipantUDP* Isolated(); - - /// @brief True if the participant is running isolated. - /// Isolated participants do not communicate with other participants - #pragma endregion Init +#pragma region Properties + +public: /// @brief True if the participant is running isolated. /// Isolated participants do not communicate with other participants bool isIsolated = false; + /// @brief The remote site when this participant is connected to a site Participant* remoteSite = nullptr; @@ -94,6 +92,8 @@ class ParticipantUDP : public Participant { void begin(); bool connected = false; +#pragma endregion Properties + #pragma region Update public: @@ -114,7 +114,7 @@ class ParticipantUDP : public Participant { void SendThingInfo(Participant* remoteParticipant, Thing* thing); void PublishThingInfo(Thing* thing); - bool Send(Participant* remoteParticipant, IMessage* msg); + //bool Send(Participant* remoteParticipant, IMessage* msg); bool Publish(IMessage* msg); #pragma endregion Send @@ -139,6 +139,8 @@ protected: virtual void Process(Participant* sender, ModelUrlMsg* msg); virtual void Process(Participant* sender, PoseMsg* msg); virtual void Process(Participant* sender, BinaryMsg* msg); + virtual void Process(Participant* sender, TextMsg* msg); + virtual void Process(Participant* sender, DestroyMsg* msg); #pragma endregion Receive diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 9eb7ce9..cc61639 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -42,10 +42,10 @@ void SiteServer::UpdateMyThings() { continue; PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - this->Send(participant, poseMsg); + participant->Send(poseMsg); delete poseMsg; BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); - this->Send(participant, binaryMsg); + participant->Send(binaryMsg); delete binaryMsg; } } @@ -62,7 +62,7 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { // sender->ipAddress // << ":" << (int)sender->port << "\n"; NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId); - this->Send(sender, msg); + sender->Send(msg); delete msg; } } diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index 67bc5bf..87029c5 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -2,11 +2,11 @@ #include "ParticipantUDP.h" -#if !defined(NO_STD) -#include -#include -#include -#endif +// #if !defined(NO_STD) +// #include +// #include +// #include +// #endif namespace RoboidControl { From b8eb9bb712d72bca8c86c3e67cfc8d19386a9535 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 6 Jun 2025 09:36:24 +0200 Subject: [PATCH 42/56] Improved touch sensor --- Arduino/Things/DigitalInput.cpp | 6 +++--- Arduino/Things/UltrasonicSensor.cpp | 2 +- Things/TouchSensor.cpp | 9 +++++++-- Things/TouchSensor.h | 3 ++- examples/BB2B.cpp | 4 ++-- 5 files changed, 15 insertions(+), 9 deletions(-) diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index 4f379cd..e65aa1e 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -29,7 +29,7 @@ DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent) : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {} void DigitalInput::TouchSensor::Update(bool recursive) { - this->touchedSomething = digitalInput.isLow; + this->internalTouch = digitalInput.isLow; } #pragma endregion Touch sensor @@ -98,8 +98,8 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) { this->pulseFrequency = pulseCount / timeStep; this->rotationSpeed = pulseFrequency / pulsesPerRevolution; - std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency - << " timestep " << timeStep << std::endl; + // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency + // << " timestep " << timeStep << std::endl; this->lastUpdateTime = currentTimeMs; } diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 1ec4db5..338353e 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -65,7 +65,7 @@ UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent) void UltrasonicSensor::TouchSensor::Update(bool recursive) { RoboidControl::TouchSensor::Update(recursive); this->ultrasonic.Update(false); - this->touchedSomething |= (this->ultrasonic.distance > 0 && + this->internalTouch |= (this->ultrasonic.distance > 0 && this->ultrasonic.distance <= this->touchDistance); } diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index 85d9bbe..9d347db 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -7,8 +7,12 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent) { this->name = "Touch sensor"; } +bool TouchSensor::IsTouching() { + return this->internalTouch || this->externalTouch; +} + void TouchSensor::PrepareForUpdate() { - this->touchedSomething = this->externalTouch; + //this->internalTouch = this->externalTouch; } void TouchSensor::Update(bool recursive) { @@ -16,7 +20,8 @@ void TouchSensor::Update(bool recursive) { } int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { - bytes[(*ix)++] = this->touchedSomething ? 1 : 0; + std::cout << "BinaryMsg Touch " << this->internalTouch << std::endl; + bytes[(*ix)++] = this->internalTouch ? 1 : 0; return 1; } diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index cd7bd73..e8eb2bc 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -16,7 +16,7 @@ class TouchSensor : public Thing { /// @brief Value which is true when the sensor is touching something, false /// otherwise - bool touchedSomething = false; + bool IsTouching(); virtual void PrepareForUpdate() override; virtual void Update(bool recursive) override; @@ -30,6 +30,7 @@ class TouchSensor : public Thing { virtual void ProcessBinary(char* bytes) override; protected: bool externalTouch = false; + bool internalTouch = false; }; } // namespace RoboidControl diff --git a/examples/BB2B.cpp b/examples/BB2B.cpp index b1ab231..8a51d01 100644 --- a/examples/BB2B.cpp +++ b/examples/BB2B.cpp @@ -27,10 +27,10 @@ int main() { while (true) { // The left wheel turns forward when nothing is touched on the right side // and turn backward when the roboid hits something on the right - float leftWheelSpeed = (touchRight.touchedSomething) ? -600.0f : 600.0f; + float leftWheelSpeed = (touchRight.internalTouch) ? -600.0f : 600.0f; // The right wheel does the same, but instead is controlled by // touches on the left side - float rightWheelSpeed = (touchLeft.touchedSomething) ? -600.0f : 600.0f; + float rightWheelSpeed = (touchLeft.internalTouch) ? -600.0f : 600.0f; // When both sides are touching something, both wheels will turn backward // and the roboid will move backwards bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed); From 9a2ce4dd5e3e5b3df6f100324edaac40826e7c5c Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 6 Jun 2025 09:36:42 +0200 Subject: [PATCH 43/56] use SwingTwist for orientation --- Messages/LowLevelMessages.cpp | 130 ++++++++++++++++++++-------------- Messages/LowLevelMessages.h | 4 ++ Messages/PoseMsg.cpp | 4 +- 3 files changed, 83 insertions(+), 55 deletions(-) diff --git a/Messages/LowLevelMessages.cpp b/Messages/LowLevelMessages.cpp index a186511..4526a04 100644 --- a/Messages/LowLevelMessages.cpp +++ b/Messages/LowLevelMessages.cpp @@ -5,6 +5,83 @@ namespace RoboidControl { +void LowLevelMessages::SendSpherical(char* buffer, + unsigned char* ix, + Spherical s) { + SendFloat16(buffer, ix, s.distance); + SendAngle8(buffer, ix, s.direction.horizontal.InDegrees()); + SendAngle8(buffer, ix, s.direction.vertical.InDegrees()); +} +Spherical LowLevelMessages::ReceiveSpherical(const char* buffer, + unsigned char* startIndex) { + float distance = ReceiveFloat16(buffer, startIndex); + + Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex); + Angle horizontal = Angle::Radians(horizontal8.InRadians()); + + Angle8 vertical8 = ReceiveAngle8(buffer, startIndex); + Angle vertical = Angle::Radians(vertical8.InRadians()); + + Spherical s = Spherical(distance, horizontal, vertical); + return s; +} + +void LowLevelMessages::SendSwingTwist(char* buffer, + unsigned char* ix, + SwingTwist s) { + SendAngle8(buffer, ix, s.swing.horizontal.InDegrees()); + SendAngle8(buffer, ix, s.swing.vertical.InDegrees()); + SendAngle8(buffer, ix, s.twist.InDegrees()); +} + +SwingTwist LowLevelMessages::ReceiveSwingTwist(const char* buffer, + unsigned char* startIndex) { + Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex); + Angle horizontal = Angle::Radians(horizontal8.InRadians()); + + Angle8 vertical8 = ReceiveAngle8(buffer, startIndex); + Angle vertical = Angle::Radians(vertical8.InRadians()); + + Angle8 twist8 = ReceiveAngle8(buffer, startIndex); + Angle twist = Angle::Radians(twist8.InRadians()); + + SwingTwist s = SwingTwist(horizontal, vertical, twist); + return s; +} + +void LowLevelMessages::SendQuat32(char* buffer, + unsigned char* ix, + SwingTwist rotation) { + Quaternion q = rotation.ToQuaternion(); + unsigned char qx = (char)(q.x * 127 + 128); + unsigned char qy = (char)(q.y * 127 + 128); + unsigned char qz = (char)(q.z * 127 + 128); + unsigned char qw = (char)(q.w * 255); + if (q.w < 0) { + qx = -qx; + qy = -qy; + qz = -qz; + qw = -qw; + } + // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw + // << "\n"; + buffer[(*ix)++] = qx; + buffer[(*ix)++] = qy; + buffer[(*ix)++] = qz; + buffer[(*ix)++] = qw; +} + +SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer, + unsigned char* ix) { + float qx = (buffer[(*ix)++] - 128.0F) / 127.0F; + float qy = (buffer[(*ix)++] - 128.0F) / 127.0F; + float qz = (buffer[(*ix)++] - 128.0F) / 127.0F; + float qw = buffer[(*ix)++] / 255.0F; + Quaternion q = Quaternion(qx, qy, qz, qw); + SwingTwist s = SwingTwist::FromQuaternion(q); + return s; +} + void LowLevelMessages::SendAngle8(char* buffer, unsigned char* ix, const float angle) { @@ -42,58 +119,5 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer, return (float)f.toFloat(); } -void LowLevelMessages::SendSpherical(char* buffer, - unsigned char* ix, - Spherical s) { - SendFloat16(buffer, ix, s.distance); - SendAngle8(buffer, ix, s.direction.horizontal.InDegrees()); - SendAngle8(buffer, ix, s.direction.vertical.InDegrees()); -} -Spherical LowLevelMessages::ReceiveSpherical(const char* buffer, - unsigned char* startIndex) { - float distance = ReceiveFloat16(buffer, startIndex); - - Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex); - Angle horizontal = Angle::Radians(horizontal8.InRadians()); - - Angle8 vertical8 = ReceiveAngle8(buffer, startIndex); - Angle vertical = Angle::Radians(vertical8.InRadians()); - - Spherical s = Spherical(distance, horizontal, vertical); - return s; -} - -void LowLevelMessages::SendQuat32(char* buffer, - unsigned char* ix, - SwingTwist rotation) { - Quaternion q = rotation.ToQuaternion(); - unsigned char qx = (char)(q.x * 127 + 128); - unsigned char qy = (char)(q.y * 127 + 128); - unsigned char qz = (char)(q.z * 127 + 128); - unsigned char qw = (char)(q.w * 255); - if (q.w < 0) { - qx = -qx; - qy = -qy; - qz = -qz; - qw = -qw; - } - // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw - // << "\n"; - buffer[(*ix)++] = qx; - buffer[(*ix)++] = qy; - buffer[(*ix)++] = qz; - buffer[(*ix)++] = qw; -} - -SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer, - unsigned char* ix) { - float qx = (buffer[(*ix)++] - 128.0F) / 127.0F; - float qy = (buffer[(*ix)++] - 128.0F) / 127.0F; - float qz = (buffer[(*ix)++] - 128.0F) / 127.0F; - float qw = buffer[(*ix)++] / 255.0F; - Quaternion q = Quaternion(qx, qy, qz, qw); - SwingTwist s = SwingTwist::FromQuaternion(q); - return s; -} } // namespace RoboidControl \ No newline at end of file diff --git a/Messages/LowLevelMessages.h b/Messages/LowLevelMessages.h index 144e50c..c21b32d 100644 --- a/Messages/LowLevelMessages.h +++ b/Messages/LowLevelMessages.h @@ -11,6 +11,10 @@ class LowLevelMessages { static Spherical ReceiveSpherical(const char* buffer, unsigned char* startIndex); + static void SendSwingTwist(char* buffer, unsigned char* ix, SwingTwist s); + static SwingTwist ReceiveSwingTwist(const char* buffer, + unsigned char* startIndex); + static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q); static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix); diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index 79d2e4a..aca128e 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) { this->thingId = buffer[ix++]; this->poseType = buffer[ix++]; this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix); - this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix); + this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix); // linearVelocity // angularVelocity } @@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) { if ((this->poseType & Pose_Position) != 0) LowLevelMessages::SendSpherical(buffer, &ix, this->position); if ((this->poseType & Pose_Orientation) != 0) - LowLevelMessages::SendQuat32(buffer, &ix, this->orientation); + LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation); if ((this->poseType & Pose_LinearVelocity) != 0) LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity); if ((this->poseType & Pose_AngularVelocity) != 0) From c2ac149df69dec193d6089ec2f39cb28081e35aa Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 6 Jun 2025 09:37:30 +0200 Subject: [PATCH 44/56] Optimise sending of messages --- Participant.cpp | 2 +- Participants/ParticipantUDP.cpp | 98 +++++++++++++-------------------- Participants/ParticipantUDP.h | 3 +- Things/ControlledMotor.cpp | 4 +- Things/Motor.cpp | 16 ++++++ Things/Motor.h | 4 +- 6 files changed, 59 insertions(+), 68 deletions(-) diff --git a/Participant.cpp b/Participant.cpp index bd03908..c3cf0ad 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -62,7 +62,7 @@ bool Participant::Send(IMessage* msg) { return true; // std::cout << "send msg " << (static_cast(this->buffer[0]) & 0xff) - // << " to " << remoteParticipant->ipAddress << std::endl; + // << " to " << this->ipAddress << std::endl; #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 89682ea..fc99aae 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -97,7 +97,7 @@ void ParticipantUDP::Update() { if (this->remoteSite == nullptr) this->Publish(msg); else - this->remoteSite->Send(msg); + this->Send(msg); delete msg; @@ -127,19 +127,19 @@ void ParticipantUDP::UpdateMyThings() { continue; // std::cout << thing->name << "\n"; - if (thing->hierarchyChanged) { - if (!(this->isIsolated || this->networkId == 0)) { - ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); - this->remoteSite->Send(thingMsg); - delete thingMsg; + // if (thing->hierarchyChanged) { + // if (!(this->isIsolated || this->networkId == 0)) { + // ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); + // this->remoteSite->Send(thingMsg); + // delete thingMsg; - if (thing->nameChanged) { - NameMsg* nameMsg = new NameMsg(this->networkId, thing); - this->remoteSite->Send(nameMsg); - delete nameMsg; - } - } - } + // if (thing->nameChanged) { + // NameMsg* nameMsg = new NameMsg(this->networkId, thing); + // this->remoteSite->Send(nameMsg); + // delete nameMsg; + // } + // } + // } // std::cout << "B\n"; // Why don't we do recursive? @@ -149,26 +149,26 @@ void ParticipantUDP::UpdateMyThings() { thing->Update(false); // std::cout << "C\n"; - if (!(this->isIsolated || this->networkId == 0)) { - if (thing->terminate) { - DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); - this->remoteSite->Send(destroyMsg); - delete destroyMsg; - } else { - // Send to remote site - if (thing->nameChanged) { - NameMsg* nameMsg = new NameMsg(this->networkId, thing); - this->remoteSite->Send(nameMsg); - delete nameMsg; - } - PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - this->remoteSite->Send(poseMsg); - delete poseMsg; - BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); - this->remoteSite->Send(binaryMsg); - delete binaryMsg; - } - } + // if (!(this->isIsolated || this->networkId == 0)) { + // if (thing->terminate) { + // DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); + // this->remoteSite->Send(destroyMsg); + // delete destroyMsg; + // } else { + // // Send to remote site + // if (thing->nameChanged) { + // NameMsg* nameMsg = new NameMsg(this->networkId, thing); + // this->remoteSite->Send(nameMsg); + // delete nameMsg; + // } + // PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); + // this->remoteSite->Send(poseMsg); + // delete poseMsg; + // BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); + // this->remoteSite->Send(binaryMsg); + // delete binaryMsg; + // } + // } // std::cout << "D\n"; if (thing->terminate) this->Remove(thing); @@ -230,34 +230,10 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, delete binaryMsg; } -// bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { -// int bufferSize = msg->Serialize(this->buffer); -// if (bufferSize <= 0) -// return true; - -// // std::cout << "send msg " << (static_cast(this->buffer[0]) & 0xff) -// // << " to " << remoteParticipant->ipAddress << std::endl; - -// #if defined(_WIN32) || defined(_WIN64) -// Windows::ParticipantUDP* thisWindows = -// static_cast(this); -// return thisWindows->Send(remoteParticipant, bufferSize); -// #elif defined(__unix__) || defined(__APPLE__) -// Posix::ParticipantUDP* thisPosix = -// static_cast(this); return -// thisPosix->Send(remoteParticipant, bufferSize); -// #elif defined(ARDUINO) -// Arduino::ParticipantUDP* thisArduino = -// static_cast(this); -// return thisArduino->Send(remoteParticipant, bufferSize); -// #elif defined(IDF_VER) -// EspIdf::ParticipantUDP* thisEspIdf = -// static_cast(this); -// return thisEspIdf->Send(remoteParticipant, bufferSize); -// #else -// return false; -// #endif -// } +bool ParticipantUDP::Send(IMessage* msg) { + if (this->remoteSite != nullptr) + this->remoteSite->Send(msg); +} void ParticipantUDP::PublishThingInfo(Thing* thing) { // std::cout << "Publish thing info" << thing->networkId << "\n"; diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 9d92251..8b4b769 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -77,7 +77,6 @@ public: long publishInterval = 3000; // 3 seconds protected: - char buffer[1024]; #if !defined(ARDUINO) #if defined(__unix__) || defined(__APPLE__) @@ -114,7 +113,7 @@ public: void SendThingInfo(Participant* remoteParticipant, Thing* thing); void PublishThingInfo(Thing* thing); - //bool Send(Participant* remoteParticipant, IMessage* msg); + virtual bool Send(IMessage* msg) override; bool Publish(IMessage* msg); #pragma endregion Send diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp index 26b16bc..9bc5d88 100644 --- a/Things/ControlledMotor.cpp +++ b/Things/ControlledMotor.cpp @@ -39,8 +39,8 @@ void ControlledMotor::Update(bool recurse) { this->lastError = error; float output = p_term + i_term + d_term; - std::cout << "target " << this->targetVelocity << " actual " - << this->actualVelocity << " output = " << output << std::endl; + // std::cout << "target " << this->targetVelocity << " actual " + // << this->actualVelocity << " output = " << output << std::endl; // float acceleration = // error * timeStep * pidP; // Just P is used at this moment // std::cout << "motor acc. " << acceleration << std::endl; diff --git a/Things/Motor.cpp b/Things/Motor.cpp index 936d4ec..c80e82e 100644 --- a/Things/Motor.cpp +++ b/Things/Motor.cpp @@ -1,5 +1,8 @@ #include "Motor.h" +#include "Messages/BinaryMsg.h" +#include "Participant.h" + namespace RoboidControl { Motor::Motor(Thing* parent) : Thing(parent) { @@ -7,7 +10,20 @@ Motor::Motor(Thing* parent) : Thing(parent) { } void Motor::SetTargetVelocity(float targetSpeed) { + if (targetSpeed != this->targetVelocity) { this->targetVelocity = targetSpeed; + + if (this->owner->networkId != 0) { + // in other word: if we are connected... + BinaryMsg* binaryMsg = new BinaryMsg(this->owner->networkId, this); + this->owner->Send(binaryMsg); + delete binaryMsg; + } + } +} + +float Motor::GetTargetVelocity() { + return this->targetVelocity; } int Motor::GenerateBinary(char* data, unsigned char* ix) { diff --git a/Things/Motor.h b/Things/Motor.h index 38ac149..0332289 100644 --- a/Things/Motor.h +++ b/Things/Motor.h @@ -14,11 +14,11 @@ class Motor : public Thing { Direction direction; virtual void SetTargetVelocity(float velocity); // -1..0..1 + virtual float GetTargetVelocity(); int GenerateBinary(char* bytes, unsigned char* ix) override; - // virtual void ProcessBinary(char* bytes) override; - //protected: + protected: float targetVelocity = 0; }; From 315c37d3bf0d4eefda5c905c750d6936f77a173c Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 9 Jun 2025 12:07:50 +0200 Subject: [PATCH 45/56] Fix MacOS compatibility issues --- Participant.cpp | 2 +- examples/BB2B.cpp | 4 ++-- test/thing_test.cc | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Participant.cpp b/Participant.cpp index c3cf0ad..b8e0d16 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -70,7 +70,7 @@ bool Participant::Send(IMessage* msg) { return thisWindows->Send(remoteParticipant, bufferSize); #elif defined(__unix__) || defined(__APPLE__) Posix::ParticipantUDP* thisPosix = static_cast(this); - return thisPosix->Send(remoteParticipant, bufferSize); + return thisPosix->Send(this, bufferSize); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = static_cast(this); diff --git a/examples/BB2B.cpp b/examples/BB2B.cpp index 8a51d01..05d80f4 100644 --- a/examples/BB2B.cpp +++ b/examples/BB2B.cpp @@ -27,10 +27,10 @@ int main() { while (true) { // The left wheel turns forward when nothing is touched on the right side // and turn backward when the roboid hits something on the right - float leftWheelSpeed = (touchRight.internalTouch) ? -600.0f : 600.0f; + float leftWheelSpeed = (touchRight.IsTouching()) ? -600.0f : 600.0f; // The right wheel does the same, but instead is controlled by // touches on the left side - float rightWheelSpeed = (touchLeft.internalTouch) ? -600.0f : 600.0f; + float rightWheelSpeed = (touchLeft.IsTouching()) ? -600.0f : 600.0f; // When both sides are touching something, both wheels will turn backward // and the roboid will move backwards bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed); diff --git a/test/thing_test.cc b/test/thing_test.cc index ff651c3..9414754 100644 --- a/test/thing_test.cc +++ b/test/thing_test.cc @@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) { unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds; while (milliseconds < startTime + 1000) { - Thing::UpdateThings(); + thing->Update(); milliseconds = Thing::GetTimeMs(); } @@ -23,7 +23,7 @@ TEST(RoboidControlSuite, HiddenParticipant) { } TEST(RoboidControlSuite, IsolatedParticipant) { - ParticipantUDP* participant = ParticipantUDP::Isolated(); + ParticipantUDP* participant = new ParticipantUDP(0); Thing* thing = new Thing(); unsigned long milliseconds = Thing::GetTimeMs(); From 70c4afca89c208d21a25facbd60eff611805f69f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 17 Jun 2025 15:50:56 +0200 Subject: [PATCH 46/56] Add setup documentation --- CMakeLists.txt | 17 +++++---- DoxyGen/Doxyfile | 2 +- Participant.cpp | 2 +- Participants/ParticipantUDP.cpp | 4 ++- README.md | 54 +++++++++++++++++++++++++++- examples/{BB2B.cpp => BB2A/main.cpp} | 0 examples/CMakeLists.txt | 27 +++----------- examples/README.md | 2 +- 8 files changed, 74 insertions(+), 34 deletions(-) rename examples/{BB2B.cpp => BB2A/main.cpp} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index e4a9a1b..bcec558 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,9 +22,18 @@ else() set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard set(CMAKE_POSITION_INDEPENDENT_CODE ON) + include_directories( + . + LinearAlgebra + ) + add_library(RoboidControl STATIC ${srcs}) + project(RoboidControl) add_subdirectory(LinearAlgebra) - add_subdirectory(Examples) + + # Examples + option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF) + add_subdirectory(examples) add_compile_definitions(GTEST) include(FetchContent) @@ -38,12 +47,6 @@ else() set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) - include_directories( - . - LinearAlgebra - ) - add_library(RoboidControl STATIC ${srcs}) - enable_testing() file(GLOB_RECURSE test_srcs test/*_test.cc) diff --git a/DoxyGen/Doxyfile b/DoxyGen/Doxyfile index 853c02a..d551ef0 100644 --- a/DoxyGen/Doxyfile +++ b/DoxyGen/Doxyfile @@ -48,7 +48,7 @@ PROJECT_NAME = "Roboid Control for C++" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = +PROJECT_NUMBER = 0.4 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/Participant.cpp b/Participant.cpp index b8e0d16..dc02a15 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -67,7 +67,7 @@ bool Participant::Send(IMessage* msg) { #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = static_cast(this); - return thisWindows->Send(remoteParticipant, bufferSize); + return thisWindows->Send(this, bufferSize); #elif defined(__unix__) || defined(__APPLE__) Posix::ParticipantUDP* thisPosix = static_cast(this); return thisPosix->Send(this, bufferSize); diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index fc99aae..06d3fed 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -232,7 +232,9 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, bool ParticipantUDP::Send(IMessage* msg) { if (this->remoteSite != nullptr) - this->remoteSite->Send(msg); + return this->remoteSite->Send(msg); + + return true; } void ParticipantUDP::PublishThingInfo(Thing* thing) { diff --git a/README.md b/README.md index e0a3c1a..876fdf2 100644 --- a/README.md +++ b/README.md @@ -14,4 +14,56 @@ Supporting: # Basic components - RoboidControl::Thing -- RoboidControl::Participant \ No newline at end of file +- RoboidControl::Participant + +# Installation + +## Core code + +The repository uses cmake for building. You can place it in a subfolder of your project and include it in you `CMakeLists.txt`. +For example if the library is placed in the subfolder `roboidcontrol`: +``` +add_subdirectory(roboidcontrol) + +add_executable(my_executable main.cpp) # Your source files/executable +target_link_libraries(my_executable RoboidControl) +``` + +## Arduino (PlatformIO) + +Arduino is only supported in combination with PlatformIO. The Arduino IDE is not (yet?) supported. + +The best way to include support for Roboid Control in PlatformIO is +to clone the Roboid Control for C++ repository into a subfolder of the /lib folder. +Alternatively you can download the zip file and unpack it as a subfolder of the /lib folder. + +## ESP-IDF + +The best way to include support for Roboid Control in PlatformIO is +to clone the Roboid Control for C++ repository into a subfolder of the /components folder. +Alternatively you can download the zip file and unpack it as a subfolder of the /components folder. + +Make sure you have included RoboidControl as a component in your top-level CMakeLists.txt, for example: +``` +list(APPEND EXTRA_COMPONENT_DIRS + components/RoboidControl + ) +``` + +# Get Started + +## Core C++ Examples + +This repository contains examples in the `examples` folder. You can build these using cmake. + +For example, to build the BB2A example: +``` +cmake -B build -D BUILD_EXAMPLE_BB2A=ON +cmake --build build +``` +The resulting executable is then `build/examples/Debug/BB2A.exe` + +## Arduino (PlatformIO) Examples + +Specific examples for the Arduino platform are found in the `Arduino\examples` folder. +To use them you should create a new project in PlatformIO and then copy the example code to your project. \ No newline at end of file diff --git a/examples/BB2B.cpp b/examples/BB2A/main.cpp similarity index 100% rename from examples/BB2B.cpp rename to examples/BB2A/main.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 00282a2..0cd9d2a 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,25 +1,8 @@ # examples/CMakeLists.txt -# Specify the minimum CMake version -cmake_minimum_required(VERSION 3.10) +# Check if the options are enabled and add the corresponding examples +if(BUILD_EXAMPLE_BB2A) + add_executable(BB2A BB2A/main.cpp) # Adjust the path as necessary -# Specify the path to the main project directory -set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..") - -# Set the project name -project(Examples) - -include_directories(..) - -# Add the executable for the main project -#add_executable(MainExecutable ${SOURCES}) -# Find the main project library (assuming it's defined in the root CMakeLists.txt) -#find_package(RoboidControl REQUIRED) # Replace MyLibrary with your actual library name - -# Add example executables -add_executable(BB2B BB2B.cpp) -target_link_libraries( - BB2B - RoboidControl - LinearAlgebra -) + target_link_libraries(BB2A RoboidControl) +endif() diff --git a/examples/README.md b/examples/README.md index b694fe9..bfba240 100644 --- a/examples/README.md +++ b/examples/README.md @@ -1 +1 @@ -Important: this folder has to be names 'examples' exactly to maintain compatibility with Arduino \ No newline at end of file +Important: this folder has to be named 'examples' exactly to maintain compatibility with Arduino \ No newline at end of file From 3bb3c68e9530a24995fade45ce763298aaaf6292 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 17 Jun 2025 17:18:17 +0200 Subject: [PATCH 47/56] Moved unit test cmake commands to test --- CMakeLists.txt | 27 +-------------------------- Testing/Temporary/LastTest.log | 3 +++ test/CMakeLists.txt | 30 ++++++++++++++++++++++++++++++ 3 files changed, 34 insertions(+), 26 deletions(-) create mode 100644 Testing/Temporary/LastTest.log create mode 100644 test/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index bcec558..63a3359 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,33 +35,8 @@ else() option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF) add_subdirectory(examples) - add_compile_definitions(GTEST) - include(FetchContent) - FetchContent_Declare( - googletest - DOWNLOAD_EXTRACT_TIMESTAMP ON - URL https://github.com/google/googletest/archive/refs/heads/main.zip - ) - - # For Windows: Prevent overriding the parent project's compiler/linker settings - set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) - FetchContent_MakeAvailable(googletest) - enable_testing() + add_subdirectory(test) - file(GLOB_RECURSE test_srcs test/*_test.cc) - add_executable( - RoboidControlTest - ${test_srcs} - ) - target_link_libraries( - RoboidControlTest - gtest_main - RoboidControl - LinearAlgebra - ) - - include(GoogleTest) - gtest_discover_tests(RoboidControlTest) endif() diff --git a/Testing/Temporary/LastTest.log b/Testing/Temporary/LastTest.log new file mode 100644 index 0000000..3ab3dd5 --- /dev/null +++ b/Testing/Temporary/LastTest.log @@ -0,0 +1,3 @@ +Start testing: Jun 17 17:17 W. Europe Summer Time +---------------------------------------------------------- +End testing: Jun 17 17:17 W. Europe Summer Time diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..99ee89c --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,30 @@ +# Unit test configuration + +add_compile_definitions(GTEST) +include(FetchContent) +FetchContent_Declare( + googletest + DOWNLOAD_EXTRACT_TIMESTAMP ON + URL https://github.com/google/googletest/archive/refs/heads/main.zip +) + +# For Windows: Prevent overriding the parent project's compiler/linker settings +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) +FetchContent_MakeAvailable(googletest) + +file(GLOB_RECURSE test_srcs *.cc) +message(STATUS "Test sources: ${test_srcs}") +add_executable( + RoboidControlTest + ${test_srcs} +) +message(STATUS "RoboidControlTest target created") +target_link_libraries( + RoboidControlTest + gtest_main + RoboidControl + LinearAlgebra +) + +include(GoogleTest) +gtest_discover_tests(RoboidControlTest) \ No newline at end of file From f4d36ad960928f9321bf9351cbcde1ff50cab50b Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 17 Jun 2025 17:55:01 +0200 Subject: [PATCH 48/56] Add target_include_directories --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 63a3359..75b9504 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,9 @@ else() LinearAlgebra ) add_library(RoboidControl STATIC ${srcs}) + target_include_directories(RoboidControl PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + ) project(RoboidControl) add_subdirectory(LinearAlgebra) From 6532d656de66131d41b8b3cc01e99f20d3498d0e Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 17 Jun 2025 17:58:07 +0200 Subject: [PATCH 49/56] Updated core code installation doc. --- README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 876fdf2..77862b9 100644 --- a/README.md +++ b/README.md @@ -23,9 +23,13 @@ Supporting: The repository uses cmake for building. You can place it in a subfolder of your project and include it in you `CMakeLists.txt`. For example if the library is placed in the subfolder `roboidcontrol`: ``` +# Add the path to Roboid Control add_subdirectory(roboidcontrol) -add_executable(my_executable main.cpp) # Your source files/executable +# Your source files/executable +add_executable(my_executable main.cpp) + +# Link against RoboidControl target_link_libraries(my_executable RoboidControl) ``` From e5916466a6c353309c27873b9e35600bc6178508 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 14:00:39 +0200 Subject: [PATCH 50/56] Fixed force sending posmsg --- Messages/PoseMsg.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index aca128e..d72e453 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { this->thingId = thing->id; this->poseType = 0; - if (thing->positionUpdated || (force && thing->IsRoot())) { + if (thing->positionUpdated || (force || thing->IsRoot())) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; } - if (thing->orientationUpdated || (force && thing->IsRoot())) { + if (thing->orientationUpdated || (force || thing->IsRoot())) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; } From e89c2467539019e7fa845c907c35d7fe0744d8e9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 14:04:01 +0200 Subject: [PATCH 51/56] Increased update rate --- examples/BB2A/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/BB2A/main.cpp b/examples/BB2A/main.cpp index 05d80f4..ef8f269 100644 --- a/examples/BB2A/main.cpp +++ b/examples/BB2A/main.cpp @@ -40,9 +40,9 @@ int main() { // and sleep for 100ms #if defined(ARDUINO) - delay(100); + delay(10); #else - sleep_for(milliseconds(100)); + sleep_for(milliseconds(10)); #endif } From e0b8f3eac3b92898cb07f9ab4f36f8565c0d6cd8 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 14:37:06 +0200 Subject: [PATCH 52/56] Fix touchsensor not resetting --- Arduino/Things/UltrasonicSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 338353e..804c2bc 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -65,7 +65,7 @@ UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent) void UltrasonicSensor::TouchSensor::Update(bool recursive) { RoboidControl::TouchSensor::Update(recursive); this->ultrasonic.Update(false); - this->internalTouch |= (this->ultrasonic.distance > 0 && + this->internalTouch = (this->ultrasonic.distance > 0 && this->ultrasonic.distance <= this->touchDistance); } From 6eafbe538f168a75c7994e142e00233b1f530a0a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 15:27:08 +0200 Subject: [PATCH 53/56] Fix intialization issues --- Arduino/Things/DRV8833.cpp | 11 +++++++---- Arduino/Things/DRV8833.h | 2 ++ 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index ed04d79..77dbbbc 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -8,6 +8,7 @@ namespace Arduino { #pragma region DRV8833 DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) { + std::cout << "DRV8833\n"; this->type = Type::Undetermined; this->name = "DRV8833"; this->pinStandby = config.standby; @@ -26,10 +27,12 @@ DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) { DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, Thing* parent) - : RoboidControl::DifferentialDrive(this->drv8833.motorA, - this->drv8833.motorB, - parent), - drv8833(config, this) {} + : drv8833(config, this), + RoboidControl::DifferentialDrive(nullptr, nullptr, parent) { + this->drv8833 = DRV8833(config, this); + this->leftWheel = this->drv8833.motorA; + this->rightWheel = this->drv8833.motorB; +} void DRV8833::DifferentialDrive::Update(bool recurse) { RoboidControl::DifferentialDrive::Update(recurse); diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index 9762980..a4cbd9f 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -19,6 +19,8 @@ class DRV8833 : public Thing { int BIn1; int BIn2; int standby = 255; + + constexpr Configuration(int a1, int a2, int b1, int b2, int standby = 255) : AIn1(a1), AIn2(a2), BIn1(b1), BIn2(b2), standby(standby) {} }; /// @brief Setup a DRV8833 motor controller From 634d560ee1b7a596c9e41aa0dd48b03ee594bb3a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 15:38:46 +0200 Subject: [PATCH 54/56] Fix crash when updating DRV8833 Diff Drive --- Arduino/Things/DRV8833.cpp | 12 +++++------- Arduino/Things/DRV8833.h | 2 +- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 77dbbbc..7421e73 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -8,7 +8,6 @@ namespace Arduino { #pragma region DRV8833 DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) { - std::cout << "DRV8833\n"; this->type = Type::Undetermined; this->name = "DRV8833"; this->pinStandby = config.standby; @@ -27,16 +26,15 @@ DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) { DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, Thing* parent) - : drv8833(config, this), - RoboidControl::DifferentialDrive(nullptr, nullptr, parent) { - this->drv8833 = DRV8833(config, this); - this->leftWheel = this->drv8833.motorA; - this->rightWheel = this->drv8833.motorB; + : RoboidControl::DifferentialDrive(nullptr, nullptr, parent) { + this->drv8833 = new DRV8833(config, this); + this->leftWheel = this->drv8833->motorA; + this->rightWheel = this->drv8833->motorB; } void DRV8833::DifferentialDrive::Update(bool recurse) { RoboidControl::DifferentialDrive::Update(recurse); - this->drv8833.Update(false); + this->drv8833->Update(false); } #pragma endregion Differential drive diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index a4cbd9f..9cc7d1a 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -45,7 +45,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { virtual void Update(bool recurse = false) override; protected: - DRV8833 drv8833; + DRV8833* drv8833 = nullptr; }; #pragma endregion Differential drive From 17916ae3dbdf608b668357967a1b24307f72fa62 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 17:16:42 +0200 Subject: [PATCH 55/56] Cleanup --- Arduino/Examples/BB2A/BB2A.cpp | 95 +++++++++++++++++++++++++++ Arduino/Examples/BB2A/configuration.h | 52 +++++++++++++++ Arduino/Things/DRV8833.cpp | 3 +- Participants/ParticipantUDP.cpp | 51 -------------- Participants/ParticipantUDP.h | 2 +- Thing.cpp | 6 -- Thing.h | 2 +- Things/TouchSensor.cpp | 6 +- Things/TouchSensor.h | 2 +- 9 files changed, 154 insertions(+), 65 deletions(-) create mode 100644 Arduino/Examples/BB2A/BB2A.cpp create mode 100644 Arduino/Examples/BB2A/configuration.h diff --git a/Arduino/Examples/BB2A/BB2A.cpp b/Arduino/Examples/BB2A/BB2A.cpp new file mode 100644 index 0000000..440fc87 --- /dev/null +++ b/Arduino/Examples/BB2A/BB2A.cpp @@ -0,0 +1,95 @@ +#include "Arduino.h" + +#include "Things/ControlledMotor.h" +#include "Things/DifferentialDrive.h" +#include "Things/RelativeEncoder.h" +#include "Things/TouchSensor.h" + +#include "Arduino/Things/DRV8833.h" +#include "Arduino/Things/DigitalInput.h" +#include "Arduino/Things/UltrasonicSensor.h" +#include "Arduino/ArduinoUtils.h" + +#include "Participants/ParticipantUDP.h" + +#include "configuration.h" + +#include + +using namespace RoboidControl; +using namespace RoboidControl::Arduino; + +ParticipantUDP* localParticipant; + +DifferentialDrive* bb2b; +TouchSensor* touchLeft; +TouchSensor* touchRight; + +// RelativeEncoder* encoderLeft; +// RelativeEncoder* encoderRight; + +void setup() { + Serial.begin(115200); + delay(3000); + Serial.println("started"); + + StartWifi("serrarens", "192.168.76.44"); + localParticipant = new ParticipantUDP("192.168.77.76"); + + bb2b = new DifferentialDrive(); + touchLeft = new TouchSensor(bb2b); + touchRight = new TouchSensor(bb2b); + + // bb2b = new DRV8833::DifferentialDrive(driveConfig); + // touchLeft = new UltrasonicSensor::TouchSensor(leftTouchConfig, bb2b); + // touchRight = new UltrasonicSensor::TouchSensor(rightTouchConfig, bb2b); + + touchLeft->name = "Left Touch Sensor"; + touchLeft->SetPosition(Spherical::Degrees(0.15, -30, 0)); + touchRight->name = "Right Touch Sensor"; + touchRight->SetPosition(Spherical::Degrees(0.15, 30, 0)); + + // encoderLeft = new DigitalInput::RelativeEncoder(leftEncoderConfig); + // encoderRight = new DigitalInput::RelativeEncoder(rightEncoderConfig); + + // bb2b->leftWheel = new ControlledMotor(bb2b->leftWheel, encoderLeft, bb2b); + // bb2b->rightWheel = new ControlledMotor(bb2b->rightWheel, encoderRight, bb2b); +} + +void loop() { + + // std::cout << touchLeft->touchedSomething << " | " + // << touchRight->touchedSomething << std::endl; + // std::cout << encoderLeft->rotationSpeed << " : " + // << encoderRight->rotationSpeed << std::endl; + // std::cout << bb2b->leftWheel->encoder->rotationSpeed + // << " :: " << bb2b->rightWheel->encoder->rotationSpeed << std::endl; + + + // The left wheel turns forward when nothing is touched on the right side + // and turn backward when the roboid hits something on the right + float leftMotorVelocity = (touchRight->IsTouching()) ? -1.0f : 1.0f; + // The right wheel does the same, but instead is controlled by + // touches on the left side + float rightMotorVelocity = (touchLeft->IsTouching()) ? -1.0f : 1.0f; + // When both sides are touching something, both wheels will turn backward + // and the roboid will move backwards + + bb2b->leftWheel->SetTargetVelocity(leftMotorVelocity); + bb2b->rightWheel->SetTargetVelocity(rightMotorVelocity); + // std::cout << " " << bb2b->leftWheel->GetTargetVelocity() << " : " << bb2b->rightWheel->GetTargetVelocity() << std::endl; + + // float leftWheelVelocity = (touchRight->touchedSomething) ? -1.0f : 1.0f; + // float rightWheelVelocity = (touchLeft->touchedSomething) ? -1.0f : 1.0f; + // bb2b->SetWheelVelocity(leftWheelVelocity, rightWheelVelocity); + // std::cout << " " << leftWheelVelocity << " # " << rightWheelVelocity << std::endl; + + // std::cout << leftMotor->actualVelocity << std::endl; + + //Serial.println("."); + // Update the roboid state + localParticipant->Update(); + + // and sleep for 100ms + delay(10); +} diff --git a/Arduino/Examples/BB2A/configuration.h b/Arduino/Examples/BB2A/configuration.h new file mode 100644 index 0000000..dc4e4e1 --- /dev/null +++ b/Arduino/Examples/BB2A/configuration.h @@ -0,0 +1,52 @@ +#pragma once + +#include "Arduino/Things/UltrasonicSensor.h" +#include "Arduino/Things/DRV8833.h" +#include "Arduino/Things/DigitalInput.h" + +using namespace RoboidControl::Arduino; + +#if defined(ESP32) +static constexpr DRV8833::Configuration driveConfig = { + 17, // AIn1 + 16, // AIn2 + 14, // BIn1 + 27 // BIn2 +}; +static constexpr UltrasonicSensor::Configuration leftTouchConfig = { + 25, // Trigger + 33 // Echo +}; +static constexpr UltrasonicSensor::Configuration rightTouchConfig = { + 15, // Trigger + 5 // Echo +}; + +#elif defined(UNO) || defined(UNO_R4) +static constexpr DRV8833::Configuration driveConfig = { + 5, // AIn1 + 6, // AIn2 + 7, // BIn1 + 10 // BIn2 +}; + +static constexpr UltrasonicSensor::Configuration leftTouchConfig = { + A0, // Trigger + 12 // Echo +}; +static constexpr UltrasonicSensor::Configuration rightTouchConfig = { + A1, // Trigger + 11 // Echo +}; + +static constexpr DigitalInput::RelativeEncoder::Configuration + leftEncoderConfig = { + 2, // Input pin + 80 // Pulses per revolution +}; +static constexpr DigitalInput::RelativeEncoder::Configuration + rightEncoderConfig = { + 3, // Input pin + 80 // Pulses per revolution +}; +#endif \ No newline at end of file diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 7421e73..9ab9839 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -49,8 +49,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse) - : Motor() { - this->SetParent(driver); + : Motor(driver) { this->pinIn1 = pinIn1; this->pinIn2 = pinIn2; diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 06d3fed..dc26caf 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -86,8 +86,6 @@ void ParticipantUDP::SetupUDP(int localPort, void ParticipantUDP::Update() { unsigned long currentTimeMs = Thing::GetTimeMs(); - PrepMyThings(); - if (this->isIsolated == false) { if (this->connected == false) begin(); @@ -111,68 +109,19 @@ void ParticipantUDP::Update() { UpdateOtherThings(); } -void ParticipantUDP::PrepMyThings() { - for (Thing* thing : this->things) { - if (thing == nullptr) - continue; - - thing->PrepareForUpdate(); - } -} - void ParticipantUDP::UpdateMyThings() { - // std::cout << this->things.size() << std::endl; for (Thing* thing : this->things) { if (thing == nullptr) // || thing->GetParent() != nullptr) continue; - // std::cout << thing->name << "\n"; - // if (thing->hierarchyChanged) { - // if (!(this->isIsolated || this->networkId == 0)) { - // ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); - // this->remoteSite->Send(thingMsg); - // delete thingMsg; - - // if (thing->nameChanged) { - // NameMsg* nameMsg = new NameMsg(this->networkId, thing); - // this->remoteSite->Send(nameMsg); - // delete nameMsg; - // } - // } - // } - - // std::cout << "B\n"; // 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! thing->Update(false); - // std::cout << "C\n"; - // if (!(this->isIsolated || this->networkId == 0)) { - // if (thing->terminate) { - // DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); - // this->remoteSite->Send(destroyMsg); - // delete destroyMsg; - // } else { - // // Send to remote site - // if (thing->nameChanged) { - // NameMsg* nameMsg = new NameMsg(this->networkId, thing); - // this->remoteSite->Send(nameMsg); - // delete nameMsg; - // } - // PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - // this->remoteSite->Send(poseMsg); - // delete poseMsg; - // BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); - // this->remoteSite->Send(binaryMsg); - // delete binaryMsg; - // } - // } - // std::cout << "D\n"; if (thing->terminate) this->Remove(thing); - // std::cout << "E\n"; } } diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 8b4b769..a23bb6b 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -102,7 +102,7 @@ public: unsigned long nextPublishMe = 0; /// @brief Prepare the local things for the next update - virtual void PrepMyThings(); + //virtual void PrepMyThings(); virtual void UpdateMyThings(); virtual void UpdateOtherThings(); diff --git a/Thing.cpp b/Thing.cpp index 5f9694c..efb6bfb 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -253,12 +253,6 @@ unsigned long Thing::GetTimeMs() { #endif } -// void Thing::Update(bool recursive) { -// Update(GetTimeMs(), recursive); -// } - -void Thing::PrepareForUpdate() {} - void Thing::Update(bool recursive) { this->positionUpdated = false; this->orientationUpdated = false; diff --git a/Thing.h b/Thing.h index e425f9b..f538c33 100644 --- a/Thing.h +++ b/Thing.h @@ -223,7 +223,7 @@ class Thing { #pragma region Update public: - virtual void PrepareForUpdate(); + //virtual void PrepareForUpdate(); /// @brief Updates the state of the thing /// @param recurse When true, this will Update the descendants recursively diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index 9d347db..e5a593b 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -11,9 +11,9 @@ bool TouchSensor::IsTouching() { return this->internalTouch || this->externalTouch; } -void TouchSensor::PrepareForUpdate() { - //this->internalTouch = this->externalTouch; -} +// void TouchSensor::PrepareForUpdate() { +// //this->internalTouch = this->externalTouch; +// } void TouchSensor::Update(bool recursive) { Thing::Update(recursive); diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index e8eb2bc..9890ef8 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -18,7 +18,7 @@ class TouchSensor : public Thing { /// otherwise bool IsTouching(); - virtual void PrepareForUpdate() override; + //virtual void PrepareForUpdate() override; virtual void Update(bool recursive) override; /// @brief Function used to generate binary data for this touch sensor From 230a41f1434be09ad0ec299437197fcdce73bf83 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 19 Jun 2025 11:58:25 +0200 Subject: [PATCH 56/56] Add Distance Sensor --- Arduino/Things/UltrasonicSensor.cpp | 28 +++++++++++++++++--- Arduino/Things/UltrasonicSensor.h | 20 +++++++++++++- Things/DistanceSensor.cpp | 29 ++++++++++++++++++++ Things/DistanceSensor.h | 41 +++++++++++++++++++++++++++++ Things/TouchSensor.h | 2 +- 5 files changed, 115 insertions(+), 5 deletions(-) create mode 100644 Things/DistanceSensor.cpp create mode 100644 Things/DistanceSensor.h diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 804c2bc..8fa940d 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -8,7 +8,7 @@ namespace Arduino { UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) : Thing(parent) { - this->type = Type::DistanceSensor; + this->type = Type::DistanceSensor; this->name = "Ultrasonic sensor"; this->pinTrigger = config.trigger; this->pinEcho = config.echo; @@ -57,6 +57,28 @@ void UltrasonicSensor::Update(bool recursive) { Thing::Update(recursive); } +#pragma region Distance sensor + +UltrasonicSensor::DistanceSensor::DistanceSensor( + UltrasonicSensor::Configuration config, + Thing* parent) + : RoboidControl::DistanceSensor(parent), ultrasonic(config, this) {} + +void UltrasonicSensor::DistanceSensor::Update(bool recursive) { + RoboidControl::DistanceSensor::Update(recursive); + this->ultrasonic.Update(false); + if (this->ultrasonic.distance > 0) + this->internalDistance = this->ultrasonic.distance; + else +#if ARDUNIO + this->internalDistance = INFINITY; +#else + this->internalDistance = std::numeric_limits::infinity(); +#endif +} + +#pragma endregion Distance sensor + #pragma region Touch sensor UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent) @@ -66,10 +88,10 @@ void UltrasonicSensor::TouchSensor::Update(bool recursive) { RoboidControl::TouchSensor::Update(recursive); this->ultrasonic.Update(false); this->internalTouch = (this->ultrasonic.distance > 0 && - this->ultrasonic.distance <= this->touchDistance); + this->ultrasonic.distance <= this->touchDistance); } -#pragma region Touch sensor +#pragma endregion Touch sensor } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index 8e72887..c991e23 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -1,5 +1,6 @@ #pragma once +#include "Things/DistanceSensor.h" #include "Things/TouchSensor.h" namespace RoboidControl { @@ -39,9 +40,26 @@ class UltrasonicSensor : Thing { unsigned char pinEcho = 0; public: + class DistanceSensor; class TouchSensor; }; +#pragma region Distance sensor + +class UltrasonicSensor::DistanceSensor : public RoboidControl::DistanceSensor { + public: + DistanceSensor(UltrasonicSensor::Configuration config, + Thing* parent = Thing::LocalRoot()); + + /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) + virtual void Update(bool recursive = false) override; + + protected: + UltrasonicSensor ultrasonic; +}; + +#pragma endregion Distance sensor + #pragma region Touch sensor class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { @@ -58,7 +76,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { UltrasonicSensor ultrasonic; }; -#pragma region Touch sensor +#pragma endregion Touch sensor } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Things/DistanceSensor.cpp b/Things/DistanceSensor.cpp new file mode 100644 index 0000000..c33d64a --- /dev/null +++ b/Things/DistanceSensor.cpp @@ -0,0 +1,29 @@ +#include "DistanceSensor.h" + +#include "Messages/LowLevelMessages.h" + +namespace RoboidControl { + +DistanceSensor::DistanceSensor(Thing* parent) : Thing(parent) { + this->type = Type::DistanceSensor; + this->name = "Distance sensor"; +} + +float DistanceSensor::GetDistance() { + if (this->externalDistance < this->internalDistance) + return this->externalDistance; + else + return this->internalDistance; +} + +int DistanceSensor::GenerateBinary(char* bytes, unsigned char* ix) { + LowLevelMessages::SendFloat16(bytes, ix, this->internalDistance); + return *ix; +} + +void DistanceSensor::ProcessBinary(char* bytes) { + unsigned char ix = 0; + this->externalDistance = LowLevelMessages::ReceiveFloat16(bytes, &ix); +} + +} // namespace RoboidControl \ No newline at end of file diff --git a/Things/DistanceSensor.h b/Things/DistanceSensor.h new file mode 100644 index 0000000..08e324d --- /dev/null +++ b/Things/DistanceSensor.h @@ -0,0 +1,41 @@ +#pragma once + +#if !NO_STD +#include +#endif + +#include "Thing.h" + +namespace RoboidControl { + +/// @brief A sensor measuring distance +class DistanceSensor : public Thing { + public: + /// @brief Create a new child touch sensor + /// @param parent The parent thing + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + DistanceSensor(Thing* parent = Thing::LocalRoot()); + + /// @brief Get the current distance + float GetDistance(); + + /// @brief Function used to generate binary data for this sensor + /// @param buffer The byte array for thw binary data + /// @param ix The starting position for writing the binary data + int GenerateBinary(char* bytes, unsigned char* ix) override; + /// @brief Function used to process binary data received for this sensor + /// @param bytes The binary data to process + virtual void ProcessBinary(char* bytes) override; + + protected: +#if ARDUNIO + float internalDistance = INFINITY; + float externalDistance = INFINITY; +#else + float internalDistance = std::numeric_limits::infinity(); + float externalDistance = std::numeric_limits::infinity(); +#endif +}; + +} // namespace RoboidControl diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index 9890ef8..bdc5279 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -6,7 +6,7 @@ namespace RoboidControl { /// @brief A sensor which can detect touches class TouchSensor : public Thing { -// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor +// When finishing this release (0.3), I notice that this is equivalent to a digital sensor public: /// @brief Create a new child touch sensor /// @param parent The parent thing