From 8a248814daa10a961c93085517de6258a731ed03 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Mon, 18 Mar 2024 17:45:39 +0100
Subject: [PATCH] Add TrackedObject orientation

---
 NetworkSync.cpp   | 14 ++++++++------
 NetworkSync.h     |  3 ++-
 Perception.cpp    |  5 +++--
 Perception.h      |  2 +-
 TrackedObject.cpp |  4 +++-
 TrackedObject.h   |  7 +++++--
 6 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index 353c336..76f6943 100644
--- a/NetworkSync.cpp
+++ b/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
diff --git a/NetworkSync.h b/NetworkSync.h
index 7495116..e7a8fbd 100644
--- a/NetworkSync.h
+++ b/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);
diff --git a/Perception.cpp b/Perception.cpp
index 556ab84..5383955 100644
--- a/Perception.cpp
+++ b/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;
diff --git a/Perception.h b/Perception.h
index 482daa5..abbf40b 100644
--- a/Perception.h
+++ b/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
diff --git a/TrackedObject.cpp b/TrackedObject.cpp
index 752e79f..e75c01b 100644
--- a/TrackedObject.cpp
+++ b/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) {
diff --git a/TrackedObject.h b/TrackedObject.h
index a81309b..7d0fba9 100644
--- a/TrackedObject.h
+++ b/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;