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