private void solveWithTRS(Vector3 translation,
Quaternion rotation,
out Vector3 solvedTranslation,
out Quaternion solvedRotation)
{
Vector3[] v0 = new Vector3[3];
v0[0] = new Vector3(1, 0, 0);
v0[1] = new Vector3(0, 1, 0);
v0[2] = new Vector3(0, 0, 1);
var v1 = v0.Select(v => rotation * v + translation).ToArray();
for (int i = 0; i < 3; i++)
{
var l0 = (v0[i]).ToCVector();
var l1 = (v1[i]).ToCVector();
KabschC.AddPoint(ref _kabsch, ref l0, ref l1, 1.0f);
}
KabschC.Solve(ref _kabsch);
LEAP_VECTOR leapTranslation;
LEAP_QUATERNION leapRotation;
KabschC.GetTranslation(ref _kabsch, out leapTranslation);
KabschC.GetRotation(ref _kabsch, out leapRotation);
solvedTranslation = leapTranslation.ToVector3();
solvedRotation = leapRotation.ToQuaternion();
}
protected override void Init(InteractionBehaviour obj)
{
base.Init(obj);
_handIdToPoints = new Dictionary <int, HandPointCollection>();
KabschC.Construct(ref _kabsch);
}
protected void performSolve()
{
switch (_solveMethod)
{
case SolveMethod.SixDegreeSolve:
KabschC.Solve(ref _kabsch);
break;
case SolveMethod.PivotAroundOrigin:
LEAP_VECTOR v = new LEAP_VECTOR();
v.x = v.y = v.z = 0;
KabschC.SolveWithPivot(ref _kabsch, ref v);
break;
}
}
public override void GetHoldingPose(ReadonlyList <Hand> hands, out Vector3 newPosition, out Quaternion newRotation)
{
KabschC.Reset(ref _kabsch);
Vector3 bodyPosition = _obj.warper.RigidbodyPosition;
Quaternion bodyRotation = _obj.warper.RigidbodyRotation;
Matrix4x4 it = Matrix4x4.TRS(bodyPosition, bodyRotation, Vector3.one);
for (int h = 0; h < hands.Count; h++)
{
Hand hand = hands[h];
var collection = _handIdToPoints[hand.Id];
for (int f = 0; f < NUM_FINGERS; f++)
{
Finger finger = hand.Fingers[f];
Finger.FingerType fingerType = finger.Type;
for (int j = 0; j < NUM_BONES; j++)
{
Bone.BoneType boneType = (Bone.BoneType)j;
Bone bone = finger.Bone(boneType);
Vector3 localPos = collection.GetLocalPosition(fingerType, boneType);
Vector3 bonePos = bone.NextJoint.ToVector3();
//Do the solve such that the objects positions are matched to the new bone positions
LEAP_VECTOR point1 = (it.MultiplyPoint3x4(localPos) - bodyPosition).ToCVector();
LEAP_VECTOR point2 = (bonePos - bodyPosition).ToCVector();
KabschC.AddPoint(ref _kabsch, ref point1, ref point2, 1.0f);
}
}
}
performSolve();
LEAP_VECTOR leapTranslation;
LEAP_QUATERNION leapRotation;
KabschC.GetTranslation(ref _kabsch, out leapTranslation);
KabschC.GetRotation(ref _kabsch, out leapRotation);
newPosition = bodyPosition + leapTranslation.ToVector3();
newRotation = leapRotation.ToQuaternion() * bodyRotation;
}
public void Teardown()
{
KabschC.Destruct(ref _kabsch);
}
public void Setup()
{
KabschC.Construct(ref _kabsch);
}
