static double Angle0 = 1.5707963267; //Math.PI / 2;
static Point[] CalculateRobotHands()
{
var mass = RobotMathematics.MoveTo(X0, Y0, Angle0);
mass[2] = -Angle0;
/*if (X0 < 0)
* mass[1] = Math.PI * 2 - ( mass[1] - Math.PI + mass[0]);
* else*/
mass[1] = mass[1] - Math.PI + mass[0];
var arm = new Point[4];
arm[0] = new Point(0, 0, 250, 250);
if (double.IsNaN(mass[0]))
{
mass[0] = 0.2;
}
for (int i = 1; i < 4; ++i)
{
if (double.IsNaN(mass[0]))
{
return(null);
}
int X = arm[i - 1].X2 + (int)(Math.Cos(mass[i - 1]) * RobotMathematics.Arm(i) * DrowCoefficient);
int Y = arm[i - 1].Y2 - (int)(Math.Sin(mass[i - 1]) * RobotMathematics.Arm(i) * DrowCoefficient);
if (X == double.NaN || Y == double.NaN || X < 0 || X > 500 || Y < 0 || Y > 500)
{
return(null);
}
arm[i] = new Point(arm[i - 1].X2, arm[i - 1].Y2, X, Y);
}
return(arm);
}
private static void ChangeAngles(double x, double y, double angle)
{
var angles = RobotMathematics.MoveTo(x / 10, y / 10, angle);
Shoulder = 2 * Math.PI - angles[0];
Elbow = 2 * Math.PI - angles[1];
Wrist = 2 * Math.PI - angles[2];
lines[0].x2 = lines[0].x1 + RobotMathematics.UpperArm * Math.Cos(Shoulder) * 4;
lines[0].y2 = lines[0].y1 + RobotMathematics.UpperArm * Math.Sin(Shoulder) * 4;
lines[1].x1 = lines[0].x2;
lines[1].y1 = lines[0].y2;
lines[1].x2 = lines[1].x1 + RobotMathematics.Forearm * Math.Cos(Elbow + Shoulder - Math.PI) * 4;
lines[1].y2 = lines[1].y1 + RobotMathematics.Forearm * Math.Sin(Elbow + Shoulder - Math.PI) * 4;
lines[2].x1 = lines[1].x2;
lines[2].y1 = lines[1].y2;
lines[2].x2 = lines[2].x1 + RobotMathematics.Palm * Math.Cos(Wrist - Elbow - Shoulder) * 4;
lines[2].y2 = lines[2].y1 + RobotMathematics.Palm * Math.Sin(Wrist - Elbow - Shoulder) * 4;
}