Use AngleAxis for tracked thing orientation (but this will change again in the future)

LinearAlgebra

@@ -1 +1 @@
-Subproject commit 51772a18433c702244af814e0e7c24b28e904ada
+Subproject commit 353cb1bc7f3c12f703c0f963d89d08d567fc446a

NetworkSync.cpp

@@ -211,7 +211,8 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid,
   // Vector3 localPosition = object->position.ToVector3();
   Vector3 localPosition = Vector3(object->position);
   Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition;
-  Quaternion worldOrientation = roboidOrientation * object->orientation;
+  Quaternion worldOrientation =
+      roboidOrientation * object->orientation.ToQuaternion();
 
   unsigned char ix = 0;
   buffer[ix++] = PoseMsg;     // Position2DMsg;

Perception.cpp

@@ -306,8 +306,8 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
         //   this->roboid->networkSync->SendText("Update\0");
         // else
         //   this->roboid->networkSync->SendText("Refresh\0");
-        this->trackedObjects[thingIx]->Refresh(thing->position,
+        this->trackedObjects[thingIx]->Refresh(
-                                               thing->orientation);
+            thing->position, thing->orientation.ToQuaternion());
         delete thing;
         return this->trackedObjects[thingIx];
       }

Roboid.cpp

@@ -23,7 +23,7 @@ Roboid::Roboid() : Thing(0) {
   // this->actuation = nullptr;
   this->worldPosition = Vector3::zero;
   // this->worldOrientation = Quaternion::identity;
-  this->worldAngleAxis = AngleAxis();
+  this->worldAngleAxis = AngleAxis<float>();
 }
 
 Roboid::Roboid(Propulsion* propulsion) : Roboid() {
@@ -92,9 +92,9 @@ void Roboid::SetPosition(Vector3 newWorldPosition) {
   Quaternion roboidOrientation = this->GetOrientation();
   Vector3 translation = newWorldPosition - this->worldPosition;
   float distance = translation.magnitude();
-  float angle = Vector3::SignedAngle(roboidOrientation * Vector3::forward,
+  AngleOf<float> angle = Vector3::SignedAngle(
-                                     translation, Vector3::up);
+      roboidOrientation * Vector3::forward, translation, Vector3::up);
-  Polar polarTranslation = Polar(angle, distance);
+  Polar polarTranslation = Polar(angle.ToFloat(), distance);
   if (perception != nullptr)
     perception->UpdatePose(polarTranslation);
   this->worldPosition = newWorldPosition;
@@ -114,12 +114,12 @@ void Roboid::SetOrientation(Quaternion worldOrientation) {
   if (perception != nullptr)
     perception->UpdatePose(delta);
 
-  AngleAxis angleAxis = AngleAxis(angle, Axis(axis));
+  AngleAxis<float> angleAxis = AngleAxis<float>(angle, Direction<float>(axis));
   this->worldAngleAxis = angleAxis;
 }
 
 void Roboid::SetOrientation2D(float angle) {
-  this->worldAngleAxis = AngleAxis(angle, Axis::up);
+  this->worldAngleAxis = AngleAxis<float>(angle, Direction<float>::up);
 }
 
 void Roboid::AddChild(Thing* child) {

Roboid.h

@@ -12,13 +12,13 @@ class NetworkSync;
 
 /// @brief A Roboid is used to control autonomous robots
 class Roboid : public Thing {
-public:
+ public:
   /// @brief Default constructor for a Roboid
   Roboid();
   /// @brief Creates a Roboid with Perception and Propulsion abilities
   /// @param perception The Perception implementation to use for this Roboid
   /// @param propulsion The Propulsion implementation to use for this Roboid
-  Roboid(Propulsion *propulsion);
+  Roboid(Propulsion* propulsion);
 
   /// @brief Update the state of the Roboid
   /// @param currentTimeMs The time in milliseconds when calling this
@@ -26,13 +26,13 @@ public:
   void Update(unsigned long currentTimeMs);
 
   /// @brief The Perception module of this Roboid
-  Perception *perception = nullptr;
+  Perception* perception = nullptr;
   /// @brief The Propulsion module of this Roboid
-  Propulsion *propulsion = nullptr;
+  Propulsion* propulsion = nullptr;
   // ServoMotor *actuation = nullptr;
 
   /// @brief The reference to the module to synchronize states across a network
-  NetworkSync *networkSync = nullptr;
+  NetworkSync* networkSync = nullptr;
 
   /// @brief Retrieve the current position of the roboid
   /// @return The position in carthesian coordinates in world space
@@ -65,9 +65,9 @@ public:
   virtual void SetOrientation(Quaternion worldOrientation);
   void SetOrientation2D(float angle);
 
-  virtual void AddChild(Thing *child) override;
+  virtual void AddChild(Thing* child) override;
 
-private:
+ private:
   /// @brief The position of the roboid in carthesian coordinates in world space
   /// @details This position may be set when NetworkSync is used to receive
   /// positions from an external tracking system. These values should not be set
@@ -79,12 +79,12 @@ private:
   /// set directly, but SetOrientation should be used instead.
   // Quaternion worldOrientation = Quaternion::identity;
 
-  AngleAxis worldAngleAxis = AngleAxis();
+  AngleAxis<float> worldAngleAxis = AngleAxis<float>();
 
   unsigned long lastUpdateTimeMs = 0;
 };
 
-} // namespace RoboidControl
+}  // namespace RoboidControl
-} // namespace Passer
+}  // namespace Passer
 
 using namespace Passer::RoboidControl;

