public void SwitchState(CoordinateAlignerStates newState)
{
myPrevState = newState;
switch (newState)
{
case CoordinateAlignerStates.Idle:
break;
case CoordinateAlignerStates.CaptureInitialPair:
myInternalState = InternalState.Show_Message_Space;
break;
case CoordinateAlignerStates.CapturingFinalpair:
break;
case CoordinateAlignerStates.CaptureFinalPair:
if (myDriver.initialPoints.Count > myDriver.minSamplePointsNeeded)
{
myInternalState = InternalState.Show_Message_CompleteCapture1;
myDriver.myCurrentState = CoordinateAlignerStates.PendingAlignment;
}
else
{
myInternalState = InternalState.Show_Message_CompleteCapture0;
}
break;
case CoordinateAlignerStates.Reset:
break;
case CoordinateAlignerStates.Calculating:
myInternalState = InternalState.Show_Message_CalculatingAlignment0;
break;
}
}
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;
}
}
// Update is called once per frame
void Update()
{
bool handInAllFrames = rightHand.activeSelf;
handIsVisible = handInAllFrames;
switch (myCurrentState)
{
case CoordinateAlignerStates.PostAlignment:
if (Input.GetKeyDown(KeyCode.R))
{
myCurrentState = CoordinateAlignerStates.Reset;
break;
}
break;
case CoordinateAlignerStates.PendingAlignment:
if (Input.GetKeyDown(solveForRelativeTransformKey))
{
myCurrentState = CoordinateAlignerStates.Calculating;
break;
}
break;
case CoordinateAlignerStates.Idle:
if (handInAllFrames)
{
if (Input.GetKeyDown(takeCalibrationSampleKey))
{
myCurrentState = CoordinateAlignerStates.CaptureInitialPair;
}
}
break;
case CoordinateAlignerStates.CaptureInitialPair:
Debug.Log("Capturing the initial pose");
if (handInAllFrames)
{
SetAllEnabledHandReferencePointsInitial();
Debug.Log("Saved Initial Pair, continuing to capture final pair");
myCurrentState = CoordinateAlignerStates.CapturingFinalpair;
EnableLines();
}
else
{
Debug.Log("Couldn't Capture initial Pose");
myCurrentState = CoordinateAlignerStates.Idle;
}
break;
case CoordinateAlignerStates.CapturingFinalpair:
if (handInAllFrames)
{
SetAllEnabledHandReferencePointsFinal();
}
SetLineRenderers();
if (Input.GetKeyUp(takeCalibrationSampleKey))
{
myCurrentState = CoordinateAlignerStates.CaptureFinalPair;
Debug.Log("Attempting to capture final pose");
DisableLines();
}
break;
case CoordinateAlignerStates.CaptureFinalPair:
if (handInAllFrames)
{
Debug.Log("Capturing the shifted Pose");
SetAllEnabledHandReferencePointsFinal();
AddAllEnabledHandReferencePoints(referenceInitialPoints, referenceFinalPoints);
SendMessage("StoredSample", SendMessageOptions.DontRequireReceiver);
Debug.Log("saved new relative points, total sets:" + initialPoints.Count + "," + finalPoints.Count);
}
else
{
Debug.Log("FailedToCapture");
}
myCurrentState = CoordinateAlignerStates.Idle;
break;
case CoordinateAlignerStates.Calculating:
SolveKabschAndAlign();
break;
case CoordinateAlignerStates.Reset:
handRig.SetActive(false);
initialPoints.Clear();
finalPoints.Clear();
Debug.Log("Resetting the state");
myCurrentState = CoordinateAlignerStates.Idle;
transform.position = initialPosition;
transform.rotation = initialRotation;
break;
}
}
