public void SolveKabschAndAlign()
{
Debug.Log("Trying solve...");
if (finalPoints.Count >= minSamplePointsNeeded)
{
#if UNITY_EDITOR && !UNITY_ANDROID
KabschSolver solver = new KabschSolver();
Matrix4x4 deviceToOriginDeviceMatrix;
if (!inverseSolve)
{
deviceToOriginDeviceMatrix =
solver.SolveKabsch(finalPoints, initialPoints, 200);
}
else
{
deviceToOriginDeviceMatrix =
solver.SolveKabsch(initialPoints, finalPoints, 200);
}
//If child set, remove from hierarchy first
if (ignoreChild != null)
{
Transform childParent = ignoreChild.parent;
ignoreChild.parent = null;
Tracker.transform.Transform(deviceToOriginDeviceMatrix);
ignoreChild.parent = childParent;
}
else
{
Tracker.transform.Transform(deviceToOriginDeviceMatrix);
}
SendMessage("StoredSample", 0, SendMessageOptions.DontRequireReceiver);
handRig.SetActive(true);
myCurrentState = CoordinateAlignerStates.PostAlignment;
#endif
}
else
{
Debug.Log("FAIL: Not enough samples, need at least " + minSamplePointsNeeded.ToString());
handRig.SetActive(false);
myCurrentState = CoordinateAlignerStates.Reset;
}
}
public void SolveKabschAndAlign()
{
Debug.Log("Trying solve...");
if (finalPoints.Count >= minSamplePointsNeeded)
{
#if UNITY_EDITOR && !UNITY_ANDROID
KabschSolver solver = new KabschSolver();
Matrix4x4 deviceToOriginDeviceMatrix;
if (!inverseSolve)
{
deviceToOriginDeviceMatrix =
solver.SolveKabsch(finalPoints, initialPoints, 200);
}
else
{
deviceToOriginDeviceMatrix =
solver.SolveKabsch(initialPoints, finalPoints, 200);
}
//If child set, remove from hierarchy first
//device to origin matrix is from the LM -> the 6DOF tracker, we need to solve it
Matrix4x4 WorldToLM = LeapMotionTransform.worldToLocalMatrix;
Matrix4x4 DOFTrackerToOrigin = (WorldToLM * deviceToOriginDeviceMatrix).inverse;
Tracker.myOffsets.LocalRigTranslation = DOFTrackerToOrigin.MultiplyPoint(Vector3.zero);
Tracker.myOffsets.LocalRigRotation = DOFTrackerToOrigin.rotation;
Tracker.RigCenter.localPosition = Tracker.myOffsets.LocalRigTranslation;
Tracker.RigCenter.localRotation = Tracker.myOffsets.LocalRigRotation;
SendMessage("StoredSample", 0, SendMessageOptions.DontRequireReceiver);
myCurrentState = CoordinateAlignerStates.PostAlignment;
#endif
}
else
{
Debug.Log("FAIL: Not enough samples, need at least " + minSamplePointsNeeded.ToString());
myCurrentState = CoordinateAlignerStates.Reset;
}
}
