/*

## Tutorial example: solving two link inverse kinematics

copyright Ken Perlin, 2000
*/
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);
}
}