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(); }
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; }