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

@@ -79,7 +79,7 @@ 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;
 };

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"
@@ -15,7 +16,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
@@ -66,7 +68,8 @@ 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;