Improved angular support

LinearAlgebra

@@ -1 +1 @@
-Subproject commit 09e25a8a218690157ba46ca5f7ccd5f33eab1c07
+Subproject commit 3363388a95d8fe9708b615fc2ecdc9474f4930a1

NetworkSync.cpp

@@ -49,7 +49,6 @@ void NetworkSync::PublishState(Roboid* roboid) {
   // if (roboid->updated == false)
   //   return;
 
-  if (roboid->GetVelocity().magnitude() > 0)
   SendPose(roboid);
   PublishPerception(roboid);
 }
@@ -177,32 +176,40 @@ void NetworkSync::SendDestroyThing(InterestingThing* thing) {
 #endif
 }
 
-void NetworkSync::SendPose(Roboid* roboid, bool recurse) {
+// void NetworkSync::SendPose(Roboid* roboid, bool recurse) {
+//   if (networkId == 0)  // We're not connected to a site yet
+//     return;
+
+//   if (roboid->GetVelocity().magnitude() > 0) {
+//     unsigned char ix = 0;
+//     buffer[ix++] = PoseMsg;
+//     buffer[ix++] = 0x00;
+//     buffer[ix++] = Pose_Position | Pose_Orientation;
+//     SendSpherical(buffer, &ix,
+//     Spherical::FromVector3(roboid->GetPosition())); SendQuat32(buffer, &ix,
+//     roboid->GetOrientation()); SendBuffer(ix);
+//   }
+
+// #if RC_DEBUG
+//   printf("Sent PoseMsg [%d/%d]\n", networkId, buffer[1]);
+// #endif
+
+//   if (recurse) {
+//     for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++)
+//     {
+//       Thing* child = roboid->GetChild(childIx);
+//       if (child != nullptr)
+//         SendPose(child, true);
+//     }
+//   }
+// }
+
+void NetworkSync::SendPose(Thing* thing, bool recurse) {
   if (networkId == 0)  // We're not connected to a site yet
     return;
 
-  unsigned char ix = 0;
+  if (thing->GetLinearVelocity().distance > 0 ||
-  buffer[ix++] = PoseMsg;
+      thing->GetAngularVelocity().angle > 0) {
-  buffer[ix++] = 0x00;
-  buffer[ix++] = Pose_Position | Pose_Orientation;
-  SendSpherical(buffer, &ix, Spherical::FromVector3(roboid->GetPosition()));
-  SendQuat32(buffer, &ix, roboid->GetOrientation());
-  SendBuffer(ix);
-
-#if RC_DEBUG
-  printf("Sent PoseMsg [%d/%d]\n", networkId, buffer[1]);
-#endif
-
-  if (recurse) {
-    for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) {
-      Thing* child = roboid->GetChild(childIx);
-      if (child != nullptr)
-        SendPose(child, true);
-    }
-  }
-}
-
-void NetworkSync::SendPose(Thing* thing, bool recurse) {
     unsigned char ix = 0;
     buffer[ix++] = PoseMsg;
     buffer[ix++] = thing->id;
@@ -210,6 +217,7 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) {
     SendSpherical16(buffer, &ix, thing->position);
     SendQuat32(buffer, &ix, thing->orientation);
     SendBuffer(ix);
+  }
 
 #if RC_DEBUG
   printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]);

NetworkSync.h

