Performance improvement

LinearAlgebra

@@ -1 +1 @@
-Subproject commit 4b07328790ffd565f9f3303b625dbde68abaecc7
+Subproject commit 64ca76830c81974eaac9b0d7edad631522a14b12

NetworkPerception.cpp

@@ -50,14 +50,16 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
     if (objectId == 0) {
       roboid->SetPosition(worldPosition);
     } else {
+      Vector3 roboidPosition = roboid->GetPosition();
 
-      Vector3 myPosition = roboid->GetPosition();
-      Quaternion myOrientation = roboid->GetOrientation();
+      float distance = Vector3::Distance(roboidPosition, worldPosition);
+      if (roboid->perception->IsInteresting(distance) == false)
+        return;
 
-      Vector3 localPosition =
-          Quaternion::Inverse(myOrientation) * (worldPosition - myPosition);
+      Quaternion roboidOrientation = roboid->GetOrientation();
+      Vector3 localPosition = Quaternion::Inverse(roboidOrientation) *
+                              (worldPosition - roboidPosition);
 
-      float distance = Vector3::Magnitude(localPosition);
       float angle =
           Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
 

Perception.cpp

@@ -293,6 +293,17 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
   return thing;
 }
 
+bool Perception::IsInteresting(float distance) {
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = this->trackedObjects[objIx];
+    if (thing == nullptr)
+      continue;
+    if (thing->position.distance > distance)
+      return true;
+  }
+  return false;
+}
+
 InterestingThing *Perception::FindTrackedObject(char objectId) {
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
     InterestingThing *thing = this->trackedObjects[objIx];

Perception.h

@@ -97,6 +97,8 @@ public:
                    unsigned char objectId, Spherical position,
                    Quaternion orientation = Quaternion::identity);
 
+  bool IsInteresting(float distance);
+
   InterestingThing *FindTrackedObject(char objectId);
   InterestingThing *FindTrackedObject(unsigned char networkId,
                                       unsigned char objectId);

Roboid.cpp

@@ -17,7 +17,8 @@ Roboid::Roboid() {
   this->propulsion = nullptr;
   this->actuationRoot = nullptr;
   this->worldPosition = Vector3::zero;
-  this->worldOrientation = Quaternion::identity;
+  // this->worldOrientation = Quaternion::identity;
+  this->worldAngleAxis = AngleAxis();
 }
 
 Roboid::Roboid(Perception *perception, Propulsion *propulsion) : Roboid() {
@@ -50,10 +51,11 @@ void Roboid::Update(float currentTimeMs) {
     propulsion->Update(currentTimeMs);
 
     float deltaTime = (currentTimeMs - lastUpdateTimeMs) / 1000;
+    Quaternion roboidOrientation = this->GetOrientation();
     SetPosition(this->worldPosition +
-                this->worldOrientation * Vector3::forward *
+                roboidOrientation * Vector3::forward *
                     this->propulsion->GetVelocity().distance * deltaTime);
-    SetOrientation(this->worldOrientation *
+    SetOrientation(roboidOrientation *
                    Quaternion::AngleAxis(this->propulsion->GetAngularVelocity(),
                                          Vector3::up));
   }
@@ -71,15 +73,32 @@ Vector2 Roboid::GetPosition2D() {
   return Vector2(this->worldPosition.x, this->worldPosition.z);
 }
 
-Quaternion Roboid::GetOrientation() { return this->worldOrientation; }
+#include <Arduino.h>
+Quaternion Roboid::GetOrientation() {
+  Vector3 axis = this->worldAngleAxis.axis.ToVector3();
+  Quaternion q = Quaternion::AngleAxis(this->worldAngleAxis.angle, axis);
+  return q;
+}
+
+#include "FloatSingle.h"
 float Roboid::GetOrientation2D() {
-  return Quaternion::GetAngleAround(Vector3::up, this->worldOrientation);
+  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 > maxAngle)
+    return this->worldAngleAxis.angle;
+  if (this->worldAngleAxis.axis.verticalAngle < -maxAngle)
+    return -this->worldAngleAxis.angle;
+
+  Quaternion q = GetOrientation();
+  return Quaternion::GetAngleAround(Vector3::up, q);
 }
 
 void Roboid::SetPosition(Vector3 newWorldPosition) {
+  Quaternion roboidOrientation = this->GetOrientation();
   Vector3 translation = newWorldPosition - this->worldPosition;
   float distance = translation.magnitude();
-  float angle = Vector3::SignedAngle(this->worldOrientation * Vector3::forward,
+  float angle = Vector3::SignedAngle(roboidOrientation * Vector3::forward,
                                      translation, Vector3::up);
   Polar polarTranslation = Polar(angle, distance);
   perception->UpdatePose(polarTranslation);
@@ -87,12 +106,22 @@ void Roboid::SetPosition(Vector3 newWorldPosition) {
 
   if (networkSync != nullptr)
     // networkSync->SendPosition(this->worldPosition);
-    networkSync->SendPose(this->worldPosition, this->worldOrientation);
+    networkSync->SendPose(this->worldPosition, roboidOrientation);
 }
 
+#include <math.h>
 void Roboid::SetOrientation(Quaternion worldOrientation) {
-  Quaternion delta =
-      Quaternion::Inverse(this->worldOrientation) * worldOrientation;
+  float angle;
+  Vector3 axis;
+  worldOrientation.ToAngleAxis(&angle, &axis);
+
+  Quaternion delta = Quaternion::Inverse(GetOrientation()) * worldOrientation;
   perception->UpdatePose(delta);
-  this->worldOrientation = worldOrientation;
+
+  AngleAxis angleAxis = AngleAxis(angle, Axis(axis));
+  this->worldAngleAxis = angleAxis;
+}
+
+void Roboid::SetOrientation2D(float angle) {
+  this->worldAngleAxis = AngleAxis(angle, Axis::up);
 }

Roboid.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "LinearAlgebra/AngleAxis.h"
 #include "Perception.h"
 #include "Propulsion.h"
 #include "ServoMotor.h"
@@ -66,6 +67,7 @@ public:
   /// perceived objects by the roboid (roboid->perception->perceivedObjets),
   /// as these are local to the roboid' orientation.
   virtual void SetOrientation(Quaternion worldOrientation);
+  void SetOrientation2D(float angle);
 
 private:
   /// @brief The position of the roboid in carthesian coordinates in world space
@@ -77,7 +79,9 @@ private:
   /// @details The position may be set when NetworkSync is used to receive
   /// orientations from an external tracking system. This value should not be
   /// set directly, but SetOrientation should be used instead.
-  Quaternion worldOrientation = Quaternion::identity;
+  // Quaternion worldOrientation = Quaternion::identity;
+
+  AngleAxis worldAngleAxis = AngleAxis();
 
   unsigned long lastUpdateTimeMs = 0;
 };