private void MoveAndRedraw(object sender, MouseEventArgs e)
{
var(X, Y) = ScreenToModel(e.X, e.Y);
float Z = tZ;
//float Z = X * (float)Math.Sin(tZ);
//X = X * (float)Math.Cos(tZ);
Console.WriteLine($"Target: ({X}, {Y}, {Z})");
a.MoveTo(X, Y, Z);
var(AX, AY, AZ) = a.CalculatePosition();
Console.WriteLine($"Actual: ({AX}, {AY}, {AZ})");
Console.WriteLine($"Shoulder: ({a.CurrentRolls[0]}, {a.CurrentPitches[0]}, {a.CurrentYaws[0]})");
this.Invalidate();
}