@@ -77,7 +77,7 @@ class NetworkSync {
 
   virtual void SendPosition(Vector3 worldPosition) {};
   virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {};
-  void SendPose(Roboid* roboid, bool recurse = true);
+  // void SendPose(Roboid* roboid, bool recurse = true);
   void SendPose(Thing* thing, bool recurse = true);
 
   virtual void SendText(const char* s);
@@ -90,9 +90,9 @@ class NetworkSync {
   void PublishState(Sensor* sensor);
 
   void PublishTrackedObject(Roboid* roboid, InterestingThing* object);
-  void PublishRelativeObject(Buffer sendBuffer,
+  // void PublishRelativeObject(Buffer sendBuffer,
-                             UInt8 parentId,
+  //                            UInt8 parentId,
-                             InterestingThing* object);
+  //                            InterestingThing* object);
 
   void SendSingle100(unsigned char* data, unsigned int startIndex, float value);
   void SendFloat16(unsigned char* data, unsigned char* startIndex, float value);

Roboid.cpp

@@ -23,7 +23,7 @@ Roboid::Roboid() : Thing(0) {
   // this->actuation = nullptr;
   this->worldPosition = Vector3::zero;
   // this->worldOrientation = Quaternion::identity;
-  this->worldAngleAxis = AngleAxis<float>();
+  this->worldAngleAxis = AngleAxisOf<float>();
 }
 
 Roboid::Roboid(Propulsion* propulsion) : Roboid() {
@@ -74,8 +74,7 @@ Vector2 Roboid::GetPosition2D() {
 
 Quaternion Roboid::GetOrientation() {
   Vector3 axis = this->worldAngleAxis.axis.ToVector3();
-  Quaternion q =
+  Quaternion q = Quaternion::AngleAxis(this->worldAngleAxis.angle, axis);
-      Quaternion::AngleAxis(this->worldAngleAxis.angle.ToFloat(), axis);
   return q;
 }
 
@@ -83,10 +82,10 @@ float Roboid::GetOrientation2D() {
   float maxAngle = 90 - Float::epsilon;  // note: range vertical angle = -90..90
 
   // rotation axis is vertical, so we have a simple 2D orientation
-  if (this->worldAngleAxis.axis.verticalAngle.ToFloat() > maxAngle)
+  if (this->worldAngleAxis.axis.vertical.InDegrees() > maxAngle)
-    return this->worldAngleAxis.angle.ToFloat();
+    return this->worldAngleAxis.angle;
-  if (this->worldAngleAxis.axis.verticalAngle.ToFloat() < -maxAngle)
+  if (this->worldAngleAxis.axis.vertical.InDegrees() < -maxAngle)
-    return -this->worldAngleAxis.angle.ToFloat();
+    return -this->worldAngleAxis.angle;
 
   Quaternion q = GetOrientation();
   return Quaternion::GetAngleAround(Vector3::up, q);
@@ -118,12 +117,13 @@ void Roboid::SetOrientation(Quaternion worldOrientation) {
   if (perception != nullptr)
     perception->UpdatePose(delta);
 
-  AngleAxis<float> angleAxis = AngleAxis<float>(angle, Direction<float>(axis));
+  AngleAxisOf<float> angleAxis =
+      AngleAxisOf<float>(angle, DirectionOf<float>(axis));
   this->worldAngleAxis = angleAxis;
 }
 
 void Roboid::SetOrientation2D(float angle) {
-  this->worldAngleAxis = AngleAxis<float>(angle, Direction<float>::up);
+  this->worldAngleAxis = AngleAxisOf<float>(angle, DirectionOf<float>::up);
 }
 
 Vector3 Passer::RoboidControl::Roboid::GetVelocity() {

Roboid.h

@@ -84,7 +84,7 @@ class Roboid : public Thing {
   /// set directly, but SetOrientation should be used instead.
   // Quaternion worldOrientation = Quaternion::identity;
 
-  AngleAxis<float> worldAngleAxis = AngleAxis<float>();
+  AngleAxis worldAngleAxis = AngleAxis();
 
   unsigned long lastUpdateTimeMs = 0;
 };

ServoMotor.cpp

@@ -13,7 +13,7 @@ ServoMotor::ServoMotor()
 void ServoMotor::SetTargetAngle(Angle16 angle) {
   angle = Float::Clamp(angle.ToFloat(), minAngle.ToFloat(), maxAngle.ToFloat());
 
-  if (maxVelocity == 0.0F) {
+  if (maxSpeed == 0.0F) {
     SetAngle(angle);
     this->limitedTargetAngle = angle;
   }
@@ -28,12 +28,11 @@ Angle16 ServoMotor::GetTargetAngle() {
 }
 
 void ServoMotor::SetMaximumVelocity(float maxVelocity) {
-  this->maxVelocity = maxVelocity;
+  this->maxSpeed = maxVelocity;
 }
 
 void ServoMotor::SetTargetVelocity(float targetVelocity) {
-  targetVelocity =
+  targetVelocity = Float::Clamp(targetVelocity, -this->maxSpeed, maxSpeed);
-      Float::Clamp(targetVelocity, -this->maxVelocity, maxVelocity);
 
   this->controlMode = ControlMode::Velocity;
   this->targetVelocity = targetVelocity;
@@ -62,12 +61,12 @@ void ServoMotor::Update(unsigned long currentTimeMs) {
 
   // Position control
   if (controlMode == ControlMode::Position) {
-    if (maxVelocity == 0.0F || hasTargetAngle == false) {
+    if (maxSpeed == 0.0F || hasTargetAngle == false) {
       this->lastUpdateTimeMs = currentTimeMs;
       return;
 
     } else {
-      float angleStep = maxVelocity * deltaTime;
+      float angleStep = maxSpeed * deltaTime;
       float deltaAngle =
           this->targetAngle.ToFloat() - this->limitedTargetAngle.ToFloat();
       float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle;

ServoMotor.h

@@ -36,7 +36,7 @@ class ServoMotor : public Thing {
   Angle16 targetAngle = 0.0F;
   Angle16 actualAngle = 0.0F;
 
-  float maxVelocity = 0.0F;
+  float maxSpeed = 0.0F;
 
   float targetVelocity = 0.0F;
 

Thing.cpp

@@ -76,3 +76,11 @@ Thing* Thing::GetChild(unsigned char childIx) {
   } else
     return nullptr;
 }
+
+Spherical16 Thing::GetLinearVelocity() {
+  return this->linearVelocity;
+}
+
+AngleAxis16 Thing::GetAngularVelocity() {
+  return this->angularVelocity;
+}

Thing.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "LinearAlgebra/AngleAxis.h"
 #include "LinearAlgebra/Quaternion.h"
 #include "LinearAlgebra/Spherical.h"
 
@@ -17,8 +18,8 @@ class Thing {
   /// @brief The type of Thing
   unsigned int type;
 
-  // I hate this, better is to have an additional field stating the thing classificaton
+  // I hate this, better is to have an additional field stating the thing
-  // Motor, Sensor etc.
+  // classificaton Motor, Sensor etc.
   /// @brief The type of a switch sensor
   static const unsigned int SwitchType;
   /// @brief The type of a distance sensor
@@ -51,6 +52,9 @@ class Thing {
   Quaternion orientation;
   Quaternion worldOrientation;
 
+  virtual Spherical16 GetLinearVelocity();
+  virtual AngleAxis16 GetAngularVelocity();
+
   /// @brief Sets the parent Thing
   /// @param parent The Thing which should become the parnet
   /// @remark This is equivalent to calling parent->AddChild(this);
@@ -109,6 +113,9 @@ class Thing {
 
   Thing* parent = nullptr;
   Thing** children = nullptr;
+
+  AngleAxis16 angularVelocity = AngleAxis16::zero;
+  Spherical16 linearVelocity = Spherical16::zero;
 };
 
 }  // namespace RoboidControl

TrackedObject.cpp

@@ -21,7 +21,7 @@ InterestingThing::InterestingThing(Sensor* sensor,
   float angle;
   Vector3 axis;
   orientation.ToAngleAxis(&angle, &axis);
-  this->orientation = AngleAxis<float>(angle, axis);
+  this->orientation = AngleAxisOf<float>(angle, axis);
   this->updated = true;
 }
 
@@ -85,7 +85,7 @@ void InterestingThing::Refresh(Spherical16 position, Quaternion orientation) {
   float angle;
   Vector3 axis;
   orientation.ToAngleAxis(&angle, &axis);
-  this->orientation = AngleAxis<float>(angle, axis);
+  this->orientation = AngleAxisOf<float>(angle, axis);
   this->confidence = maxConfidence;
   this->updated = true;
 }

TrackedObject.h

@@ -69,7 +69,7 @@ class InterestingThing {
   /// @brief The current position of the object
   Spherical16 position = Spherical16::zero;
   /// @brief The current orientation of the object
-  AngleAxis<float> orientation = AngleAxis<float>();
+  AngleAxisOf<float> orientation = AngleAxisOf<float>();
   // Quaternion orientation = Quaternion::identity;
   /// @brief The sensor which provided that lastet pose this object
   Sensor* sensor = nullptr;