Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
/**
FinalIKを使ったLeapMotion Orion用HandController
(VRIKバージョン)
Author: MiyuMiyu (https://twitter.com/miyumiyuna5)
Source: https://qiita.com/miyumiyu/items/72b965df46a79f3ec523
Modified by: Emiliana (https://twitter.com/Emiliana_vt)
Modifications: Updated for current SDK version, supports hand position reset on tracking loss, hand mirroring and interpolation.
*/
/*
`LeapMotion\Core\Scripts\Hands\RiggedHands.cs` needs to be modified slightly. `jointList` and `localRotations` need to be set to `public` rather than `private`.
`LeapMotion\Core\Scripts\LeapProvider.cs` needs to have the following method added to the `LeapProvider` class:
public void ClearHandlers() {
OnFixedFrame = null;
OnUpdateFrame = null;
}
*/
using AutoRigType = Leap.Unity.LeapHandsAutoRig;
using RiggedHandType = Leap.Unity.RiggedHand;
using RiggedFingerType = Leap.Unity.RiggedFinger;
using System;
using System.Collections.Generic;
using UnityEngine;
using Leap;
using Leap.Unity;
using RootMotion.FinalIK;
public class FinalIKOrionLeapHandController : HandModelManager {
[SerializeField]
public VRIK vrIK;
public Transform reference = null;
public bool track = false;
public bool leftActive = false;
public bool rightActive = false;
public bool swap = false;
public bool applyMirror = false;
// This is actually not used because the Leap Motion SDK always returns 1 anyways.
public float confidenceThreshold = 0.85f;
public int skip = 0;
public float smoothing = 0.5f;
public float gracePeriod = 0.25f;
public float lerpPeriodIn = 0.4f;
public float lerpPeriodOut = 1.25f;
public bool rangeLimit = true;
public float maximumRange = 0.5f;
[HideInInspector]
public HandModel leftHand;
[HideInInspector]
public HandModel rightHand;
private AutoRigType autoRig;
private bool initialized = false;
private RiggedHandType l = null;
private RiggedHandType r = null;
private HashSet<Transform> fingerBones;
private float firstGotLeft = -10f;
private float firstGotRight = -10f;
private float lastLeft = -10f;
private float lastRight = -10f;
private List<Quaternion> lastLeftRotations;
private List<Quaternion> lastRightRotations;
private List<Quaternion> newLeftRotations;
private List<Quaternion> newRightRotations;
private List<Quaternion> lastGoodLeftRotations;
private List<Quaternion> lastGoodRightRotations;
private bool gotLeft = false;
private bool gotRight = false;
private Vector3 leftPos = Vector3.zero;
private Vector3 rightPos = Vector3.zero;
private Quaternion leftRot = Quaternion.identity;
private Quaternion rightRot = Quaternion.identity;
private Vector3 lastLeftPos = Vector3.zero;
private Vector3 lastRightPos = Vector3.zero;
private Quaternion lastLeftRot = Quaternion.identity;
private Quaternion lastRightRot = Quaternion.identity;
private int interpolateState = 0;
private int interpolationCount = 1;
private float avgInterps = 1f;
private bool lastMirror = false;
private bool failed = false;
// Ghost hunting
private int leftAliveCount = 0;
private int rightAliveCount = 0;
private List<Quaternion> lastRawLeftRotations = null;
private List<Quaternion> lastRawRightRotations = null;
private Vector3 lastRawLeftPosition = Vector3.zero;
private Vector3 lastRawRightPosition = Vector3.zero;
private int leftBad = 0;
private int rightBad = 0;
private float dtLeftIn = 0f;
private float dtLeftOut = 0f;
private float dtRightIn = 0f;
private float dtRightOut = 0f;
public void Initialize() {
autoRig = vrIK.gameObject.GetComponent<AutoRigType>();
if (autoRig == null)
autoRig = vrIK.gameObject.AddComponent<AutoRigType>() as AutoRigType;
RemoveGroup("VRM");
RemoveGroup("EmilianaCecil");
autoRig.ModelGroupName = "VRM";
failed = false;
try {
autoRig.AutoRig();
} catch (Exception e) {
Debug.LogWarning("AutoRig failed: " + e);
failed = true;
return;
}
leftHand = autoRig.RiggedHand_L;
rightHand = autoRig.RiggedHand_R;
Destroy(leftHand.gameObject.GetComponent<HandDrop>());
Destroy(rightHand.gameObject.GetComponent<HandDrop>());
fingerBones = new HashSet<Transform>();
l = null;
r = null;
if (leftHand) {
Quaternion handRot = leftHand.transform.rotation;
leftHand.transform.rotation = Quaternion.identity;
foreach (var finger in leftHand.fingers) {
if (finger == null)
continue;
RiggedFingerType riggedFinger = finger as RiggedFingerType;
foreach (var bone in finger.bones) {
fingerBones.Add(bone);
}
if (finger.fingerType == Leap.Finger.FingerType.TYPE_THUMB) {
riggedFinger.modelFingerPointing = new Vector3(-1, 0, 1);
}
int i;
for (i = 0; i < 4; i++) {
if (finger.bones[i] != null)
break;
}
if (i < 3) {
// Thanks to https://twitter.com/Virtual_Deat for this!
if (finger.bones[i+1] != null && finger.bones[i] != null)
riggedFinger.modelFingerPointing = (finger.bones[i+1].position - finger.bones[i].position).normalized;
}
}
leftHand.transform.rotation = handRot;
l = leftHand as RiggedHandType;
}
if (rightHand) {
Quaternion handRot = rightHand.transform.rotation;
rightHand.transform.rotation = Quaternion.identity;
foreach (var finger in rightHand.fingers) {
if (finger == null)
continue;
RiggedFingerType riggedFinger = finger as RiggedFingerType;
foreach (var bone in finger.bones) {
fingerBones.Add(bone);
}
if (finger.fingerType == Leap.Finger.FingerType.TYPE_THUMB) {
riggedFinger.modelFingerPointing = new Vector3(1, 0, 1);
}
int i;
for (i = 0; i < 4; i++) {
if (finger.bones[i] != null)
break;
}
if (i < 3) {
if (finger.bones[i+1] != null && finger.bones[i] != null)
riggedFinger.modelFingerPointing = (finger.bones[i+1].position - finger.bones[i].position).normalized;
}
}
rightHand.transform.rotation = handRot;
r = rightHand as RiggedHandType;
}
}
protected virtual void Awake() {
if (!initialized) {
Initialize();
initialized = true;
}
if (vrIK == null) {
vrIK = gameObject.transform.root.GetComponent<VRIK>();
}
if (vrIK == null) {
Debug.LogError("FinalIKOrionLeapHandController:: no FullBodyBipedIK found on GameObject or any of its parent transforms. ");
}
if (leftHand == null)
Debug.LogError("IKOrionLeapHandController::Awake::No Rigged Hand set for left hand parameter. You have to set this in the inspector.");
if (rightHand == null)
Debug.LogError("IKOrionLeapHandController::Awake::No Rigged Hand set for right hand parameter. You have to set this in the inspector.");
// Physic Handは使用しないのでDisableにする
physicsEnabled = false;
}
protected virtual void Start() {
leapProvider = GetComponent<LeapProvider>();
if (leapProvider == null) {
Debug.LogError("IKOrionLeapHandController::Start::No Leap Provider component was present on " + gameObject.name);
Debug.Log("Added a Leap Service Provider with default settings.");
leapProvider = gameObject.AddComponent<LeapServiceProvider>() as LeapProvider;
}
leapProvider.ClearHandlers();
}
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);
}
Vector3 LocalSpace(Vector3 v) {
return v - transform.position;
}
Vector3 GlobalSpace(Vector3 v) {
return v + transform.position;
}
List<Quaternion> GetJointRotations(RiggedHandType hand) {
if (hand == null)
return null;
List<Quaternion> fingerRots = new List<Quaternion>();
foreach (var t in hand.jointList) {
if (t == null || !fingerBones.Contains(t))
continue;
fingerRots.Add(t.localRotation);
}
return fingerRots;
}
void SetJointRotations(RiggedHandType hand, List<Quaternion> rotationsFrom, List<Quaternion> rotationsTo, float factor, bool mirror) {
if (hand == null)
return;
int i = 0;
foreach (var t in hand.jointList) {
if (t == null || !fingerBones.Contains(t) || i >= rotationsFrom.Count || i >= rotationsTo.Count)
continue;
Quaternion r = Quaternion.Lerp(rotationsFrom[i], rotationsTo[i], factor);
i++;
if (mirror)
r = MirrorQuaternion(r);
t.localRotation = r;
}
}
void FadeJointRotations(RiggedHandType hand, List<Quaternion> rotations, float dt) {
int i = 0;
if (hand == null || rotations == null)
return;
foreach (var t in hand.jointList) {
if (t == null || !fingerBones.Contains(t) || i >= rotations.Count || i >= hand.localRotations.Count)
continue;
t.localRotation = Quaternion.Lerp(rotations[i], hand.localRotations[i], dt);
i++;
}
}
public void LateUpdate() {
if (failed || skip > 0) {
if (skip > 0)
skip--;
return;
}
if (graphicsEnabled) {
Vector3 preReferencePos = transform.position;
Quaternion preReferenceRot = transform.rotation;
if (reference != null) {
transform.position = reference.position;
transform.rotation = reference.rotation;
}
lastLeftRotations = newLeftRotations;
lastRightRotations = newRightRotations;
if (lastGoodLeftRotations == null)
lastGoodLeftRotations = GetJointRotations(l);
if (lastGoodRightRotations == null)
lastGoodRightRotations = GetJointRotations(r);
UpdateHandRepresentations();
Vector3 rightPalmPos = Vector3.zero;
Vector3 leftPalmPos = Vector3.zero;
if (leftAliveCount > 0)
leftPalmPos = leftHand.GetWristPosition();
if (rightAliveCount > 0)
rightPalmPos = rightHand.GetWristPosition();
newLeftRotations = GetJointRotations(l);
newRightRotations = GetJointRotations(r);
List<Quaternion> rawLeftRotations = new List<Quaternion>();
List<Quaternion> rawRightRotations = new List<Quaternion>();
SetJointRotations(l, lastGoodLeftRotations, lastGoodLeftRotations, 0.5f, false);
SetJointRotations(r, lastGoodRightRotations, lastGoodRightRotations, 0.5f, false);
int badThreshold = 10;
int goodFactor = 2;
int badFactor = 1;
int maxBadness = 112;
float angularThreshold = 5f;
float distanceThresholdHigh = 0.013f;
float distanceThresholdLow = 0.001f;
float jumpThreshold = 1f * maximumRange / 2f + 0.05f;
float angularDiffRight = 0f;
for (int i = 0; newRightRotations != null && lastRightRotations != null && i < newRightRotations.Count && i < lastRightRotations.Count; i++) {
rawRightRotations.Add(newRightRotations[i]);
if (lastRawRightRotations != null && rawRightRotations != null && lastRawRightRotations.Count == rawRightRotations.Count)
angularDiffRight += Mathf.Abs(Quaternion.Angle(lastRawRightRotations[i], rawRightRotations[i]));
newRightRotations[i] = Quaternion.Lerp(lastRightRotations[i], newRightRotations[i], 1f - smoothing);
}
lastRawRightRotations = rawRightRotations;
if (newRightRotations != null)
angularDiffRight /= (float)newRightRotations.Count;
float rightDist = Vector3.Distance(LocalSpace(rightPalmPos) / transform.lossyScale.x, lastRawRightPosition);
if (angularDiffRight > angularThreshold && (rightAliveCount == 1 || (distanceThresholdLow < rightDist && rightDist < distanceThresholdHigh)))
rightBad = Math.Min(maxBadness, rightBad + badFactor + Math.Max(0, 6 - rightAliveCount));
else
rightBad = Math.Max(0, rightBad - goodFactor);
if (rangeLimit && rightDist > jumpThreshold && rightAliveCount > 2) {
rightBad = Math.Min(maxBadness, rightBad + badThreshold);
}
if (rangeLimit && rightAliveCount == 1 && Vector3.Distance(LocalSpace(rightPalmPos) / transform.lossyScale.x, Vector3.zero) > maximumRange) {
rightAliveCount = 0;
rightActive = false;
}
lastRawRightPosition = LocalSpace(rightPalmPos) / transform.lossyScale.x;
float angularDiffLeft = 0f;
for (int i = 0; newLeftRotations != null && lastLeftRotations != null && i < newLeftRotations.Count && i < lastLeftRotations.Count; i++) {
rawLeftRotations.Add(newLeftRotations[i]);
if (lastRawLeftRotations != null && rawLeftRotations != null && lastRawLeftRotations.Count == rawLeftRotations.Count)
angularDiffLeft += Mathf.Abs(Quaternion.Angle(lastRawLeftRotations[i], rawLeftRotations[i]));
newLeftRotations[i] = Quaternion.Lerp(lastLeftRotations[i], newLeftRotations[i], 1f - smoothing);
}
lastRawLeftRotations = rawLeftRotations;
if (newLeftRotations != null)
angularDiffLeft /= (float)newLeftRotations.Count;
float leftDist = Vector3.Distance(LocalSpace(leftPalmPos) / transform.lossyScale.x, lastRawLeftPosition);
if (angularDiffLeft > angularThreshold && (leftAliveCount == 1 || (distanceThresholdLow < leftDist && leftDist < distanceThresholdHigh)))
leftBad = Math.Min(maxBadness, leftBad + badFactor + Math.Max(0, 6 - leftAliveCount));
else
leftBad = Math.Max(0, leftBad - goodFactor);
if (rangeLimit && leftDist > jumpThreshold && leftAliveCount > 2) {
leftBad = Math.Min(maxBadness, leftBad + badThreshold);
}
if (rangeLimit && leftAliveCount == 1 &&Vector3.Distance(LocalSpace(leftPalmPos) / transform.lossyScale.x, Vector3.zero) > maximumRange) {
leftAliveCount = 0;
leftActive = false;
}
lastRawLeftPosition = LocalSpace(leftPalmPos) / transform.lossyScale.x;
bool doMirror = (applyMirror && !swap) || (swap && !applyMirror);
if (lastMirror != doMirror) {
Vector3 tmpV = lastRightPos;
lastRightPos = lastLeftPos;
lastLeftPos = tmpV;
Quaternion tmpQ = lastRightRot;
lastRightRot = lastLeftRot;
lastLeftRot = tmpQ;
}
bool lastGotLeft = gotLeft;
bool lastGotRight = gotRight;
gotLeft = false;
gotRight = false;
lastLeftPos = leftPos;
lastLeftRot = leftRot;
lastRightPos = rightPos;
lastRightRot = rightRot;
if (doMirror) {
Quaternion inverse = Quaternion.Inverse(transform.rotation);
if (rightActive && rightHand != null) {
leftPos = GlobalSpace(transform.rotation * MirrorTranslation(inverse * LocalSpace(rightPalmPos)));
leftRot = rightHand.GetPalmRotation() * r.Reorientation() * Quaternion.AngleAxis(180f, Vector3.up);
leftRot = transform.rotation * MirrorQuaternion(inverse * leftRot);
gotLeft = true;
if (rightBad > badThreshold || (lastGotLeft != gotLeft && rightBad > 0))
gotLeft = false;
}
if (leftActive && leftHand != null) {
rightPos = GlobalSpace(transform.rotation * MirrorTranslation(inverse * LocalSpace(leftPalmPos)));
rightRot = leftHand.GetPalmRotation() * l.Reorientation() * Quaternion.AngleAxis(180f, Vector3.up);
rightRot = transform.rotation * MirrorQuaternion(inverse * rightRot);
gotRight = true;
if (leftBad > badThreshold || (lastGotRight != gotRight && leftBad > 0))
gotRight = false;
}
} else {
if (leftActive && leftHand != null) {
leftPos = leftPalmPos;
leftRot = leftHand.GetPalmRotation() * l.Reorientation() * Quaternion.AngleAxis(180f, Vector3.up);
gotLeft = true;
if (leftBad > badThreshold || (lastGotLeft != gotLeft && leftBad > 0))
gotLeft = false;
}
if (rightActive && rightHand != null) {
rightPos = rightPalmPos;
rightRot = rightHand.GetPalmRotation() * r.Reorientation() * Quaternion.AngleAxis(180f, Vector3.up);
gotRight = true;
if (rightBad > badThreshold || (lastGotRight != gotRight && rightBad > 0))
gotRight = false;
}
}
if (lastGotLeft != gotLeft && gotLeft) {
firstGotLeft = Time.time;
lastLeftPos = leftPos;
lastLeftRot = leftRot;
}
if (lastGotRight != gotRight && gotRight) {
firstGotRight = Time.time;
lastRightPos = rightPos;
lastRightRot = rightRot;
}
if ((gotLeft || gotRight) && interpolateState < 2)
interpolateState++;
if (interpolateState > 1)
avgInterps = Mathf.Lerp(avgInterps, (float)interpolationCount, 0.15f);
interpolationCount = 0;
leftPos = Vector3.Lerp(lastLeftPos, leftPos, 1f - smoothing);
leftRot = Quaternion.Lerp(lastLeftRot, leftRot, 1f - smoothing);
lastGotLeft = gotLeft;
rightPos = Vector3.Lerp(lastRightPos, rightPos, 1f - smoothing);
rightRot = Quaternion.Lerp(lastRightRot, rightRot, 1f - smoothing);
lastGotRight = gotRight;
lastMirror = doMirror;
Interpolate();
transform.position = preReferencePos;
transform.rotation = preReferenceRot;
}
}
void Interpolate() {
if (!track) {
gotLeft = false;
gotRight = false;
}
float t = Mathf.Clamp((float)interpolationCount / avgInterps, 0f, 0.985f);
float now = Time.time;
interpolationCount++;
if (gotLeft) {
float dt = Mathf.Clamp((now - firstGotLeft) / lerpPeriodIn + (1f - dtLeftOut), 0f, 1f);
dtLeftIn = dt;
if (skip <= 0) {
vrIK.solver.leftArm.IKPosition = Vector3.Lerp(lastLeftPos, leftPos, t);
vrIK.solver.leftArm.IKRotation = Quaternion.Lerp(lastLeftRot, leftRot, t);
vrIK.solver.leftArm.positionWeight = dt;
vrIK.solver.leftArm.rotationWeight = dt;
}
lastLeft = now;
} else {
float dt = Mathf.Clamp(((now - lastLeft) - gracePeriod) / lerpPeriodOut - (1f - dtLeftIn), 0f, 1f);
dtLeftOut = dt;
if (skip <= 0) {
vrIK.solver.leftArm.positionWeight = Mathf.Lerp(1f, 0f, dt);
vrIK.solver.leftArm.rotationWeight = Mathf.Lerp(1f, 0f, dt);
}
FadeJointRotations(l, lastGoodLeftRotations, dt);
}
if (gotRight) {
float dt = Mathf.Clamp((now - firstGotRight) / lerpPeriodIn + (1f - dtRightOut), 0f, 1f);
dtRightIn = dt;
if (skip <= 0) {
vrIK.solver.rightArm.IKPosition = Vector3.Lerp(lastRightPos, rightPos, t);
vrIK.solver.rightArm.IKRotation = Quaternion.Lerp(lastRightRot, rightRot, t);
vrIK.solver.rightArm.positionWeight = dt;
vrIK.solver.rightArm.rotationWeight = dt;
}
lastRight = now;
} else {
float dt = Mathf.Clamp(((now - lastRight) - gracePeriod) / lerpPeriodOut - (1f - dtRightIn), 0f, 1f);
dtRightOut = dt;
if (skip <= 0) {
vrIK.solver.rightArm.positionWeight = Mathf.Lerp(1f, 0f, dt);
vrIK.solver.rightArm.rotationWeight = Mathf.Lerp(1f, 0f, dt);
}
FadeJointRotations(r, lastGoodRightRotations, dt);
}
bool doMirror = (applyMirror && !swap) || (swap && !applyMirror);
if (skip <= 0) {
if (doMirror) {
if (gotLeft)
SetJointRotations(l, lastRightRotations, newRightRotations, t, true);
if (gotRight)
SetJointRotations(r, lastLeftRotations, newLeftRotations, t, true);
}
else {
if (gotLeft)
SetJointRotations(l, lastLeftRotations, newLeftRotations, t, false);
if (gotRight)
SetJointRotations(r, lastRightRotations, newRightRotations, t, false);
}
} else {
skip--;
}
lastGoodLeftRotations = GetJointRotations(l);
lastGoodRightRotations = GetJointRotations(r);
}
Hand UpdateHand (HandModel hand, Leap.Hand curHand) {
Vector3[] pos = new Vector3[4];
Quaternion[] rot = new Quaternion[4];
if (hand.palm != null) {
pos[0] = hand.palm.localPosition;
rot[0] = hand.palm.localRotation;
}
if (hand.forearm != null) {
pos[1] = hand.forearm.localPosition;
rot[1] = hand.forearm.localRotation;
}
if (hand.wristJoint != null) {
pos[2] = hand.wristJoint.localPosition;
rot[2] = hand.wristJoint.localRotation;
}
if (hand.elbowJoint != null) {
pos[3] = hand.elbowJoint.localPosition;
rot[3] = hand.elbowJoint.localRotation;
}
hand.SetLeapHand(curHand);
hand.UpdateHand();
Hand leapHand = hand.GetLeapHand();
if (hand.palm != null) {
hand.palm.localPosition = pos[0];
hand.palm.localRotation = rot[0];
}
if (hand.forearm != null) {
hand.forearm.localPosition = pos[1];
hand.forearm.localRotation = rot[1];
}
if (hand.wristJoint != null) {
hand.wristJoint.localPosition = pos[2];
hand.wristJoint.localRotation = rot[2];
}
if (hand.elbowJoint != null) {
hand.elbowJoint.localPosition = pos[3];
hand.elbowJoint.localRotation = rot[3];
}
return leapHand;
}
/// <summary>
/// Tells the hands to update to match the new Leap Motion hand frame data. Also keeps track of
/// which hands are currently active.
/// </summary>
void UpdateHandRepresentations() {
if (failed)
return;
(leapProvider as LeapServiceProvider).RetransformFrames();
leftActive = false;
rightActive = false;
foreach (Leap.Hand curHand in leapProvider.CurrentFrame.Hands) {
if (curHand.IsLeft && l != null) {
Hand hand = UpdateHand(leftHand, curHand);
leftActive = true;
leftAliveCount++;
}
if (curHand.IsRight && r != null) {
Hand hand = UpdateHand(rightHand, curHand);
rightActive = true;
rightAliveCount++;
}
}
if (!leftActive) {
lastRawLeftRotations = null;
leftAliveCount = 0;
leftBad = 0;
}
int minCount = 4;
if (leftAliveCount < minCount)
leftActive = false;
if (!rightActive) {
lastRawRightRotations = null;
rightAliveCount = 0;
rightBad = 0;
}
if (rightAliveCount < minCount)
rightActive = false;
if (!leftActive && !rightActive) {
interpolateState = 0;
interpolationCount = 1;
avgInterps = 1f;
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.