From 84435308196a2c7ac6af282907ff7b922dddc8d1 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Fri, 27 Sep 2024 09:46:20 +0200
Subject: [PATCH] Replace AngleAxis with Spherical

---
 LinearAlgebra   | 2 +-
 NetworkSync.cpp | 5 ++++-
 Roboid.cpp      | 2 +-
 ServoMotor.h    | 2 +-
 Thing.cpp       | 2 +-
 Thing.h         | 4 ++--
 6 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/LinearAlgebra b/LinearAlgebra
index 2e61e1b..edbb4b1 160000
--- a/LinearAlgebra
+++ b/LinearAlgebra
@@ -1 +1 @@
-Subproject commit 2e61e1b710cb0c0455d80970b7d072d70db7ac83
+Subproject commit edbb4b1ffc928523ff973c0769abd45672c08e53
diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index 2b123f9..00be150 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -193,7 +193,7 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) {
     return;
 
   if (thing->GetLinearVelocity().distance > 0 ||
-      thing->GetAngularVelocity().angle > 0) {
+      thing->GetAngularVelocity().distance > 0) {
     unsigned char ix = 0;
     buffer[ix++] = PoseMsg;
     buffer[ix++] = thing->id;
@@ -371,6 +371,9 @@ void NetworkSync::SendText(const char* s) {
 #endif
 }
 
+#if ESP32
+#include <Arduino.h>
+#endif
 void NetworkSync::SendInt(const int x) {
   String s = String(x);
   char length = s.length();
diff --git a/Roboid.cpp b/Roboid.cpp
index 766b056..cb1d6ae 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -52,7 +52,7 @@ void Roboid::Update(unsigned long currentTimeMs) {
     SetOrientation(this->worldOrientation *
                    SwingTwist16::AngleAxis(
                        this->propulsion->GetAngularVelocity() * deltaTime,
-                       Spherical16::up));
+                       Direction16::up));
   }
 
   if (childCount > 0 && children != nullptr) {
diff --git a/ServoMotor.h b/ServoMotor.h
index 204c8aa..5f59462 100644
--- a/ServoMotor.h
+++ b/ServoMotor.h
@@ -10,7 +10,7 @@ class ServoMotor : public Thing {
  public:
   ServoMotor();
 
-  Vector3 rotationAxis = Vector3::up;
+  Direction16 rotationAxis = Direction16::up;
   Angle16 minAngle = Angle16::Degrees(-90);
   Angle16 maxAngle = Angle16::Degrees(90);
 
diff --git a/Thing.cpp b/Thing.cpp
index c1e5d32..35b7eff 100644
--- a/Thing.cpp
+++ b/Thing.cpp
@@ -81,6 +81,6 @@ Spherical16 Thing::GetLinearVelocity() {
   return this->linearVelocity;
 }
 
-AngleAxis16 Thing::GetAngularVelocity() {
+Spherical16 Thing::GetAngularVelocity() {
   return this->angularVelocity;
 }
\ No newline at end of file
diff --git a/Thing.h b/Thing.h
index 5fb888e..6629acf 100644
--- a/Thing.h
+++ b/Thing.h
@@ -54,7 +54,7 @@ class Thing {
   SwingTwist16 worldOrientation;
 
   virtual Spherical16 GetLinearVelocity();
-  virtual AngleAxis16 GetAngularVelocity();
+  virtual Spherical16 GetAngularVelocity();
 
   /// @brief Sets the parent Thing
   /// @param parent The Thing which should become the parnet
@@ -116,7 +116,7 @@ class Thing {
   Thing** children = nullptr;
 
  public:
-  AngleAxis16 angularVelocity = AngleAxis16::zero;
+  Spherical16 angularVelocity = Spherical16::zero;
   Spherical16 linearVelocity = Spherical16::zero;
 };