Add TrackedObject orientation

NetworkSync.cpp

@@ -6,11 +6,11 @@
 
 #include "LinearAlgebra/DiscreteAngle.h"
 
-void NetworkSync::SendVector3(unsigned char *data, const int startIndex,
+void NetworkSync::SendVector3(unsigned char *data, unsigned int &startIndex,
                               const Vector3 v) {
   SendSingle100(data, startIndex, v.x);
-  SendSingle100(data, startIndex + 4, v.y);
-  SendSingle100(data, startIndex + 8, v.z);
+  SendSingle100(data, startIndex += 4, v.y);
+  SendSingle100(data, startIndex += 8, v.z);
 }
 
 void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
@@ -101,7 +101,8 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
       (UInt8)object->id,
       Pose_Position,
   };
-  SendVector3(buffer, 3, worldPosition3);
+  unsigned int ix = 3;
+  SendVector3(buffer, ix, worldPosition3);
   sendBuffer(buffer, bufferSize);
 #endif
 }
@@ -135,8 +136,9 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
       0, // objectId;
       Pose_LinearVelocity | Pose_AngularVelocity,
   };
-  SendVector3(buffer, 3, worldVelocity3);
-  SendVector3(buffer, 15, worldAngularVelocity);
+  unsigned int ix = 3;
+  SendVector3(buffer, ix, worldVelocity3);
+  SendVector3(buffer, ix, worldAngularVelocity);
 
   sendBuffer(buffer, bufferSize);
 #endif

NetworkSync.h

@@ -53,7 +53,8 @@ protected:
   void SendSingle100(unsigned char *data, const int startIndex, float value);
   void SendInt32(unsigned char *data, const int startIndex, Int32 value);
   void SendAngle(unsigned char *data, const int startIndex, float value);
-  void SendVector3(unsigned char *data, const int startIndex, const Vector3 v);
+  void SendVector3(unsigned char *data, unsigned int &startIndex,
+                   const Vector3 v);
   void SendQuaternion(unsigned char *data, const int startIndex,
                       const Quaternion q);
   void SendQuat32(unsigned char *data, int startIndex, const Quaternion q);

Perception.cpp

@@ -185,8 +185,9 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
   }
 }
 
-void Perception::AddTrackedObject(Sensor *sensor, Spherical position) {
-  InterestingThing *obj = new InterestingThing(sensor, position);
+void Perception::AddTrackedObject(Sensor *sensor, Spherical position,
+                                  Quaternion orientation) {
+  InterestingThing *obj = new InterestingThing(sensor, position, orientation);
 
   unsigned char farthestObjIx = 0;
   unsigned char availableSlotIx = 0;

Perception.h

@@ -86,7 +86,7 @@ public:
   /// @param position The position of the sensor in polar coordinates local to
   /// the roboid
   void AddTrackedObject(Sensor *sensor, Polar position);
-  void AddTrackedObject(Sensor *sensor, Spherical position);
+  void AddTrackedObject(Sensor *sensor, Spherical position, Quaternion orientation = Quaternion::identity);
 
   /// @brief Retrieve the number of objects currently being tracked by the
   /// roboid

TrackedObject.cpp

@@ -9,11 +9,13 @@ InterestingThing::InterestingThing(Sensor *sensor, Polar position) {
   this->position = Spherical(position);
 }
 
-InterestingThing::InterestingThing(Sensor *sensor, Spherical position) {
+InterestingThing::InterestingThing(Sensor *sensor, Spherical position,
+                                   Quaternion orientation) {
   this->id = 0;
   this->confidence = maxConfidence;
   this->sensor = sensor;
   this->position = position;
+  this->orientation = orientation;
 }
 
 bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) {

TrackedObject.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "LinearAlgebra/Polar.h"
+#include "LinearAlgebra/Quaternion.h"
 #include "LinearAlgebra/Spherical.h"
 #include "Sensor.h"
 
@@ -14,7 +15,8 @@ public:
   /// @param sensor The Sensor which detected this object
   /// @param position The position in polar coordinates local to the roboid
   InterestingThing(Sensor *sensor, Polar position);
-  InterestingThing(Sensor *sensor, Spherical position);
+  InterestingThing(Sensor *sensor, Spherical position,
+                   Quaternion orientation = Quaternion::identity);
 
   /// @brief Update the position of the object
   /// @param position The latest known position of the object
@@ -59,8 +61,9 @@ public:
   char id;
 
   /// @brief The current position of the object
-  // Polar position = Polar::zero;
   Spherical position = Spherical::zero;
+  /// @brief The current orientation of the object
+  Quaternion orientation = Quaternion::identity;
   /// @brief The sensor which provided that lastet pose this object
   Sensor *sensor = nullptr;