private void DoForwardKinematics()
{
UpdateThetaValues();
circlesCreation.UpdateInvisibleCircles(thetas);
endEffector.localPosition = circlesCreation.endEffectorPosition;
legsRotation.RotateLegs(thetas, true);
legsRotation.RotateParallelograms();
}
private void DoInverseKinematics()
{
Vector3 P = new Vector3(endEffector.localPosition.x, endEffector.localPosition.z, endEffector.localPosition.y);
legsGenerator.GenerateEi(P);
legsGenerator.GenerateFi(P);
legsGenerator.GenerateGi(P);
SetLegsGenerator();
for (int i = 0; i < DeltaRobotIKUtils.NO_OF_LEGS; i++)
{
double currentFi = serviceConverter.ConvertToTwoDecimalPlaces(Fi[i]);
double currentEi = serviceConverter.ConvertToTwoDecimalPlaces(Ei[i]);
double currentGi = serviceConverter.ConvertToTwoDecimalPlaces(Gi[i]);
double roots = legsGenerator.GetRootsTi(currentFi, currentEi, currentGi);
thetas[i] = 2 * Math.Atan(roots);
}
legsRotation.RotateLegs(thetas, false);
legsRotation.RotateParallelograms();
}