Tutorial example: solving two link inverse kinematics

*/
import java.awt.*;
        
public class ikTest extends BA
{
   double A = 1, B = .5; // THE TWO LINK LENGTHS

//--------- AN EXAMPLE MOTION PATH ---------

   void pointOnPath(double[] P, double theta) {
      P[0] = .8+.5*Math.cos(theta);
      P[1] = .4+.2*Math.cos(.03*clicks);
      P[2] = .5+.5*Math.sin(theta);
   }

//----- DRAW THE OBJECT EVERY FRAME -------

   int clicks = 0;
   double[] P = {0,0,0}, D = {0,1,0}, Q = {0,0,0}, U={0,0,0},V={0,0,0};
   void drawObject() {

      D[2] = Math.cos(.01 * clicks);   // DEFINE AIM POINT
      pointOnPath(P, .05 * clicks);    // DEFINE END EFFECTOR POINT

      g.setColor(Color.cyan);          // SHOW THE COORDINATE AXES
      draw(-1,0,0, 1,0,0);
      draw(0,0,-1, 0,0,1);

      g.setColor(Color.green);         // SHOW THE MOTION PATH
      for (int i = 0 ; i < 20 ; i++) {
         pointOnPath(U, 2*Math.PI*i/20);
         pointOnPath(V, 2*Math.PI*(i+1)/20);
         draw(U,V);
      }

      g.setColor(Color.blue);          // SHOW THE AIM POINT
      draw(D[0]-.1,D[1],D[2], D[0]+.1,D[1],D[2]);
      draw(D[0],D[1]-.1,D[2], D[0],D[1]+.1,D[2]);
      draw(D[0],D[1],D[2]-.1, D[0],D[1],D[2]+.1);

      boolean isIk = ik.solve(A,B,P,D,Q);     // FIND THE IK SOLUTION, IF ANY

      g.setColor(isIk ? Color.black : Color.red); // SHOW THE TWO LINKS
      draw(0,0,0, Q[0],Q[1],Q[2]);
      draw(Q[0],Q[1],Q[2], P[0],P[1],P[2]);

      clicks++;                        // ADVANCE THE ANIMATION
      damage = true;
   }

//--- ROTATE VIEW IN RESPONSE TO MOUSE DRAG ---

   int mx=0, my=0;
   double theta = 0, phi = 0;
   public boolean mouseDown(Event e, int x, int y) {
      mx = x;
      my = y;
      return true;
   }
   public boolean mouseDrag(Event e, int x, int y) {
      theta = Math.max(-Math.PI/2,
              Math.min( Math.PI/2,theta + .01 * (x - mx)));
      phi   = Math.max(-Math.PI/2,
              Math.min( Math.PI/2,phi   + .01 * (y - my)));
      mx = x;
      my = y;
      damage = true;
      return true;
   }

//--------------- RENDER A FRAME ----------------------

   int w, h;
   Matrix3D m = new Matrix3D(), c = new Matrix3D();
   Graphics g;

   public void render(Graphics g) {
      this.g = g;
      w = bounds().width;
      h = bounds().height;

      c.identity();
      c.rotateX(phi);
      c.rotateY(theta);
      c.rotateZ(.7*phi*theta);

      m.identity();

      g.setColor(Color.white);
      g.fillRect(0,0,w,h);
      drawObject();
   }

   void draw(double ax,double ay,double az,double bx,double by,double bz) {
      double[] a = {ax,ay,az}, b = {bx,by,bz};
      draw(a,b);
   }

   int[] p = new int[2], q = new int[2], r = new int[2], s = new int[2];
   void draw(double[] a, double[] b) {
      if (transform(a, p) && transform(b, q))
         g.drawLine(p[0],p[1],q[0],q[1]);
   }

   Vector3D v = new Vector3D();
   boolean transform(double[] a, int[] p) {
      v.set(a[0],a[1],a[2]);
      v.transform(m);
      v.transform(c);
      if (v.get(2) > 2)
         return false;
      double scale = h/(4 - v.get(2));
      p[0] = (int)(w/2+scale*v.get(0));
      p[1] = (int)(h/2-scale*v.get(1));
      return (p[0] >= -4*w && p[0] < 4*w && p[1] >= -4*w && p[1] < 4*w);
   }
}