TrackedObject.cpp

@@ -17,7 +17,10 @@ InterestingThing::InterestingThing(Sensor* sensor,
   this->confidence = maxConfidence;
   this->sensor = sensor;
   this->position = position;
-  this->orientation = orientation;
+  float angle;
+  Vector3 axis;
+  orientation.ToAngleAxis(&angle, &axis);
+  this->orientation = AngleAxis<float>(angle, axis);
   this->updated = true;
 }
 
@@ -76,7 +79,11 @@ void InterestingThing::Refresh(Polar position) {
 
 void InterestingThing::Refresh(Spherical position, Quaternion orientation) {
   this->position = position;
-  this->orientation = orientation;
+
+  float angle;
+  Vector3 axis;
+  orientation.ToAngleAxis(&angle, &axis);
+  this->orientation = AngleAxis<float>(angle, axis);
   this->confidence = maxConfidence;
   this->updated = true;
 }

TrackedObject.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "LinearAlgebra/AngleAxis.h"
 #include "LinearAlgebra/Polar.h"
 #include "LinearAlgebra/Quaternion.h"
 #include "LinearAlgebra/Spherical.h"
@@ -10,12 +11,13 @@ namespace RoboidControl {
 
 /// @brief An object tracked by the roboid
 class InterestingThing {
-public:
+ public:
   /// @brief An object tracked by the roboid
   /// @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, Polar position);
-  InterestingThing(Sensor *sensor, Spherical position,
+  InterestingThing(Sensor* sensor,
+                   Spherical position,
                    Quaternion orientation = Quaternion::identity);
 
   /// @brief Update the position of the object
@@ -39,7 +41,7 @@ public:
   /// @return Returns true when both objects are considered the same
   /// The result of this check depends on the equalityDistance and equalityAngle
   /// value.
-  bool IsTheSameAs(InterestingThing *otherObj);
+  bool IsTheSameAs(InterestingThing* otherObj);
   /// @brief The maximum difference in distance from the roboid in which two
   /// objects may be considered the same
   /// @details When the difference in distance is exactly this
@@ -66,18 +68,19 @@ public:
   /// @brief The current position of the object
   Spherical position = Spherical::zero;
   /// @brief The current orientation of the object
-  Quaternion orientation = Quaternion::identity;
+  AngleAxis<float> orientation = AngleAxis<float>();
+  // Quaternion orientation = Quaternion::identity;
   /// @brief The sensor which provided that lastet pose this object
-  Sensor *sensor = nullptr;
+  Sensor* sensor = nullptr;
 
   unsigned char type = 0x00;
   unsigned char confidence;
   bool updated = false;
 
-protected:
+ protected:
   static constexpr unsigned char maxConfidence = 255;
-  static constexpr unsigned char confidenceDropSpeed = 10; // 150; // 2;
+  static constexpr unsigned char confidenceDropSpeed = 10;  // 150; // 2;
 };
 
-} // namespace RoboidControl
+}  // namespace RoboidControl
-} // namespace Passer
+}  // namespace Passer