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