private void SetPositionsOfArms(Configuration angles)
{
Angle1 = angles.Angle1;
Angle2 = angles.Angle2;
Length2 = angles.Length2;
Angle3 = angles.Angle3;
Angle4 = angles.Angle4;
Angle5 = angles.Angle5;
Matrix[] transform = CalculateTransformation();
Matrix F1 = transform[0];
Matrix F2 = transform[1];
Matrix F3 = transform[2];
Matrix F4 = transform[3];
Matrix F5 = transform[4];
//Matrix EndPoint = transform[5];
//TODO: Reset all cylinders to 0 0 0 position
L1 = new Cylinder(device, Length1, 0.5f, 16, Color.Green);
L2 = new Cylinder(device, Length2, 0.5f, 16, Color.Red);
L3 = new Cylinder(device, Length3, 0.5f, 16, Color.Green);
L4 = new Cylinder(device, Length4, 0.5f, 16, Color.Green);
L5 = new Cylinder(device, Length5, 0.2f, 16, Color.Green);
//TODO: clear memory?
L1.Transform(F1);
L2.Transform(F2);
L3.Transform(F3);
L4.Transform(F4);
L5.Transform(F5);
//debug
var a1 = Angle1 * 180 / Math.PI;
var a2 = Angle2 * 180 / Math.PI;
var a3 = Angle3 * 180 / Math.PI;
var a4 = Angle4 * 180 / Math.PI;
var a5 = Angle5 * 180 / Math.PI;
}
