From eaaf036e7650faa6e6bdeac652395e76a6502b40 Mon Sep 17 00:00:00 2001 From: Emiliana <38952746+emilianavt@users.noreply.github.com> Date: Sat, 26 Sep 2020 20:21:46 +0200 Subject: [PATCH] Disabled ThunderSVM loading and added better interpolation. --- Examples/OpenSeeVRMDriver.cs | 265 ++++++++++++++----------- Unity/OpenSeeExpression.cs | 6 +- Unity/OpenSeeIKTarget.cs | 121 ++++++++++- Unity/OpenSeeKinematicInterpolation.cs | 28 +-- 4 files changed, 270 insertions(+), 150 deletions(-) diff --git a/Examples/OpenSeeVRMDriver.cs b/Examples/OpenSeeVRMDriver.cs index 7ee85bb..13a6d29 100644 --- a/Examples/OpenSeeVRMDriver.cs +++ b/Examples/OpenSeeVRMDriver.cs @@ -39,6 +39,9 @@ public class OpenSeeVRMDriver : MonoBehaviour { public float blinkSmoothing = 0.75f; [Tooltip("With tracked eye blinking, the eyes will close when looking down. Enable this to compensate this by modifying thresholds.")] public bool lookDownCompensation = false; + [Tooltip("This is an angle value that can be used to adjust the angle from which a face is considered to be looking downwards.")] + [Range(-10, 20)] + public float lookDownAdjustment = 0f; [Tooltip("When enabled, auto blinking will be enabled while looking down.")] public bool autoBlinkLookingDown = false; [Tooltip("When enabled, the blink state for both eyes is permanently linked together.")] @@ -161,21 +164,20 @@ public class OpenSeeVRMDriver : MonoBehaviour { private float turnLeftBoundaryAngle = -30f; private float turnRightBoundaryAngle = 20f; private float turnDownBoundaryAngle = 205f; + private float wasLookingDown = 0f; private double lastGaze = 0f; private float lastLookUpDown = 0f; private float lastLookLeftRight = 0f; private float currentLookUpDown = 0f; private float currentLookLeftRight = 0f; - private int interpolationCount = 0; - private int interpolationState = 0; + private TimeInterpolate gazeInterpolate; private float[] lastVisemeValues; private double lastMouth = 0f; private float[] lastMouthStates; private float[] currentMouthStates; - private int mouthInterpolationCount = 0; - private int mouthInterpolationState = 0; + private TimeInterpolate mouthInterpolate; private double startedSilVisemes = -10; private double silVisemeHybridThreshold = 0.3; @@ -186,8 +188,7 @@ public class OpenSeeVRMDriver : MonoBehaviour { private float lastBlinkRight = 0f; private float currentBlinkLeft = 0f; private float currentBlinkRight = 0f; - private int blinkInterpolationCount = 0; - private int blinkInterpolationState = 0; + private TimeInterpolate blinkInterpolate; private bool overridden = false; private OpenSeeVRMExpression toggledExpression = null; @@ -209,11 +210,39 @@ public class OpenSeeVRMDriver : MonoBehaviour { private float lastBrowUpDown = 0f; private float[] lastBrowStates; private float[] currentBrowStates; - private int browInterpolationCount = 0; - private int browInterpolationState = 0; + private TimeInterpolate browInterpolate; private BlendShapeKey[] browClips; private float lastAudioTime = -1f; + + private class TimeInterpolate { + private float interpolateT = 0f; + private float updateT = 0f; + private double lastT = 0f; + private double currentT = 0f; + private float clamp = 1f; + private int gotData = 0; + + public TimeInterpolate() {} + public TimeInterpolate(float clamp) { + this.clamp = clamp; + } + + public void UpdateTime(double nowT) { + updateT = Time.time; + lastT = currentT; + currentT = nowT; + if (gotData < 2) + gotData++; + } + + public float Interpolate() { + interpolateT = Time.time; + if (interpolateT == updateT || gotData < 2) + return 0f; + return Mathf.Min((interpolateT - updateT) / (float)(currentT - lastT), clamp); + } + } private Dictionary catsData = null; void InitCatsData() { @@ -313,8 +342,32 @@ public class OpenSeeVRMDriver : MonoBehaviour { if (openSeeData == null || openSeeData.features == null) return; + if (lastMouthStates == null) + lastMouthStates = new float[]{0f, 0f, 0f, 0f, 0f}; + if (currentMouthStates == null) + currentMouthStates = new float[]{0f, 0f, 0f, 0f, 0f}; + + if (mouthInterpolate == null) + mouthInterpolate = new TimeInterpolate(); + float t = mouthInterpolate.Interpolate(); + + if (currentExpression != null && !currentExpressionEnableVisemes) + return; + + float expressionFactor = 1f; + if (currentExpression != null) + expressionFactor = currentExpressionVisemeFactor; + + for (int i = 0; i < 5; i++) { + float interpolated = Mathf.Lerp(lastMouthStates[i], currentMouthStates[i], t); + float result = interpolated * expressionFactor * visemeFactor; + if (result > 0f) + vrmBlendShapeProxy.AccumulateValue(visemePresetMap[i], result); + } + if (lastMouth < openSeeData.time) { lastMouth = openSeeData.time; + mouthInterpolate.UpdateTime(lastMouth); float open = openSeeData.features.MouthOpen; float wide = openSeeData.features.MouthWide; float[] mouthStates = new float[]{0f, 0f, 0f, 0f, 0f}; @@ -369,37 +422,11 @@ public class OpenSeeVRMDriver : MonoBehaviour { } } while (false); - if (mouthInterpolationState == 2) { - for (int i = 0; i < 5; i++) { - mouthStates[i] = Mathf.Lerp(currentMouthStates[i], mouthStates[i], 1f - mouthSmoothing); - } + for (int i = 0; i < 5; i++) { + lastMouthStates[i] = Mathf.Lerp(lastMouthStates[i], currentMouthStates[i], t); + mouthStates[i] = Mathf.Lerp(lastMouthStates[i], mouthStates[i], 1f - mouthSmoothing); } - - lastMouthStates = currentMouthStates; currentMouthStates = mouthStates; - if (mouthInterpolationState < 2) - mouthInterpolationState++; - mouthInterpolationCount = 0; - } - - if (mouthInterpolationState < 2) - return; - - float t = Mathf.Clamp((float)mouthInterpolationCount / openSeeIKTarget.averageInterpolations, 0f, 0.985f); - mouthInterpolationCount++; - - if (currentExpression != null && !currentExpressionEnableVisemes) - return; - - float expressionFactor = 1f; - if (currentExpression != null) - expressionFactor = currentExpressionVisemeFactor; - - for (int i = 0; i < 5; i++) { - float interpolated = Mathf.Lerp(lastMouthStates[i], currentMouthStates[i], t); - float result = interpolated * expressionFactor * visemeFactor; - if (result > 0f) - vrmBlendShapeProxy.AccumulateValue(visemePresetMap[i], result); } } @@ -411,6 +438,29 @@ public class OpenSeeVRMDriver : MonoBehaviour { if ((browClips == null && (faceMesh == null || faceType < 0)) || openSeeData == null) { return; } + if (browInterpolate == null) + browInterpolate = new TimeInterpolate(); + + float t = browInterpolate.Interpolate(); + float strength = eyebrowStrength; + if (currentExpression != null) + strength *= currentExpressionEyebrowWeight; + + if (browUpIndex > -1 && strength > 0f) + faceMesh.SetBlendShapeWeight(browUpIndex, strength * Mathf.Lerp(lastBrowStates[0], currentBrowStates[0], t)); + if (browDownIndex > -1 && strength > 0f) + faceMesh.SetBlendShapeWeight(browDownIndex, strength * Mathf.Lerp(lastBrowStates[1], currentBrowStates[1], t)); + if (browAngryIndex > -1 && strength > 0f) + faceMesh.SetBlendShapeWeight(browAngryIndex, strength * Mathf.Lerp(lastBrowStates[2], currentBrowStates[2], t)); + if (browSorrowIndex > -1 && strength > 0f) + faceMesh.SetBlendShapeWeight(browSorrowIndex, strength * Mathf.Lerp(lastBrowStates[3], currentBrowStates[3], t)); + if (browClips != null && strength > 0f) { + if (lastBrowUpDown > 0f) + vrmBlendShapeProxy.AccumulateValue(browClips[0], strength * Mathf.Lerp(lastBrowStates[0], currentBrowStates[0], t)); + else if (lastBrowUpDown < 0f) + vrmBlendShapeProxy.AccumulateValue(browClips[1], strength * Mathf.Lerp(lastBrowStates[1], currentBrowStates[1], t)); + } + if (lastBrows < openSeeData.time) { lastBrows = openSeeData.time; float upDownStrength = (openSeeData.features.EyebrowUpDownLeft + openSeeData.features.EyebrowUpDownRight) / 2f; @@ -448,15 +498,12 @@ public class OpenSeeVRMDriver : MonoBehaviour { } if (eyebrowIsMoving > 0) eyebrowIsMoving--; - upDownStrength = Mathf.Lerp(lastBrowUpDown, upDownStrength, 1f - eyebrowSmoothing); + upDownStrength = Mathf.LerpUnclamped(lastBrowUpDown, upDownStrength, 1f - eyebrowSmoothing); lastBrowUpDown = upDownStrength; - if (browInterpolationState < 2) - browInterpolationState++; - browInterpolationCount = 0; - for (int i = 0; i < 4; i++) - lastBrowStates[i] = currentBrowStates[i]; + lastBrowStates[i] = Mathf.Lerp(lastBrowStates[i], currentBrowStates[i], t); + browInterpolate.UpdateTime(openSeeData.time); if (browClips != null) { currentBrowStates[0] = upDownStrength; @@ -498,28 +545,6 @@ public class OpenSeeVRMDriver : MonoBehaviour { } } } - - float t = Mathf.Clamp((float)browInterpolationCount / openSeeIKTarget.averageInterpolations, 0f, 0.985f); - browInterpolationCount++; - - float strength = eyebrowStrength; - if (currentExpression != null) - strength *= currentExpressionEyebrowWeight; - - if (browUpIndex > -1 && strength > 0f) - faceMesh.SetBlendShapeWeight(browUpIndex, strength * Mathf.Lerp(lastBrowStates[0], currentBrowStates[0], t)); - if (browDownIndex > -1 && strength > 0f) - faceMesh.SetBlendShapeWeight(browDownIndex, strength * Mathf.Lerp(lastBrowStates[1], currentBrowStates[1], t)); - if (browAngryIndex > -1 && strength > 0f) - faceMesh.SetBlendShapeWeight(browAngryIndex, strength * Mathf.Lerp(lastBrowStates[2], currentBrowStates[2], t)); - if (browSorrowIndex > -1 && strength > 0f) - faceMesh.SetBlendShapeWeight(browSorrowIndex, strength * Mathf.Lerp(lastBrowStates[3], currentBrowStates[3], t)); - if (browClips != null && strength > 0f) { - if (lastBrowUpDown > 0f) - vrmBlendShapeProxy.AccumulateValue(browClips[0], strength * Mathf.Lerp(lastBrowStates[0], currentBrowStates[0], t)); - else if (lastBrowUpDown < 0f) - vrmBlendShapeProxy.AccumulateValue(browClips[1], strength * Mathf.Lerp(lastBrowStates[1], currentBrowStates[1], t)); - } } void FindFaceMesh() { @@ -924,38 +949,18 @@ public class OpenSeeVRMDriver : MonoBehaviour { } void UpdateGaze() { - if (openSeeData == null || vrmBlendShapeProxy == null || openSeeIKTarget == null || !(openSeeIKTarget.averageInterpolations > 0f)) + if (openSeeData == null || vrmBlendShapeProxy == null || openSeeIKTarget == null) return; float lookUpDown = currentLookUpDown; float lookLeftRight = currentLookLeftRight; - - if (lastGaze < openSeeData.time) { - GetLookParameters(ref lookLeftRight, ref lookUpDown, false, 66, 36, 39, 37, 38, 41, 40); - GetLookParameters(ref lookLeftRight, ref lookUpDown, true, 67, 42, 45, 43, 44, 47, 46); - - lookLeftRight = Mathf.Lerp(currentLookLeftRight, lookLeftRight, 1f - gazeSmoothing); - lookUpDown = Mathf.Lerp(currentLookUpDown, lookUpDown, 1f - gazeSmoothing); - - if (Mathf.Abs(lookLeftRight - currentLookLeftRight) + Mathf.Abs(lookUpDown - currentLookUpDown) > gazeStabilizer) { - lastLookLeftRight = currentLookLeftRight; - lastLookUpDown = currentLookUpDown; - currentLookLeftRight = lookLeftRight; - currentLookUpDown = lookUpDown; - lastGaze = openSeeData.time; - if (interpolationState < 2) - interpolationState++; - interpolationCount = 0; - } - } - if (interpolationState < 2) - return; - - float t = Mathf.Clamp((float)interpolationCount / openSeeIKTarget.averageInterpolations, 0f, 0.985f); + if (gazeInterpolate == null) + gazeInterpolate = new TimeInterpolate(); + float t = gazeInterpolate.Interpolate(); + lookLeftRight = Mathf.Lerp(lastLookLeftRight, currentLookLeftRight, t); lookUpDown = Mathf.Lerp(lastLookUpDown, currentLookUpDown, t); - interpolationCount++; if (!openSeeIKTarget.mirrorMotion) lookLeftRight = -lookLeftRight; @@ -989,6 +994,23 @@ public class OpenSeeVRMDriver : MonoBehaviour { leftEye.localRotation = rotation; } } + + if (lastGaze < openSeeData.time) { + GetLookParameters(ref lookLeftRight, ref lookUpDown, false, 66, 36, 39, 37, 38, 41, 40); + GetLookParameters(ref lookLeftRight, ref lookUpDown, true, 67, 42, 45, 43, 44, 47, 46); + + lookLeftRight = Mathf.Lerp(currentLookLeftRight, lookLeftRight, 1f - gazeSmoothing); + lookUpDown = Mathf.Lerp(currentLookUpDown, lookUpDown, 1f - gazeSmoothing); + + if (Mathf.Abs(lookLeftRight - currentLookLeftRight) + Mathf.Abs(lookUpDown - currentLookUpDown) > gazeStabilizer) { + lastLookLeftRight = Mathf.Lerp(lastLookLeftRight, currentLookLeftRight, t); + lastLookUpDown = Mathf.Lerp(lastLookUpDown, currentLookUpDown, t); + currentLookLeftRight = lookLeftRight; + currentLookUpDown = lookUpDown; + lastGaze = openSeeData.time; + gazeInterpolate.UpdateTime(lastGaze); + } + } } void BlinkEyes() { @@ -1011,27 +1033,54 @@ public class OpenSeeVRMDriver : MonoBehaviour { upDownAngle += 360f; while (upDownAngle >= 360f) upDownAngle -= 360f; - if (lookDownCompensation && upDownAngle > turnDownBoundaryAngle && autoBlinkLookingDown) { + if (lookDownCompensation && upDownAngle > turnDownBoundaryAngle - lookDownAdjustment - wasLookingDown && autoBlinkLookingDown) { + if (!(wasLookingDown > 0f)) + eyeBlinker.Blink(); float blink = eyeBlinker.Blink(); vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink), blink); return; } + if (blinkInterpolate == null) + blinkInterpolate = new TimeInterpolate(); + float t = blinkInterpolate.Interpolate(); + + left = Mathf.Lerp(lastBlinkLeft, currentBlinkLeft, t); + right = Mathf.Lerp(lastBlinkRight, currentBlinkRight, t); + + if (openSeeIKTarget.mirrorMotion) { + float tmp = left; + left = right; + right = tmp; + } + + if (linkBlinks && !allowWinking) { + float v = Mathf.Max(left, right); + vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink), v); + } else { + vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink_R), right); + vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink_L), left); + } + if (openSeeData != null && lastBlink < openSeeData.time) { lastBlink = openSeeData.time; + blinkInterpolate.UpdateTime(lastBlink); float openThreshold = eyeOpenedThreshold; float closedThreshold = eyeClosedThreshold; if (openSeeData.rawEuler.y < turnLeftBoundaryAngle || openSeeData.rawEuler.y > turnRightBoundaryAngle) openThreshold = Mathf.Lerp(eyeOpenedThreshold, eyeClosedThreshold, 0.4f); - if (lookDownCompensation && upDownAngle > turnDownBoundaryAngle) { + if (lookDownCompensation && upDownAngle > turnDownBoundaryAngle - lookDownAdjustment - wasLookingDown) { openThreshold = Mathf.Lerp(eyeOpenedThreshold, eyeClosedThreshold, 0.85f); closedThreshold = Mathf.Lerp(eyeClosedThreshold, 0f, 0.6f); - if (upDownAngle > turnDownBoundaryAngle + 10f) { + if (upDownAngle > turnDownBoundaryAngle - lookDownAdjustment + 10f) { openThreshold = 0.1f; closedThreshold = 0f; } + wasLookingDown = 1.5f; + } else { + wasLookingDown = 0f; } if (openSeeData.rightEyeOpen > openThreshold) @@ -1061,8 +1110,8 @@ public class OpenSeeVRMDriver : MonoBehaviour { } } - lastBlinkLeft = currentBlinkLeft; - lastBlinkRight = currentBlinkRight; + lastBlinkLeft = Mathf.Lerp(lastBlinkLeft, currentBlinkLeft, t); + lastBlinkRight = Mathf.Lerp(lastBlinkRight, currentBlinkRight, t); currentBlinkLeft = Mathf.Lerp(currentBlinkLeft, left, 1f - blinkSmoothing); currentBlinkRight = Mathf.Lerp(currentBlinkRight, right, 1f - blinkSmoothing); @@ -1074,32 +1123,6 @@ public class OpenSeeVRMDriver : MonoBehaviour { currentBlinkLeft = 1f; if (right > 0.99999f) currentBlinkRight = 1f; - - if (blinkInterpolationState < 2) - blinkInterpolationState++; - } - - if (blinkInterpolationState < 2) - return; - - float t = Mathf.Clamp((float)blinkInterpolationCount / openSeeIKTarget.averageInterpolations, 0f, 0.985f); - blinkInterpolationCount++; - - left = Mathf.Lerp(lastBlinkLeft, currentBlinkLeft, t); - right = Mathf.Lerp(lastBlinkRight, currentBlinkRight, t); - - if (openSeeIKTarget.mirrorMotion) { - float tmp = left; - left = right; - right = tmp; - } - - if (linkBlinks && !allowWinking) { - float v = Mathf.Max(left, right); - vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink), v); - } else { - vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink_R), right); - vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink_L), left); } } } diff --git a/Unity/OpenSeeExpression.cs b/Unity/OpenSeeExpression.cs index 2eaa988..1c05057 100644 --- a/Unity/OpenSeeExpression.cs +++ b/Unity/OpenSeeExpression.cs @@ -166,10 +166,10 @@ public class OpenSeeExpression : MonoBehaviour oser = formatter.Deserialize(gzipStream) as OpenSeeExpressionRepresentation; } expressions = oser.expressions; - if (oser.thunderSVM) + /*if (oser.thunderSVM) model = new ThunderSVMModel(oser.modelBytes); - else - model = new SVMModel(oser.modelBytes); + else*/ + model = new SVMModel(oser.modelBytes); classLabels = oser.classLabels; indices = oser.indices; //pointSelection = oser.pointSelection; diff --git a/Unity/OpenSeeIKTarget.cs b/Unity/OpenSeeIKTarget.cs index 6cd6247..12d91d1 100644 --- a/Unity/OpenSeeIKTarget.cs +++ b/Unity/OpenSeeIKTarget.cs @@ -22,6 +22,8 @@ public class OpenSeeIKTarget : MonoBehaviour [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.")] [Range(0, 1)] public float smooth = 0.2f; + [Tooltip("When set to true, a Kalman filter is used for smoothing.")] + public bool kalman = false; [Tooltip("When enabled, tracking will lag by one video capture frame, but the captured motion information is interpolated between frames, making things look smoother and less jittery and jumpy.")] public bool interpolate = true; [Tooltip("When set to true, the transform will be updated in FixedUpdate, otherwise it will be updated in Update.")] @@ -82,9 +84,65 @@ public class OpenSeeIKTarget : MonoBehaviour private bool skippedPosition = false; private float skipStartTimePosition = -1f; + // Kalman + private float frameTime = 0f; + private bool kalmanInit = false; + private float kalmanSmooth = -1f; + private KF kalmanPVX; + private KF kalmanPVY; + private KF kalmanPVZ; + private KF kalmanRVX; + private KF kalmanRVY; + private KF kalmanRVZ; + private Vector3 kalmanLastRot = Vector3.zero; + public void Calibrate() { calibrate = true; } + + public static Vector3 PrettyEuler(Vector3 last, Vector3 current) { + // Ew, but how would you do componentwise filtering/interpolation with quaternions? + if (Mathf.Abs(current.x - last.x) > Mathf.Abs((current.x - 360f) - last.x)) + current.x -= 360f; + else if (Mathf.Abs(current.x - last.x) > Mathf.Abs((current.x + 360f) - last.x)) + current.x += 360f; + if (Mathf.Abs(current.y - last.y) > Mathf.Abs((current.y - 360f) - last.y)) + current.y -= 360f; + else if (Mathf.Abs(current.y - last.y) > Mathf.Abs((current.y + 360f) - last.y)) + current.x += 360f; + if (Mathf.Abs(current.z - last.z) > Mathf.Abs((current.z - 360f) - last.z)) + current.z -= 360f; + else if (Mathf.Abs(current.z - last.z) > Mathf.Abs((current.z + 360f) - last.z)) + current.z += 360f; + return current; + } + + Vector3 FilterPos(Vector3 a, Vector3 b) { + if (!kalman || frameTime > 1f) + return Vector3.Lerp(a, b, 1f - smooth); + else { + Vector3 v = (b - a) / frameTime; + v.x = kalmanPVX.Update(v.x); + v.y = kalmanPVY.Update(v.y); + v.z = kalmanPVZ.Update(v.z); + return a + v * frameTime; + } + } + + Quaternion FilterRot(Quaternion a, Quaternion b) { + if (!kalman || frameTime > 1f) + return Quaternion.Lerp(a, b, 1f - smooth); + else { + Vector3 rotA = PrettyEuler(kalmanLastRot, a.eulerAngles); + Vector3 rotB = PrettyEuler(rotA, b.eulerAngles); + kalmanLastRot = rotA; + Vector3 v = Mathf.Deg2Rad * (rotB - rotA) / frameTime; + v.x = kalmanRVX.Update(v.x); + v.y = kalmanRVY.Update(v.y); + v.z = kalmanRVZ.Update(v.z); + return Quaternion.Euler(rotA + Mathf.Rad2Deg * v * frameTime); + } + } void Interpolate() { if (!interpolate || interpolateState < 2) @@ -110,6 +168,7 @@ public class OpenSeeIKTarget : MonoBehaviour if (openSeeData == null || openSeeData.fit3DError > openSee.maxFit3DError) return; if (openSeeData.time > updated) { + frameTime = (float)(openSeeData.time - updated); updated = openSeeData.time; trackingFrames++; if (trackingFrames < skipFirst) @@ -118,6 +177,25 @@ public class OpenSeeIKTarget : MonoBehaviour Interpolate(); return; } + + if (!kalmanInit) { + kalmanPVX = new KF(smooth); + kalmanPVY = new KF(smooth); + kalmanPVZ = new KF(smooth); + kalmanRVX = new KF(smooth); + kalmanRVY = new KF(smooth); + kalmanRVZ = new KF(smooth); + kalmanInit = true; + } + if (smooth != kalmanSmooth) { + kalmanPVX.SetSmoothness(smooth); + kalmanPVY.SetSmoothness(smooth); + kalmanPVZ.SetSmoothness(smooth); + kalmanRVX.SetSmoothness(smooth); + kalmanRVY.SetSmoothness(smooth); + kalmanRVZ.SetSmoothness(smooth); + kalmanSmooth = smooth; + } Quaternion convertedQuaternion = new Quaternion(-openSeeData.rawQuaternion.y, -openSeeData.rawQuaternion.x, openSeeData.rawQuaternion.z, openSeeData.rawQuaternion.w); Vector3 t = openSeeData.translation; @@ -219,12 +297,12 @@ public class OpenSeeIKTarget : MonoBehaviour avgInterps = Mathf.Lerp(avgInterps, (float)interpolationCount, 0.15f); interpolationCount = 0; averageInterpolations = avgInterps; - + 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); + currentT = FilterPos(currentT, (t - dT) * translationScale); + currentR = FilterRot(transform.localRotation, convertedQuaternion * dR); } else { currentT = (t - dT) * translationScale; currentR = convertedQuaternion * dR; @@ -235,11 +313,14 @@ public class OpenSeeIKTarget : MonoBehaviour if (interpolate) { Interpolate(); } else { - transform.localPosition = Vector3.Lerp(transform.localPosition, (t - dT) * translationScale, 1f - smooth); - transform.localRotation = Quaternion.Lerp(transform.localRotation, convertedQuaternion * dR, 1f - smooth); + transform.localPosition = FilterPos(transform.localPosition, (t - dT) * translationScale); + transform.localRotation = FilterRot(transform.localRotation, convertedQuaternion * dR); } if (kinematicInterpolation != null && kinematicInterpolation.gameObject != gameObject) { - kinematicInterpolation.UpdateKI(updated, transform.localPosition, transform.localRotation); + if (interpolate) + kinematicInterpolation.UpdateKI(updated, currentT, currentR); + else + kinematicInterpolation.UpdateKI(updated, transform.localPosition, transform.localRotation); } if (driftBack) { @@ -266,6 +347,34 @@ public class OpenSeeIKTarget : MonoBehaviour if (!fixedUpdate) RunUpdate(); } + + class KF { + private float R, P, Q, x; + + public KF(float smooth) { + P = 1f; + x = 0f; + SetSmoothness(smooth); + } + + public void SetSmoothness(float smooth) { + P = 1f; + Q = 0.3f;// * (Mathf.Exp(3f * smooth) + 1f); + R = 0.000001f + Mathf.Exp(smooth * 5f); + } + + public float Update(float input) { + //x = x * A; + //P = A * P * A; + + float K = P / (P + R); + x = x + K * (input - x); + //Debug.Log("P = " + P + " 1-K = " + (1f - K)); + P = (1f - K) * P + Q; + + return x; + } + } } } \ No newline at end of file diff --git a/Unity/OpenSeeKinematicInterpolation.cs b/Unity/OpenSeeKinematicInterpolation.cs index 6591a1d..99b7034 100644 --- a/Unity/OpenSeeKinematicInterpolation.cs +++ b/Unity/OpenSeeKinematicInterpolation.cs @@ -3,6 +3,8 @@ using System.Collections; using System.Collections.Generic; using UnityEngine; +namespace OpenSee { + public class OpenSeeKinematicInterpolation : MonoBehaviour { private Vector3 lastPos; @@ -13,7 +15,7 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour private Vector3 currentPosV; private double currentT = 0f; - private Vector3 lastRot; + private Vector3 lastRot = Vector3.zero; private Vector3 lastRotV; private Vector3 currentRot; @@ -28,27 +30,12 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour 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; + currentRot = OpenSeeIKTarget.PrettyEuler(currentRot, transform.localRotation.eulerAngles); + float dTNow = (float)(interpolateT - updateT); if (interpolateT - updateT > 0) { - float dTNow = (float)(interpolateT - updateT); currentPosV = (currentPos - lastPos) / dTNow; currentRotV = (currentRot - lastRot) / dTNow; } else { @@ -63,8 +50,7 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour lastT = currentT; currentPos = newPos; - currentRot = newRot.eulerAngles; - PrettyEuler(); + currentRot = OpenSeeIKTarget.PrettyEuler(lastRot, newRot.eulerAngles); currentT = newT; dT = (float)(currentT - lastT); currentPosV = (currentPos - lastPos) / dT; @@ -109,4 +95,6 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour public void LateUpdate() { Interpolate(); } +} + } \ No newline at end of file