From b0512e8f990da902fcb510c0483c592f43158a58 Mon Sep 17 00:00:00 2001
From: "Mariana R. Santos" <mrica@equinor.com>
Date: Thu, 10 Aug 2023 07:49:41 +0200
Subject: [PATCH] Update quaternion calculation

---
 backend/api.test/Database/Models.cs | 56 +++++++++++++++++++++++++++--
 backend/api/Database/Models/Pose.cs | 16 +++++++--
 2 files changed, 66 insertions(+), 6 deletions(-)

diff --git a/backend/api.test/Database/Models.cs b/backend/api.test/Database/Models.cs
index 8c37ee7e..86a7fa38 100644
--- a/backend/api.test/Database/Models.cs
+++ b/backend/api.test/Database/Models.cs
@@ -8,10 +8,10 @@ namespace Api.Test.Services
     public class TestPose
     {
         [Fact]
-        public void TestRotation()
+        public void TestRotationNorth()
         {
             var mockAngleAxisParameters = new EchoVector(0, 0, 1);
-            float mockAngle = MathF.PI / 2;
+            float mockAngle = 0;
 
             var expected = new Orientation()
             {
@@ -22,7 +22,51 @@ public void TestRotation()
             };
 
             Assert.Equal(
-                expected.X,
+                expected.Z,
+                new Pose(mockAngleAxisParameters, mockAngle).Orientation.Z,
+                3.0
+            );
+        }
+
+        [Fact]
+        public void TestRotationSouth()
+        {
+            var mockAngleAxisParameters = new EchoVector(0, 0, 1);
+            float mockAngle = MathF.PI;
+
+            var expected = new Orientation()
+            {
+                X = 0,
+                Y = 0,
+                Z = -0.7071F,
+                W = 0.7071F
+            };
+
+            var pose = new Pose(mockAngleAxisParameters, mockAngle);
+
+            Assert.Equal(
+                expected.Z,
+                pose.Orientation.Z,
+                3.0
+            );
+        }
+
+        [Fact]
+        public void TestNegativaRotation()
+        {
+            var mockAngleAxisParameters = new EchoVector(0, 0, 1);
+            float mockAngle = -180F * MathF.PI / 180F;
+
+            var expected = new Orientation()
+            {
+                X = 0,
+                Y = 0,
+                Z = 1F,
+                W = 0
+            };
+
+            Assert.Equal(
+                expected.Z,
                 new Pose(mockAngleAxisParameters, mockAngle).Orientation.Z,
                 3.0
             );
@@ -64,6 +108,12 @@ private static AxisAngle ConvertOrientation(Orientation orientation)
             if (orientation.Z >= 0)
                 angle = 2 * MathF.Acos(qw);
 
+            angle = (450 * MathF.PI / 180) - angle;
+
+            angle %= 2F * MathF.PI;
+
+            if (angle < 0) angle += 2F * MathF.PI;
+
             return new AxisAngle(new EchoVector(0, 0, 1), angle);
         }
 
diff --git a/backend/api/Database/Models/Pose.cs b/backend/api/Database/Models/Pose.cs
index faf216f6..53e59c16 100644
--- a/backend/api/Database/Models/Pose.cs
+++ b/backend/api/Database/Models/Pose.cs
@@ -93,8 +93,17 @@ public class Pose
 
         // Since this is a ground robot the only quaternion vector
         // that makes sense is up (0, 0, 1)
-        public Orientation AxisAngleToQuaternion(float angle)
+        // Echo representes North at 0deg and increases this value clockwise
+        // Our representation has East at 0deg with rotations anti-clockwise
+        public Orientation AxisAngleToQuaternion(float echoAngle)
         {
+            float angle;
+            echoAngle %= 2F * MathF.PI;
+
+            if (echoAngle < 0) echoAngle += 2F * MathF.PI;
+
+            angle = (450 * MathF.PI / 180) - echoAngle;
+
             var quaternion = new Orientation()
             {
                 X = 0,
@@ -102,6 +111,7 @@ public Orientation AxisAngleToQuaternion(float angle)
                 Z = MathF.Sin(angle / 2),
                 W = MathF.Cos(angle / 2)
             };
+
             return quaternion;
         }
 
@@ -125,10 +135,10 @@ float w
             Orientation = new Orientation(x_ori, y_ori, z_ori, w);
         }
 
-        public Pose(EchoVector enuPosition, float angle)
+        public Pose(EchoVector enuPosition, float echoAngle)
         {
             Position = new Position(enuPosition.East, enuPosition.North, enuPosition.Up);
-            Orientation = AxisAngleToQuaternion(angle);
+            Orientation = AxisAngleToQuaternion(echoAngle);
         }
 
         public Pose(Position position, Orientation orientation)