Collisions work!

Arduino/Participant.cpp

@@ -23,7 +23,7 @@ void Participant::Setup(int localPort, const char* remoteIpAddress, int remotePo
     std::cout << "No network available!\n";
     return;
   }
-  udp.begin(this->localPort);
+  udp.begin(localPort);
 
   std::cout << "Wifi sync started to port " << this->remotePort << "\n";
 #endif

Messages/PoseMsg.cpp

@@ -3,33 +3,33 @@
 
 namespace RoboidControl {
 
-PoseMsg::PoseMsg(unsigned char networkId,
+// PoseMsg::PoseMsg(unsigned char networkId,
-                 unsigned char thingId,
+//                  unsigned char thingId,
-                 unsigned char poseType,
+//                  unsigned char poseType,
-                 Spherical16 position,
+//                  Spherical16 position,
-                 SwingTwist16 orientation,
+//                  SwingTwist16 orientation,
-                 Spherical16 linearVelocity,
+//                  Spherical16 linearVelocity,
-                 Spherical16 angularVelocity) {
+//                  Spherical16 angularVelocity) {
-  this->networkId = networkId;
+//   this->networkId = networkId;
-  this->thingId = thingId;
+//   this->thingId = thingId;
 
-  this->poseType = poseType;
+//   this->poseType = poseType;
-  this->position = position;
+//   this->position = position;
-  this->orientation = orientation;
+//   this->orientation = orientation;
-  this->linearVelocity = linearVelocity;
+//   this->linearVelocity = linearVelocity;
-  this->angularVelocity = angularVelocity;
+//   this->angularVelocity = angularVelocity;
-}
+// }
-PoseMsg::PoseMsg(unsigned char networkId, Thing* thing) {
+PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
   this->networkId = networkId;
   this->thingId = thing->id;
 
   this->poseType = 0;
-  if (thing->positionUpdated) {
+  if (thing->positionUpdated || force) {
     this->position = thing->GetPosition();
     this->poseType |= Pose_Position;
     thing->positionUpdated = false;
   }
-  if (thing->orientationUpdated) {
+  if (thing->orientationUpdated || force ) {
     this->orientation = thing->GetOrientation();
     this->poseType |= Pose_Orientation;
     thing->orientationUpdated = false;

Messages/PoseMsg.h

@@ -45,18 +45,18 @@ class PoseMsg : public IMessage {
   /// @param orientation The orientation of the thing in local space
   /// @param linearVelocity The linear velocity of the thing in local space in meters per second
   /// @param angularVelocity The angular velocity of the thing in local space
-  PoseMsg(unsigned char networkId,
+//   PoseMsg(unsigned char networkId,
-          unsigned char thingId,
+//           unsigned char thingId,
-          unsigned char poseType,
+//           unsigned char poseType,
-          Spherical16 position,
+//           Spherical16 position,
-          SwingTwist16 orientation,
+//           SwingTwist16 orientation,
-          Spherical16 linearVelocity = Spherical16(),
+//           Spherical16 linearVelocity = Spherical16(),
-          Spherical16 angularVelocity = Spherical16());
+//           Spherical16 angularVelocity = Spherical16());
 
   /// @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
-  PoseMsg(unsigned char networkId, Thing* thing);
+  PoseMsg(unsigned char networkId, Thing* thing, bool force = false);
 
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   PoseMsg(const char* buffer);

Participant.cpp

@@ -26,27 +26,16 @@ namespace RoboidControl {
 Participant::Participant(int port) {
   this->ipAddress = "0.0.0.0";
   this->port = port;
-
-  // this->senders.push_back(this);
-
-  // int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
-  this->localPort = port;
-  // SetupUDP(randomPort, ipAddress, port);
 }
 
 Participant::Participant(const char* ipAddress, int port) {
-  this->ipAddress = ipAddress;
+  this->ipAddress = ipAddress; // maybe this is not needed anymore, keeping it to "0.0.0.0"
   this->port = port;
-
+  this->site = new RemoteParticipant(ipAddress, port);
-  // this->senders.push_back(this);
-
-  // int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
-  this->localPort = port;  // randomPort;
-  // SetupUDP(randomPort, ipAddress, port);
 }
 
 void Participant::begin() {
-  SetupUDP(this->localPort, this->ipAddress, this->port);
+  SetupUDP(this->port, this->ipAddress, this->port);
 }
 
 void Participant::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) {
@@ -79,14 +68,11 @@ void Participant::Update(unsigned long currentTimeMs) {
 
   if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
     ParticipantMsg* msg = new ParticipantMsg(this->networkId);
-    this->Publish(msg);
+    if (this->site == nullptr)
+      this->Publish(msg);
+    else
+      this->Send(this->site, msg);
     delete msg;
-    // std::cout << this->name << " published ParticipantMsg\n";
-
-    // for (RemoteParticipant* sender : this->senders) {
-    //   for (Thing* thing : this->things)
-    //     SendThingInfo(sender, thing);
-    // }
 
     this->nextPublishMe = currentTimeMs + this->publishInterval;
   }
@@ -135,7 +121,7 @@ RemoteParticipant* Participant::AddParticipant(const char* ipAddress, int port)
 #pragma region Send
 
 void Participant::SendThingInfo(RemoteParticipant* owner, Thing* thing) {
-  std::cout << "Send thing info " << thing->id << " \n";
+  std::cout << "Send thing info " << (int)thing->id << " \n";
   ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
   this->Send(owner, thingMsg);
   delete thingMsg;
@@ -145,11 +131,28 @@ void Participant::SendThingInfo(RemoteParticipant* owner, Thing* thing) {
   ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
   this->Send(owner, modelMsg);
   delete modelMsg;
-  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
   this->Send(owner, poseMsg);
   delete poseMsg;
 }
 
+bool Participant::Send(RemoteParticipant* remoteParticipant, IMessage* msg) {
+  int bufferSize = msg->Serialize(this->buffer);
+  if (bufferSize <= 0)
+    return true;
+
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);
+  return thisWindows->Send(remoteParticipant, bufferSize);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::Participant* thisPosix = static_cast<Posix::Participant*>(this);
+  return thisPosix->Send(remoteParticipant, bufferSize);
+#elif defined(ARDUINO)
+  Arduino::Participant* thisArduino = static_cast<Arduino::Participant*>(this);
+  return thisArduino->Send(remoteParticipant, bufferSize);
+#endif
+}
+
 void Participant::PublishThingInfo(Thing* thing) {
   // std::cout << "Publish thing info" << thing->networkId << "\n";
   //  Strange, when publishing, the network id is irrelevant, because it is
@@ -168,23 +171,6 @@ void Participant::PublishThingInfo(Thing* thing) {
   delete customMsg;
 }
 
-bool Participant::Send(RemoteParticipant* remoteParticipant, IMessage* msg) {
-  int bufferSize = msg->Serialize(this->buffer);
-  if (bufferSize <= 0)
-    return true;
-
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);
-  return thisWindows->Send(remoteParticipant, bufferSize);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::Participant* thisPosix = static_cast<Posix::Participant*>(this);
-  return thisPosix->Send(remoteParticipant, bufferSize);
-#elif defined(ARDUINO)
-  Arduino::Participant* thisArduino = static_cast<Arduino::Participant*>(this);
-  return thisArduino->Send(remoteParticipant, bufferSize);
-#endif
-}
-
 bool Participant::Publish(IMessage* msg) {
 #if defined(_WIN32) || defined(_WIN64)
   Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);

Participant.h

@@ -33,7 +33,8 @@ class Participant : public RemoteParticipant {
 
   const char* name = "Participant";
 
-  int localPort = 0;
+  //int localPort = 0;
+  RemoteParticipant* site = nullptr;
 
 #if defined(ARDUINO)
   const char* remoteIpAddress = nullptr;
@@ -55,7 +56,7 @@ class Participant : public RemoteParticipant {
 #endif
 
   Participant(int port = 7681);
-  Participant(const char* ipAddress, int port);
+  Participant(const char* ipAddress, int port = 7681);
 
   void begin();
   bool connected = false;