//

An inverse kinematic solver for a two link chain


import java.awt.*;
        
public class ik
{
//-------- SOLVE TWO LINK INVERSE KINEMATICS -------------

// Given a two link joint from [0,0,0] to end effector position P,
// let link lengths be a and b, and let norm |P| = c.  Clearly a+b >= c.
//
// Problem: find a "knee" position Q such that |Q| = a and |P-Q| = b.
//
// In the case of a point on the x axis R = [c,0,0], there is a
// closed form solution S = [d,e,0], where |S| = a and |R-S| = b:
//
//    d2+e2 = a2                  -- because |S| = a
//    (c-d)2+e2 = b2              -- because |R-S| = b
//
//    c2-2cd+d2+e2 = b2           -- combine the two equations
//    c2-2cd = b2 - a2
//    c-2d = (b2-a2)/c
//    d - c/2 = (a2-b2)/c / 2
//
//    d = (c + (a2-b2/c) / 2      -- to solve for d and e.
//    e = sqrt(a2-d2)

   static double findD(double a, double b, double c) {
      return Math.max(0, Math.min(a, (c + (a*a-b*b)/c) / 2));
   }
   static double findE(double a, double d) { return Math.sqrt(a*a-d*d); } 

// This leads to a solution to the more general problem:
//
//   (1) R = Mfwd(P)         -- rotate P onto the x axis
//   (2) Solve for S
//   (3) Q = Minv(S)         -- rotate back again

   static double[][] Mfwd = new double[3][3], Minv = new double[3][3];

   static public boolean solve(double A, double B, double[] P, double[] D, double[] Q) {
      double[] R = new double[3];
      defineM(P,D);
      rot(Minv,P,R);
      double d = findD(A,B,norm(R));
      double e = findE(A,d);
      double[] S = {d,e,0};
      rot(Mfwd,S,Q);
      return d > 0 && d < A;
   }

// If "knee" position Q needs to be as close as possible to some point D,
// then choose M such that M(D) is in the y>0 half of the z=0 plane.
//
// Given that constraint, define the forward and inverse of M as follows:

   static void defineM(double[] P, double[] D) {
      double[] X = Minv[0], Y = Minv[1], Z = Minv[2];

// Minv defines a coordinate system whose x axis contains P, so X = unit(P).

      for (int i = 0 ; i < 3 ; i++)
         X[i] = P[i];
      normalize(X);

// The y axis of Minv is perpendicular to P, so Y = unit( D - X(D·X) ).

      double dDOTx = dot(D,X);
      for (int i = 0 ; i < 3 ; i++)
         Y[i] = D[i] - dDOTx * X[i];
      normalize(Y);

// The z axis of Minv is perpendicular to both X and Y, so Z = X×Y.

      cross(X,Y,Z);

// Mfwd = (Minv)T, since transposing inverts a rotation matrix.

      for (int i = 0 ; i < 3 ; i++) {
         Mfwd[i][0] = Minv[0][i];
         Mfwd[i][1] = Minv[1][i];
         Mfwd[i][2] = Minv[2][i];
      }
   }

//------------ GENERAL VECTOR MATH SUPPORT -----------

   static double norm(double[] v) { return Math.sqrt( dot(v,v) ); }

   static void normalize(double[] v) {
      double norm = norm(v);
      for (int i = 0 ; i < 3 ; i++)
         v[i] /= norm;
   }

   static double dot(double[] a, double[] b) { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; }

   static void cross(double[] a, double[] b, double[] c) {
      c[0] = a[1] * b[2] - a[2] * b[1];
      c[1] = a[2] * b[0] - a[0] * b[2];
      c[2] = a[0] * b[1] - a[1] * b[0];
   }

   static void rot(double[][] M, double[] src, double[] dst) {
      for (int i = 0 ; i < 3 ; i++)
         dst[i] = dot(M[i],src);
   }
}