public static void fkTest(bool b)
{
float[] x = { 0f, 0f, 0f };
float[] y = { 0f, 0f, 0f };
float[] z = { 0f, 0f, 0f };
float[] length = { 50, 50, 50 };
float[] dest = { 70, 70, 70 };
int[] Xmin = { 0, 0, 0 };
int[] Xmax = { 360, 360, 360 };
int[] Ymin = { 0, 0, 0 };
int[] Ymax = { 360, 360, 360 };
int[] Zmin = { 0, 0, 0 };
int[] Zmax = { 360, 360, 360 };
ForwardKinematics fk = new ForwardKinematics(x, y, z, length, Xmin, Xmax, Ymin, Ymax, Zmin, Zmax);
if (b)
{
ikTest(fk, dest);
}
float[] ep = fk.getEndpoint();
Console.WriteLine(ep[0] + " " + ep[1] + " " + ep[2]);
}
public IK(ForwardKinematics FK, float[] destination)
{
this.FK = FK;
this.destination = destination;
currentPos = FK.getEndpoint();
cost = calculateCost();
}
public static void ikTest(ForwardKinematics fk, float[] dest)
{
IK inv = new IK(fk, dest);
float[][] finalAngles = inv.getAngles();
}