public void PPChanged()
{
fingerController.PPChanged(PPSlider.value);
PPValue.text = PPSlider.value.ToString();
}
public void JacobianIterative()
{
float degreeToRadianConstant = Mathf.PI / 180;
float mcpjGuess = uiController.getMCPJGuess();
float pipjGuess = uiController.getPIPJGuess();
float desiredX = uiController.getXInput();
float desiredY = uiController.getYInput();
int iterationCounter = 0;
float errorThreshold = 0.05f;
float currentX;
float currentY;
while (true)
{
currentX = 39.8f * Mathf.Cos(mcpjGuess * degreeToRadianConstant) + 22.4f * Mathf.Cos((mcpjGuess + pipjGuess) * degreeToRadianConstant) + 15.8f * Mathf.Cos((mcpjGuess + 5 * pipjGuess / 3) * degreeToRadianConstant);
currentY = 39.8f * Mathf.Sin(mcpjGuess * degreeToRadianConstant) + 22.4f * Mathf.Sin((mcpjGuess + pipjGuess) * degreeToRadianConstant) + 15.8f * Mathf.Sin((mcpjGuess + 5 * pipjGuess / 3) * degreeToRadianConstant);
float xError = Mathf.Abs(desiredX - currentX);
float yError = Mathf.Abs(desiredY - currentY);
Debug.Log(xError + yError);
if (iterationCounter == 500 || xError + yError < errorThreshold)
{
uiController.setSliderWholeNumbers(false);
uiController.independent.isOn = false;
uiController.updatePPSlider(mcpjGuess);
fingerController.PPChanged(mcpjGuess);
uiController.updateIPSlider(pipjGuess);
fingerController.IPChanged(pipjGuess, false);
Debug.Log("Iterations: " + iterationCounter);
return;
}
float[] jacobian = new float[4];
jacobian[0] = 39.8f * -Mathf.Sin(mcpjGuess * degreeToRadianConstant) + 22.4f * -Mathf.Sin((mcpjGuess + pipjGuess) * degreeToRadianConstant) + 15.8f * -Mathf.Sin((pipjGuess + 5 * pipjGuess / 3) * degreeToRadianConstant);
jacobian[1] = 22.4f * -Mathf.Sin((mcpjGuess + pipjGuess) * degreeToRadianConstant) + 15.8f * -Mathf.Sin((pipjGuess + 5 * pipjGuess / 3) * degreeToRadianConstant);
jacobian[2] = 39.8f * Mathf.Cos(mcpjGuess * degreeToRadianConstant) + 22.4f * Mathf.Cos((mcpjGuess + pipjGuess) * degreeToRadianConstant) + 15.8f * Mathf.Cos((pipjGuess + 5 * pipjGuess / 3) * degreeToRadianConstant);
jacobian[3] = 22.4f * Mathf.Cos((mcpjGuess + pipjGuess) * degreeToRadianConstant) + 15.8f * Mathf.Cos((pipjGuess + 5 * pipjGuess / 3) * degreeToRadianConstant);
float determinant = 1 / (jacobian[0] * jacobian[3] - jacobian[1] * jacobian[2]);
float[] inverse = new float[4];
inverse[0] = determinant * jacobian[3];
inverse[1] = determinant * -jacobian[1];
inverse[2] = determinant * -jacobian[2];
inverse[3] = determinant * jacobian[0];
// Change guesses
mcpjGuess = mcpjGuess + inverse[0] * (desiredX - currentX) + inverse[1] * (desiredY - currentY);
pipjGuess = pipjGuess + inverse[2] * (desiredX - currentX) + inverse[3] * (desiredY - currentY);
iterationCounter++;
}
}
