From edf2df6c7ec26fce13a5bbba090955c6b73c45cc Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Tue, 18 Jun 2024 13:06:58 +0200
Subject: [PATCH] Improve receive pose orientation

---
 LinearAlgebra         |  2 +-
 NetworkPerception.cpp | 55 +++++++++++++++++--------------------------
 2 files changed, 23 insertions(+), 34 deletions(-)

diff --git a/LinearAlgebra b/LinearAlgebra
index 2bad384..c70c079 160000
--- a/LinearAlgebra
+++ b/LinearAlgebra
@@ -1 +1 @@
-Subproject commit 2bad384611e7d999b4c421781dab8f1c4543fe93
+Subproject commit c70c079efc4fcfe9e9b2c29cd82f67faa63c827f
diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp
index 5f53c17..2eb5bbe 100644
--- a/NetworkPerception.cpp
+++ b/NetworkPerception.cpp
@@ -139,6 +139,10 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
   else if (objectId == 0x81)
     return ReceiveSphere(data, roboid);
 
+  Quaternion roboidOrientation = roboid->GetOrientation();
+  Spherical position = Spherical::zero;
+  Quaternion orientation = Quaternion::identity;
+
   if ((poseType & NetworkSync::Pose_Position) != 0) {
     Vector3 worldPosition = ReceiveVector3(data, 4);
     if (objectId == 0) {
@@ -152,43 +156,28 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
       //   return;
       // }
 
-      Quaternion roboidOrientation = roboid->GetOrientation();
       Vector3 localPosition = Quaternion::Inverse(roboidOrientation) *
                               (worldPosition - roboidPosition);
-      Spherical position = Spherical(localPosition);
-
-      // float angle =
-      //     Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
-
-      // Polar position = Polar(distance, angle); // Polar(angle, distance);
-      /*
-      Vector2 worldPosition2D = Vector2(worldPosition);
-
-      Vector2 myPosition = roboid->GetPosition2D();
-      float myOrientation = roboid->GetOrientation2D();
-
-      Vector2 localPosition =
-          Vector2::Rotate(worldPosition2D - myPosition, -myOrientation);
-
-      float distance = Vector2::Magnitude(localPosition);
-      float angle = Vector2::SignedAngle(Vector3::forward, localPosition);
-      Polar position = Polar(angle, distance);
-      */
-
-      // printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n",
-      //        worldPosition.x, worldPosition.z, localPosition.x,
-      //        localPosition.z, distance, angle);
-
-      // InterestingThing *thing = roboid->perception->AddTrackedObject(
-      //     this, networkId, objectId, position);
-      InterestingThing *thing = roboid->perception->AddTrackedObject(
-          this, position, Quaternion::identity, 0x81, 0x81, networkId);
-      // if (thing->networkId != 0x00 && thing->type == 0xFF) {
-      //   // Unknown thing
-      //   roboid->networkSync->SendInvestigateThing(thing);
-      // }
+      position = Spherical(localPosition);
     }
   }
+
+  if ((poseType & NetworkSync::Pose_Orientation) != 0) {
+    Vector3 worldAngles = ReceiveVector3(data, 16);
+    Quaternion worldOrientation = Quaternion::Euler(worldAngles);
+    if (objectId == 0) {
+      roboid->SetOrientation(worldOrientation);
+    } else {
+      orientation = Quaternion::Inverse(roboidOrientation) * worldOrientation;
+    }
+  }
+
+  InterestingThing *thing = roboid->perception->AddTrackedObject(
+      this, position, orientation, 0x81, 0x81, networkId);
+  if (thing->networkId != 0x00 && thing->type == 0xFF) {
+    // Unknown thing
+    roboid->networkSync->SendInvestigateThing(thing);
+  }
 }
 
 void NetworkPerception::ReceiveTypedObject(unsigned char *data,