Implemented kinetic interpolation.

Unity/OpenSeeIKTarget.cs

@@ -7,6 +7,8 @@ public class OpenSeeIKTarget : MonoBehaviour
     [Header("Settings")]
     [Tooltip("This is the source of face tracking data.")]
     public OpenSee openSee;
+    [Tooltip("When this is set to another game object, its kinematic interpolation component will get updated from values of this.")]
+    public OpenSeeKinematicInterpolation kinematicInterpolation;
     [Tooltip("This can be used to skip the first few tracking frames.")]
     public int skipFirst = 5;
     [Tooltip("When set to true, the next rotation and translation values will be set as zero rotation and zero translation.")]
@@ -87,10 +89,12 @@ public class OpenSeeIKTarget : MonoBehaviour
     void Interpolate() {
         if (!interpolate || interpolateState < 2)
             return;
-        float t = Mathf.Clamp((float)interpolationCount / avgInterps, 0f, 0.985f);
-        transform.localPosition = Vector3.Lerp(lastT, currentT, t);
-        transform.localRotation = Quaternion.Lerp(lastR, currentR, t);
+        float t = Mathf.Clamp((float)interpolationCount / avgInterps, 0f, 1.5f);
+        transform.localPosition = Vector3.SlerpUnclamped(lastT, currentT, t);
+        transform.localRotation = Quaternion.SlerpUnclamped(lastR, currentR, t);
         interpolationCount++;
+        //transform.localPosition = new Vector3(0.3f * ((Time.time/3f) % 3f) - 0.5f, 0.3f * Mathf.Sin(Mathf.Rad2Deg * ((Time.time/5f) % 3f)), 0f);
+        //transform.localRotation = Quaternion.identity * Quaternion.AngleAxis(20f * Mathf.Sin(Mathf.Rad2Deg * ((Time.time/5f) % 3f)) - 10f, Vector3.up);
     }
 
     Quaternion MirrorQuaternion(Quaternion q) {
@@ -216,8 +220,8 @@ public class OpenSeeIKTarget : MonoBehaviour
         interpolationCount = 0;
         averageInterpolations = avgInterps;
 
-        lastT = currentT;
-        lastR = currentR;
+        lastT = transform.localPosition;
+        lastR = transform.localRotation;
         if (interpolateState > 0) {
             currentT = Vector3.Lerp(currentT, (t - dT) * translationScale, 1f - smooth);
             currentR = Quaternion.Lerp(transform.localRotation, convertedQuaternion * dR, 1f - smooth);
@@ -234,6 +238,9 @@ public class OpenSeeIKTarget : MonoBehaviour
             transform.localPosition = Vector3.Lerp(transform.localPosition, (t - dT) * translationScale, 1f - smooth);
             transform.localRotation = Quaternion.Lerp(transform.localRotation, convertedQuaternion * dR, 1f - smooth);
         }
+        if (kinematicInterpolation != null && kinematicInterpolation.gameObject != gameObject) {
+            kinematicInterpolation.UpdateKI(updated, transform.localPosition, transform.localRotation);
+        }
         
         if (driftBack) {
             if (mirrorMotion) {

Unity/OpenSeeIKTarget.cs.meta

@@ -4,7 +4,7 @@ MonoImporter:
   externalObjects: {}
   serializedVersion: 2
   defaultReferences: []
-  executionOrder: 0
+  executionOrder: -1
   icon: {instanceID: 0}
   userData: 
   assetBundleName: 

Unity/OpenSeeKinematicInterpolation.cs

@@ -0,0 +1,112 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class OpenSeeKinematicInterpolation : MonoBehaviour
+{
+    private Vector3 lastPos;
+    private Vector3 lastPosV;
+    private double lastT;
+    
+    private Vector3 currentPos;
+    private Vector3 currentPosV;
+    private double currentT = 0f;
+    
+    private Vector3 lastRot;
+    private Vector3 lastRotV;
+    
+    private Vector3 currentRot;
+    private Vector3 currentRotV;
+    
+    private float dT;
+    private Vector3 coeffPosA;
+    private Vector3 coeffPosB;
+    private Vector3 coeffRotA;
+    private Vector3 coeffRotB;
+    
+    private double updateT;
+    private double interpolateT;
+    
+    public void PrettyEuler() {
+        // Ew, but how would you do this interpolation with quaternions?
+        if (currentRot.x > 270f && lastRot.x < 90f)
+            currentRot.x -= 360f;
+        if (currentRot.y > 270f && lastRot.y < 90f)
+            currentRot.y -= 360f;
+        if (currentRot.z > 270f && lastRot.z < 90f)
+            currentRot.z -= 360f;
+        if (currentRot.x < 90f && lastRot.x > 270f)
+            lastRot.x -= 360f;
+        if (currentRot.y < 90f && lastRot.y > 270f)
+            lastRot.y -= 360f;
+        if (currentRot.z < 90f && lastRot.z > 270f)
+            lastRot.z -= 360f;
+    }
+    
+    public void UpdateKI(double newT, Vector3 newPos, Quaternion newRot) {
+        Interpolate();
+        currentPos = transform.localPosition;
+        if (interpolateT - updateT > 0) {
+            float dTNow = (float)(interpolateT - updateT);
+            currentPosV = (currentPos - lastPos) / dTNow;
+            currentRotV = (currentRot - lastRot) / dTNow;
+        } else {
+            currentPosV = Vector3.zero;
+            currentRotV = Vector3.zero;
+        }
+
+        lastPos = currentPos;
+        lastPosV = currentPosV;
+        lastRot = currentRot;
+        lastRotV = currentRotV;
+        lastT = currentT;
+        
+        currentPos = newPos;
+        currentRot = newRot.eulerAngles;
+        PrettyEuler();
+        currentT = newT;
+        dT = (float)(currentT - lastT);
+        currentPosV = (currentPos - lastPos) / dT;
+        currentRotV = (currentRot - lastRot) / dT;
+
+        float dTPow2 = Mathf.Pow(dT, 2);
+        float dTPow3 = Mathf.Pow(dT, 3);
+        coeffPosA = (- (2f*(currentPosV * dT + 2f * dT * lastPosV - 3f * currentPos + 3f * lastPos)) / dTPow2) / 2f;
+        coeffPosB = ((6f*(currentPosV * dT + dT * lastPosV - 2f * currentPos + 2f * lastPos)) / dTPow3)/6f;
+        coeffRotA = (- (2f*(currentRotV * dT + 2f * dT * lastRotV - 3f * currentRot + 3f * lastRot)) / dTPow2) / 2f;
+        coeffRotB = ((6f*(currentRotV * dT + dT * lastRotV - 2f * currentRot + 2f * lastRot)) / dTPow3)/6f;
+        
+        updateT = Time.time;
+    }
+    
+    float InterpolateValue(float dTNow, float dTNowPow2, float dTNowPow3, float a, float b, float x1, float x2, float v1, float v2) {
+        return x1 + v1 * dTNow + dTNowPow2 * a + dTNowPow3 * b;
+    }
+    
+    void Interpolate() {
+        interpolateT = Time.time;
+        if (interpolateT == updateT)
+            return;
+        float dTNow = (float)(interpolateT - updateT);
+        if (dTNow >= dT) {
+            transform.localPosition = Vector3.LerpUnclamped(lastPos, currentPos, Mathf.Min((float)(dTNow / dT), 2f));
+            transform.localEulerAngles = Vector3.LerpUnclamped(lastRot, currentRot, Mathf.Min((float)(dTNow / dT), 2f));
+        } else {
+            float dTNowPow2 = Mathf.Pow(dTNow, 2);
+            float dTNowPow3 = Mathf.Pow(dTNow, 3);
+            float x = InterpolateValue(dTNow, dTNowPow2, dTNowPow3, coeffPosA.x, coeffPosB.x, lastPos.x, currentPos.x, lastPosV.x, currentPosV.x);
+            float y = InterpolateValue(dTNow, dTNowPow2, dTNowPow3, coeffPosA.y, coeffPosB.y, lastPos.y, currentPos.y, lastPosV.y, currentPosV.y);
+            float z = InterpolateValue(dTNow, dTNowPow2, dTNowPow3, coeffPosA.z, coeffPosB.z, lastPos.z, currentPos.z, lastPosV.z, currentPosV.z);
+            transform.localPosition = new Vector3(x, y, z);
+            x = InterpolateValue(dTNow, dTNowPow2, dTNowPow3, coeffRotA.x, coeffRotB.x, lastRot.x, currentRot.x, lastRotV.x, currentRotV.x);
+            y = InterpolateValue(dTNow, dTNowPow2, dTNowPow3, coeffRotA.y, coeffRotB.y, lastRot.y, currentRot.y, lastRotV.y, currentRotV.y);
+            z = InterpolateValue(dTNow, dTNowPow2, dTNowPow3, coeffRotA.z, coeffRotB.z, lastRot.z, currentRot.z, lastRotV.z, currentRotV.z);
+            transform.localEulerAngles = new Vector3(x, y, z);
+        }
+    }
+    
+    public void LateUpdate() {
+        Interpolate();
+    }
+}

Unity/OpenSeeKinematicInterpolation.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: a4112b4be90fc9e4baee9eade65378c3
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: