From 0d0a30dedb0e92d1a0aefcdc71a5547acf8a3a44 Mon Sep 17 00:00:00 2001
From: Emiliana <38952746+emilianavt@users.noreply.github.com>
Date: Sun, 9 Feb 2020 01:25:55 +0100
Subject: [PATCH] This fixes a whole bunch of coordinate and rotation issues.

---
 Unity/OpenSeeIKTarget.cs   | 10 +++++-----
 Unity/OpenSeeShowPoints.cs | 13 ++++++++++---
 2 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/Unity/OpenSeeIKTarget.cs b/Unity/OpenSeeIKTarget.cs
index 67d957b..071d47a 100644
--- a/Unity/OpenSeeIKTarget.cs
+++ b/Unity/OpenSeeIKTarget.cs
@@ -8,8 +8,8 @@ public class OpenSeeIKTarget : MonoBehaviour
     public float translationScale = 0.3f;
     public float smooth = 0.2f;
     double updated = 0.0f;
-    Quaternion dR;
-    Vector3 dT;
+    Quaternion dR = Quaternion.Euler(0f, 0f, -90f);
+    Vector3 dT = new Vector3(0f, 0f, 0f);
 
     void Update()
     {
@@ -31,19 +31,19 @@ public class OpenSeeIKTarget : MonoBehaviour
             return;
         }
         
-        // new Quaternion(-openSeeData[idx].rawQuaternion.x, -openSeeData[idx].rawQuaternion.y, openSeeData[idx].rawQuaternion.z, openSeeData[idx].rawQuaternion.w); // Good except for the head tilt in the start being flipped
         Quaternion convertedQuaternion = new Quaternion(-openSeeData[idx].rawQuaternion.y, -openSeeData[idx].rawQuaternion.x, openSeeData[idx].rawQuaternion.z, openSeeData[idx].rawQuaternion.w);
 
         if (calibrate) {
             calibrate = false;
-            dR = convertedQuaternion;//Quaternion.Euler(-openSeeData[idx].rotation.x, openSeeData[idx].rotation.y, -openSeeData[0].rotation.z);
+            dR = convertedQuaternion;
             dR = Quaternion.Inverse(dR);
             dT = openSeeData[idx].translation;
         }
         
         Vector3 v = openSeeData[idx].translation - dT;
+        v.x = -v.x;
         v.z = -v.z;
         transform.localPosition = Vector3.Lerp(transform.localPosition, v * translationScale, 1f - smooth);
-        transform.localRotation = Quaternion.Lerp(transform.localRotation, /*Quaternion.Euler(-openSeeData[idx].rotation.x, openSeeData[idx].rotation.y, -openSeeData[idx].rotation.z)*/ convertedQuaternion * dR, 1f - smooth);
+        transform.localRotation = Quaternion.Lerp(transform.localRotation, convertedQuaternion * dR, 1f - smooth);
     }
 }
diff --git a/Unity/OpenSeeShowPoints.cs b/Unity/OpenSeeShowPoints.cs
index 383eb59..53ae6f4 100644
--- a/Unity/OpenSeeShowPoints.cs
+++ b/Unity/OpenSeeShowPoints.cs
@@ -49,17 +49,24 @@ public class OpenSeeShowPoints : MonoBehaviour {
                 if (openSeeData[0].got3DPoints && openSeeData[0].confidence[i] > minConfidence) {
                     Renderer renderer = gameObjects[i].GetComponent<Renderer>();
                     renderer.material.SetColor("_Color", Color.Lerp(Color.red, Color.green, openSeeData[0].confidence[i]));
-                    gameObjects[i].transform.localPosition = openSeeData[0].points3D[i];
+                    Vector3 pt = openSeeData[0].points3D[i];
+                    pt.x = -pt.x;
+                    gameObjects[i].transform.localPosition = pt;
                 } else {
                     Renderer renderer = gameObjects[i].GetComponent<Renderer>();
                     renderer.material.SetColor("_Color", Color.cyan);
                 }
             }
             if (applyTranslation) {
-                transform.position = openSeeData[0].translation;
+                Vector3 v = openSeeData[0].translation;
+                v.x = -v.x;
+                v.z = -v.z;
+                transform.localPosition = v;
             }
             if (applyRotation) {
-                transform.eulerAngles = openSeeData[0].rotation;
+                Quaternion offset = Quaternion.Euler(0f, 0f, -90f);
+                Quaternion convertedQuaternion = new Quaternion(-openSeeData[0].rawQuaternion.y, -openSeeData[0].rawQuaternion.x, openSeeData[0].rawQuaternion.z, openSeeData[0].rawQuaternion.w) * offset;
+                transform.localRotation = convertedQuaternion;
             }
         } else {
             centerBall.gameObject.SetActive(false);