diff --git a/Unity/OpenSeeIKTarget.cs b/Unity/OpenSeeIKTarget.cs index 41d85d6..6cd6247 100644 --- a/Unity/OpenSeeIKTarget.cs +++ b/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) { @@ -210,14 +214,14 @@ public class OpenSeeIKTarget : MonoBehaviour convertedQuaternion = MirrorQuaternion(convertedQuaternion); t = MirrorTranslation(t); } - + if (interpolateState > 1) avgInterps = Mathf.Lerp(avgInterps, (float)interpolationCount, 0.15f); 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) { diff --git a/Unity/OpenSeeIKTarget.cs.meta b/Unity/OpenSeeIKTarget.cs.meta index 2507d88..94933f1 100644 --- a/Unity/OpenSeeIKTarget.cs.meta +++ b/Unity/OpenSeeIKTarget.cs.meta @@ -4,7 +4,7 @@ MonoImporter: externalObjects: {} serializedVersion: 2 defaultReferences: [] - executionOrder: 0 + executionOrder: -1 icon: {instanceID: 0} userData: assetBundleName: diff --git a/Unity/OpenSeeKinematicInterpolation.cs b/Unity/OpenSeeKinematicInterpolation.cs new file mode 100644 index 0000000..6591a1d --- /dev/null +++ b/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(); + } +} \ No newline at end of file diff --git a/Unity/OpenSeeKinematicInterpolation.cs.meta b/Unity/OpenSeeKinematicInterpolation.cs.meta new file mode 100644 index 0000000..571dd6e --- /dev/null +++ b/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: