问逆运动学:如何计算机械臂的伺服角度，以达到画布中所有可能的点？

EN

    int L1 = 170;
    int L2 = 230;
    Vector shoulderV;
    Vector targetV;
    shoulderV = new Vector(0,0);
    targetV = new Vector(0,400);


    Vector difference = Vector.Subtract(targetV, shoulderV);
    double L3 = difference.Length;
    if (L3 > 400) { L3 = 400; }
    if (L3 < 170) { L3 = 170; }

    // a + b is the equivelant of the shoulder angle
    double a = Math.Acos((L1 * L1 + L3 * L3 - L2 * L2) / (2 * L1 * L3));  
    double b = Math.Atan(difference.Y / difference.X);

   // S1 is the shoulder angle
   double S1 = a + b;
  // S2 is the elbow angle
  double S2 = Math.Acos((L1 * L1 + L2 * L2 - L3 * L3) / (2 * L1 * L2));

  int shoulderAngle = Convert.ToInt16(Math.Round(S1 * 180 / Math.PI));
  if (shoulderAngle < 0) { shoulderAngle = 180 - shoulderAngle; }
  if (shoulderAngle > 180) { shoulderAngle = 180; }
  int elbowAngle = Convert.ToInt16(Math.Round(S2 * 180 / Math.PI));

  elbowAngle = 180 - elbowAngle;