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();
}
public void InitTriangles()
{
this.S_B = serviceConverter.ConvertToTwoDecimalPlaces((topVerticesPoints[0].position - topVerticesPoints[1].position).magnitude);
this.U_B = serviceConverter.ConvertToTwoDecimalPlaces((Mathf.Sqrt(3) / 3.0) * S_B);
this.W_B = serviceConverter.ConvertToTwoDecimalPlaces((Mathf.Sqrt(3) / 6.0) * S_B);
this.S_P = serviceConverter.ConvertToTwoDecimalPlaces((PPoints[0].position - PPoints[1].position).magnitude);
this.U_P = serviceConverter.ConvertToTwoDecimalPlaces((Mathf.Sqrt(3) / 3.0) * S_P);
this.W_P = serviceConverter.ConvertToTwoDecimalPlaces((Mathf.Sqrt(3) / 6.0) * S_P);
}