From 2b6912a2c9cbf01a47e97c3d50323c71bedd7f77 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Fri, 10 May 2024 18:20:09 +0200
Subject: [PATCH] LinearAlgebra updates

---
 DifferentialDrive.cpp | 2 +-
 LinearAlgebra         | 2 +-
 NetworkPerception.cpp | 2 +-
 Perception.cpp        | 6 ++++--
 Propulsion.cpp        | 2 +-
 5 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp
index d3f2186..bf2c3d0 100644
--- a/DifferentialDrive.cpp
+++ b/DifferentialDrive.cpp
@@ -84,7 +84,7 @@ Polar DifferentialDrive::GetVelocity() {
 
   float direction = speed >= 0 ? 0.0F : 180.0F;
   float magnitude = fabsf(speed);
-  Polar velocity = Polar(direction, magnitude);
+  Polar velocity = Polar(magnitude, direction); // Polar(direction, magnitude);
   return velocity;
 }
 
diff --git a/LinearAlgebra b/LinearAlgebra
index 64ca768..791bd78 160000
--- a/LinearAlgebra
+++ b/LinearAlgebra
@@ -1 +1 @@
-Subproject commit 64ca76830c81974eaac9b0d7edad631522a14b12
+Subproject commit 791bd78e2909065f2f7e3bd47dbf3fce14c4286a
diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp
index bc951fe..22032a3 100644
--- a/NetworkPerception.cpp
+++ b/NetworkPerception.cpp
@@ -104,7 +104,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
       float angle =
           Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
 
-      Polar position = Polar(angle, distance);
+      Polar position = Polar(distance, angle); // Polar(angle, distance);
       /*
       Vector2 worldPosition2D = Vector2(worldPosition);
 
diff --git a/Perception.cpp b/Perception.cpp
index 0f3bff6..48f5173 100644
--- a/Perception.cpp
+++ b/Perception.cpp
@@ -478,14 +478,16 @@ void Perception::Update(float currentTimeMs) {
       float distance = distanceSensor->GetDistance();
       if (distance >= 0) {
         float angle = sensor->position.angle;
-        Polar position = Polar(angle, distance);
+        // Polar position = Polar(angle, distance);
+        Polar position = Polar(distance, angle);
         // AddTrackedObject(distanceSensor, position);
       }
 
     } else if (sensor->type == Thing::SwitchType) {
       Switch *switchSensor = (Switch *)sensor;
       if (switchSensor->IsOn()) {
-        Polar position = Polar(sensor->position.angle, nearbyDistance);
+        // Polar position = Polar(sensor->position.angle, nearbyDistance);
+        Polar position = Polar(nearbyDistance, sensor->position.angle);
         // AddTrackedObject(switchSensor, position);
       }
     } else {
diff --git a/Propulsion.cpp b/Propulsion.cpp
index f7a80a0..9510a4c 100644
--- a/Propulsion.cpp
+++ b/Propulsion.cpp
@@ -37,6 +37,6 @@ void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
 
 void Propulsion::SetVelocity(Polar velocity) {}
 
-Polar Propulsion::GetVelocity() { return Polar(0, 0); }
+Polar Propulsion::GetVelocity() { return Polar::zero; }
 
 float Propulsion::GetAngularVelocity() { return 0; }
\ No newline at end of file