From de16fc9ff9f92735bd6a779f0433e81f17347727 Mon Sep 17 00:00:00 2001
From: Emiliana <38952746+emilianavt@users.noreply.github.com>
Date: Thu, 13 Feb 2020 20:18:05 +0100
Subject: [PATCH] The IK target component can now mirror the pose, which can be
 convenient.

---
 .gitignore               |  3 ++-
 Unity/OpenSeeIKTarget.cs | 24 +++++++++++++++++++++++-
 2 files changed, 25 insertions(+), 2 deletions(-)

diff --git a/.gitignore b/.gitignore
index db667b6..aac297e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,4 +4,5 @@ Binary/*
 Samples/*
 *.zip
 *.7z
-CSharp/*
\ No newline at end of file
+CSharp/*
+OpenSeeDemo/*
\ No newline at end of file
diff --git a/Unity/OpenSeeIKTarget.cs b/Unity/OpenSeeIKTarget.cs
index fdb0acc..484c324 100644
--- a/Unity/OpenSeeIKTarget.cs
+++ b/Unity/OpenSeeIKTarget.cs
@@ -11,6 +11,8 @@ public class OpenSeeIKTarget : MonoBehaviour
     public bool calibrate = true;
     [Tooltip("This sets which face id to look for in the OpenSee data.")]
     public int faceId = 0;
+    [Tooltip("When this is enabled, rotations and movement are mirrored.")]
+    public bool mirrorMotion = false;
     [Tooltip("Often, the translation vector's scale is too high. Setting this somewhere between 0.05 to 0.3 seems stabilize things, but it also reduces the range of motion.")]
     public float translationScale = 0.3f;
     [Tooltip("This smoothed out the detected motion. Values can be between 0 (no smoothing) and 1 (no motion). The appropriate value probably depends on the camera's frame rate.")]
@@ -38,6 +40,7 @@ public class OpenSeeIKTarget : MonoBehaviour
     private Vector3 lastT;
     private Quaternion currentR;
     private Vector3 currentT;
+    private bool lastMirror = false;
     
     void Interpolate() {
         if (!interpolate || interpolateState < 2)
@@ -47,6 +50,14 @@ public class OpenSeeIKTarget : MonoBehaviour
         transform.localRotation = Quaternion.Lerp(lastR, currentR, t);
         interpolationCount++;
     }
+    
+    Quaternion MirrorQuaternion(Quaternion q) {
+        return new Quaternion(-q.x, q.y, q.z, -q.w);
+    }
+    
+    Vector3 MirrorTranslation(Vector3 v) {
+        return new Vector3(-v.x, v.y, v.z);
+    }
 
     void RunUpdate() {
         var openSeeData = openSee.trackingData;
@@ -74,13 +85,24 @@ public class OpenSeeIKTarget : MonoBehaviour
         t.z = -t.z;
 
         if (calibrate) {
-            calibrate = false;
             dR = convertedQuaternion;
             dR = Quaternion.Inverse(dR);
             dT = t;
             rotationOffset = new Vector3(dR.eulerAngles.x, dR.eulerAngles.y, dR.eulerAngles.z);
             translationOffset = new Vector3(dT.x, dT.y, dT.z);
         }
+        
+        if (mirrorMotion != lastMirror || (mirrorMotion && calibrate)) {
+            dR = Quaternion.Inverse(MirrorQuaternion(Quaternion.Inverse(dR)));
+            dT = MirrorTranslation(dT);
+            lastMirror = mirrorMotion;
+        }
+        calibrate = false;
+        
+        if (mirrorMotion) {
+            convertedQuaternion = MirrorQuaternion(convertedQuaternion);
+            t = MirrorTranslation(t);
+        }
 
         if (interpolateState > 1)
             avgInterps = Mathf.Lerp(avgInterps, (float)interpolationCount, 0.15f);