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