Migration to ControlCore completed

CoreThing.cpp

@@ -1,64 +0,0 @@
-#include "CoreThing.h"
-
-#include "Participant.h"
-#include <iostream>
-
-CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) {
-  this->type = thingType;
-  this->networkId = networkId;
-  this->Init();
-
-  int thingId = CoreThing::Add(this);
-
-  if (thingId < 0) {
-    std::cout << "ERROR: Thing store is full\n";
-    this->id = 0; // what to do when we cannot store any more things?
-  } else
-    this->id = thingId;
-}
-
-void CoreThing::Terminate() { CoreThing::Remove(this); }
-
-void CoreThing::Init() {}
-
-CoreThing *CoreThing::GetParent() { return this->parent; }
-
-// All things
-CoreThing *CoreThing::allThings[THING_STORE_SIZE] = {nullptr};
-
-CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) {
-  for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
-    CoreThing *thing = allThings[ix];
-    if (thing == nullptr)
-      continue;
-    if (thing->networkId == networkId && thing->id == thingId)
-      return thing;
-  }
-  return nullptr;
-}
-
-int CoreThing::Add(CoreThing *newThing) {
-  for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
-    CoreThing *thing = allThings[ix];
-    if (thing == nullptr) {
-      allThings[ix] = newThing;
-
-      // std::cout << " Add new thing " << (int)ix << "\n";
-      return ix;
-    }
-  }
-  return -1;
-}
-
-void CoreThing::Remove(CoreThing *thing) { allThings[thing->id] = nullptr; }
-
-void CoreThing::UpdateAll(unsigned long currentTimeMs) {
-  // Not very efficient, but it works for now.
-  for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
-    CoreThing *thing = allThings[ix];
-    if (thing != nullptr &&
-        thing->parent == nullptr) { // update all root things
-      thing->Update(currentTimeMs);
-    }
-  }
-}

CoreThing.h

@@ -1,75 +0,0 @@
-#pragma once
-#include <iostream>
-
-namespace Passer {
-namespace Control {
-
-#define THING_STORE_SIZE 256
-// IMPORTANT: values higher than 256 will need to change the CoreThing::id type
-// to 16-bit or higher, breaking the networking protocol!
-
-class CoreThing {
-public:
-  // Participant *client;
-  unsigned char networkId;
-  /// @char The id of the thing
-  unsigned char id;
-  CoreThing *parent;
-  /// @brief The type of Thing
-  unsigned char type;
-  const char *name = nullptr;
-  const char *modelUrl = nullptr;
-  float modelScale = 1;
-  // protected Sensor sensor;
-
-  /// @brief Basic Thing types
-  enum class Type {
-    Undetermined,
-    // Sensor,
-    Switch,
-    DistanceSensor,
-    DirectionalSensor,
-    TemperatureSensor,
-    // Motor,
-    ControlledMotor,
-    UncontrolledMotor,
-    Servo,
-    // Other
-    Humanoid,
-    ExternalSensor,
-  };
-
-public:
-  CoreThing(unsigned char networkId = 0,
-            unsigned char thingType = (unsigned char)Type::Undetermined);
-  /// @brief Terminated thins are no longer updated
-  void Terminate();
-
-  /// @brief Gets the parent Thing
-  /// @return The parent Thing
-  CoreThing *GetParent();
-
-  /// @brief Updates the state of the thing
-  /// @param currentTimeMs The current clock time in milliseconds
-  virtual void Update(unsigned long currentTimeMs) {};
-
-  virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
-  virtual void ProcessBytes(unsigned char *bytes) {};
-
-protected:
-  virtual void Init();
-
-  //------------ All things
-public:
-  static CoreThing *Get(unsigned char networkId, unsigned char thingId);
-  static int Add(CoreThing *thing);
-  static void Remove(CoreThing *thing);
-  static void UpdateAll(unsigned long currentTimeMs);
-
-private:
-  static CoreThing *allThings[];
-};
-
-} // namespace Control
-} // namespace Passer
-using namespace Passer::Control;

Messages.cpp

@@ -158,7 +158,7 @@ unsigned char NameMsg::Serialize(unsigned char *buffer) {
   return ix;
 }
 
-// bool NameMsg::Send(Participant *participant, CoreThing *thing,
+// bool NameMsg::Send(Participant *participant, Thing *thing,
 //                    unsigned char nameLength) {
 //   if (thing->name == nullptr)
 //     return true; // nothing sent, but still a success!
@@ -204,12 +204,16 @@ unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
 
 PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
                  unsigned char poseType, Spherical16 position,
-                 SwingTwist16 orientation) {
+                 SwingTwist16 orientation, Spherical16 linearVelocity,
+                 Spherical16 angularVelocity) {
   this->networkId = networkId;
   this->thingId = thingId;
+
+  this->poseType = poseType;
   this->position = position;
   this->orientation = orientation;
-  this->poseType = poseType;
+  this->linearVelocity = linearVelocity;
+  this->angularVelocity = angularVelocity;
 }
 PoseMsg::PoseMsg(const unsigned char *buffer) {
   unsigned char ix = 1; // First byte is msg id
@@ -226,8 +230,14 @@ unsigned char PoseMsg::Serialize(unsigned char *buffer) {
   buffer[ix++] = this->networkId;
   buffer[ix++] = this->thingId;
   buffer[ix++] = this->poseType;
-  LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
-  LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
+  if ((this->poseType & Pose_Position) != 0)
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
+  if ((this->poseType & Pose_Orientation) != 0)
+    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
+  if ((this->poseType & Pose_LinearVelocity) != 0)
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
+  if ((this->poseType & Pose_AngularVelocity) != 0)
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
   return ix;
 }
 
@@ -245,7 +255,7 @@ CustomMsg::CustomMsg(unsigned char *buffer) {
                    // lifetime is shorter than the buffer lifetime...
 }
 
-CustomMsg::CustomMsg(unsigned char networkId, CoreThing *thing) {
+CustomMsg::CustomMsg(unsigned char networkId, Thing *thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
   this->thing = thing;
@@ -273,7 +283,7 @@ CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
 
 #pragma region DestroyMsg
 
-DestroyMsg::DestroyMsg(unsigned char networkId, CoreThing *thing) {
+DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
 }

Messages.h

@@ -2,7 +2,7 @@
 
 #include "../LinearAlgebra/Spherical.h"
 #include "../LinearAlgebra/SwingTwist.h"
-#include "CoreThing.h"
+#include "Thing.h"
 #include "float16.h"
 
 namespace Passer {
@@ -118,10 +118,13 @@ public:
 
   Spherical16 position;
   SwingTwist16 orientation;
+  Spherical16 linearVelocity;
+  Spherical16 angularVelocity;
 
   PoseMsg(unsigned char networkId, unsigned char thingId,
           unsigned char poseType, Spherical16 position,
-          SwingTwist16 orientation);
+          SwingTwist16 orientation, Spherical16 linearVelocity = Spherical16(),
+          Spherical16 angularVelocity = Spherical16());
   PoseMsg(const unsigned char *buffer);
 
   virtual unsigned char Serialize(unsigned char *buffer) override;
@@ -134,13 +137,13 @@ public:
 
   unsigned char networkId;
   unsigned char thingId;
-  CoreThing *thing;
+  Thing *thing;
 
   unsigned char dataSize;
   unsigned char *data;
 
   CustomMsg(unsigned char *buffer);
-  CustomMsg(unsigned char networkId, CoreThing *thing);
+  CustomMsg(unsigned char networkId, Thing *thing);
 
   virtual unsigned char Serialize(unsigned char *buffer) override;
 
@@ -154,7 +157,7 @@ public:
   unsigned char networkId;
   unsigned char thingId;
 
-  DestroyMsg(unsigned char networkId, CoreThing *thing);
+  DestroyMsg(unsigned char networkId, Thing *thing);
 
   virtual unsigned char Serialize(unsigned char *buffer) override;
 };

Thing.cpp

@@ -0,0 +1,195 @@
+#include "Thing.h"
+
+#include "Participant.h"
+#include <iostream>
+#include <string.h>
+
+Thing::Thing(unsigned char networkId, unsigned char thingType) {
+  this->type = thingType;
+  this->networkId = networkId;
+  this->Init();
+
+  int thingId = Thing::Add(this);
+
+  if (thingId < 0) {
+    std::cout << "ERROR: Thing store is full\n";
+    this->id = 0; // what to do when we cannot store any more things?
+  } else
+    this->id = thingId;
+
+  this->linearVelocity = Spherical16::zero;
+  this->angularVelocity = Spherical16::zero;
+}
+
+void Thing::Terminate() { Thing::Remove(this); }
+
+void Thing::Init() {}
+
+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::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 *root, const char *name) {
+  Thing *thing = root->FindThing(name);
+  if (thing != nullptr)
+    this->SetParent(thing);
+}
+
+Thing *Thing::GetParent() { return this->parent; }
+
+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 = nullptr;
+
+  delete[] this->children;
+  this->children = newChildren;
+  this->childCount = newChildCount;
+
+  return child;
+}
+
+Thing *Passer::Control::Thing::GetChild(unsigned char id, bool recursive) {
+  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 (recursive) {
+      Thing *foundChild = child->GetChild(id, recursive);
+      if (foundChild != nullptr)
+        return foundChild;
+    }
+  }
+  return nullptr;
+}
+
+Thing *Passer::Control::Thing::GetChildByIndex(unsigned char ix) {
+  return this->children[ix];
+}
+
+void Thing::SetModel(const char *url) { this->modelUrl = url; }
+
+void Thing::SetPosition(Spherical16 position) {
+  this->position = position;
+  this->positionUpdated = true;
+}
+Spherical16 Thing::GetPosition() { return this->position; }
+
+void Thing::SetOrientation(SwingTwist16 orientation) {
+  this->orientation = orientation;
+  this->orientationUpdated = true;
+}
+
+SwingTwist16 Thing::GetOrientation() { return this->orientation; }
+
+Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }
+
+Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }
+
+// All things
+Thing *Thing::allThings[THING_STORE_SIZE] = {nullptr};
+
+Thing *Thing::Get(unsigned char networkId, unsigned char thingId) {
+  for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
+    Thing *thing = allThings[ix];
+    if (thing == nullptr)
+      continue;
+    if (thing->networkId == networkId && thing->id == thingId)
+      return thing;
+  }
+  return nullptr;
+}
+
+int Thing::Add(Thing *newThing) {
+  // Exclude 0 because that value is reserved for 'no thing'
+  for (uint16_t ix = 1; ix < THING_STORE_SIZE; ix++) {
+    Thing *thing = allThings[ix];
+    if (thing == nullptr) {
+      allThings[ix] = newThing;
+
+      // std::cout << " Add new thing " << (int)ix << "\n";
+      return ix;
+    }
+  }
+  return -1;
+}
+
+void Thing::Remove(Thing *thing) {
+  // std::cout << " remove " << (int)thing->id << "\n";
+  allThings[thing->id] = nullptr;
+}
+
+void Thing::UpdateAll(unsigned long currentTimeMs) {
+  // Not very efficient, but it works for now.
+  for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
+    Thing *thing = allThings[ix];
+    if (thing != nullptr &&
+        thing->parent == nullptr) { // update all root things
+      // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
+      //           << "\n";
+      thing->Update(currentTimeMs);
+    }
+  }
+}

Thing.h

@@ -0,0 +1,133 @@
+#pragma once
+#include "../LinearAlgebra/Spherical.h"
+#include "../LinearAlgebra/SwingTwist.h"
+#include <iostream>
+
+namespace Passer {
+namespace Control {
+
+#define THING_STORE_SIZE 256
+// IMPORTANT: values higher than 256 will need to change the Thing::id type
+// to 16-bit or higher, breaking the networking protocol!
+
+class Thing {
+public:
+  // Participant *client;
+  unsigned char networkId = 0;
+  /// @char The id of the thing
+  unsigned char id = 0;
+
+  Thing *FindThing(const char *name);
+  //  Thing *FindChild(unsigned char id);
+
+  /// @brief Sets the parent Thing
+  /// @param parent The Thing which should become the parnet
+  /// @remark This is equivalent to calling parent->AddChild(this);
+  virtual void SetParent(Thing *parent);
+  void SetParent(Thing *root, const char *name);
+  /// @brief Gets the parent Thing
+  /// @return The parent Thing
+  Thing *GetParent();
+
+  /// @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
+  virtual void AddChild(Thing *child);
+  Thing *RemoveChild(Thing *child);
+
+  unsigned char childCount = 0;
+  Thing *GetChild(unsigned char id, bool recursive = false);
+  Thing *GetChildByIndex(unsigned char ix);
+
+protected:
+  Thing *parent = nullptr;
+  Thing **children = nullptr;
+
+public:
+  /// @brief The type of Thing
+  unsigned char type = 0;
+  const char *name = nullptr;
+  const char *modelUrl = nullptr;
+  float modelScale = 1;
+  // protected Sensor sensor;
+
+  /// @brief Basic Thing types
+  enum class Type {
+    Undetermined,
+    // Sensor,
+    Switch,
+    DistanceSensor,
+    DirectionalSensor,
+    TemperatureSensor,
+    // Motor,
+    ControlledMotor,
+    UncontrolledMotor,
+    Servo,
+    // Other
+    Roboid,
+    Humanoid,
+    ExternalSensor,
+  };
+
+  void SetPosition(Spherical16 position);
+  Spherical16 GetPosition();
+  void SetOrientation(SwingTwist16 orientation);
+  SwingTwist16 GetOrientation();
+  float scale = 1; // assuming uniform scale
+
+  bool positionUpdated = false;
+  bool orientationUpdated = false;
+
+protected:
+  /// @brief The position in local space
+  /// @remark When this Thing has a parent, the position is relative to the
+  /// parent's position and orientation
+  Spherical16 position;
+  /// @brief The orientation in local space
+  /// @remark When this Thing has a parent, the orientation is relative to the
+  /// parent's orientation
+  SwingTwist16 orientation;
+
+public:
+  Spherical16 linearVelocity;
+  Spherical16 angularVelocity;
+  virtual Spherical16 GetLinearVelocity();
+  virtual Spherical16 GetAngularVelocity();
+
+public:
+  Thing(unsigned char networkId = 0,
+        unsigned char thingType = (unsigned char)Type::Undetermined);
+  /// @brief Terminated thins are no longer updated
+  void Terminate();
+
+  /// @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 Updates the state of the thing
+  /// @param currentTimeMs The current clock time in milliseconds
+  virtual void Update(unsigned long currentTimeMs) {};
+
+  virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
+  virtual void ProcessBytes(unsigned char *bytes) {};
+
+protected:
+  virtual void Init();
+
+  //------------ All things
+public:
+  static Thing *Get(unsigned char networkId, unsigned char thingId);
+  static int Add(Thing *thing);
+  static void Remove(Thing *thing);
+  static void UpdateAll(unsigned long currentTimeMs);
+
+private:
+  static Thing *allThings[];
+};
+
+} // namespace Control
+} // namespace Passer
+using namespace Passer::Control;