// Kinematics module for ijk orientation, output is xyzbc
// where b is master, c is slave, work is rotated, spindle is i=0 j=0 k=1
// +b is a + rotation around y axis.  +c is a + rotation around z axis
// Like a mill with a trunion->rotary.
// b and c must be very well aligned to y and z
// Magintue of ijk vector must be 1.  Of couse we can add math if that's not the case  
// Units are mm and radians unless noted.

// Machine unique parameters read in from a config or ini file

point1=(263.632, 382.583, -82.285) //x,y,z from machine origin to any point on the b axis
point2=(0, 241.211, -3.201) //x,y,c from point 1 to any point on the c axis

// information unique to current setup

point3=(-50, -100, 80) //x,y,z from point2 to work origin (when b and c are 0)
tooloffset=(0,0,50) // end mill is 50 long


// read in a line of gcode.  Note magintude of ijk is 1

G01 X35.18 Y88.176 Z-17.485 I-0.318138 J-0.256624 K0.912651


// forward kinematics

// sum vectors that don't rotate
// for this kin, on this machine, point 1 and tooloffset

vector1=(263.632,382.583,-82.285+50)

// sum vectors that rotate once (on the master b axis)
// for this kin, on this machine, it's just point2

vector2=(0, 241.211, -3.201)

// sum vectors that rotate twice (items on the slave c axis)
// for this kin, on this machine, it's commanded position from gcode, and point3

vector3=(35.18,88.176,-17.485)+(-50, -100, 80)=(-14.82,-11.824,62.515)

//calculate b from ijk
//here is where we need more math if magintude of ijk is not 1
//sign function is sign, not sine
//B=ACOS(K)*-SIGN(I)

B=ACOS(0.912651)*-SIGN(-0.318138)=0.4210727255

//calculate c from jik
//here is where we need more math if magintude of ijk is not 1
//C=-ATAN(J/I)

C=-ATAN(-0.256624/-0.318138)=-0.678778845

//construct unit quaternion for 1st, master, B rotation
//for this kin-machine, a B rotation is on the 0,1,0 axis
//bquat=(COS(B/2),0,SIN(B/2),0)

bquat=(0.9779190208,0,0.2089841829,0)

//construct unit quaternion for 2nd, slave, C rotation
//for this kin-machine, a C rotation is on the 0,0,1 axis
//cquat=(COS(C/2),0,0,SIN(C/2))

cquat=(0.9429581095,0,0,-0.3329114052)

//construct a unit quaternion for a "total" rotation
//because c elements are in the c (slave) frame, they're acutually rotated twice.
//first by master, then by slave.  B then C.
//the elegant solution is to use the Hamiltion product 
//that is to say, "rotate" cquat by bquat
//my hope is to use a C++ library quaternion function
//I have the manual maths to include if need be. It's not too bad. No trig functions
//tquat=cquat*bquat

tquat=(0.9221366711,-0.069573218,0.19706333,-0.3255603954)

//rotate b elements(vector2) by bquat
//my hope is to use a C++ quaternion library
//again, below are Hamiltion prodcuts
//I have the manual maths to include if need be. It's kinda long, but no trig functions.
//vector2rotated=q*r*q^(-1)
//vector2rotated=tquat*vector2*tquat^(-1)

vector2rotated=(-1.3083742273,241.211,-2.9213965635)

//rotate c elements(vector3) by tquat
//my hope is to use a C++ quaternion library
//again, below are Hamiltion prodcuts
//I have the manual maths to include if need be. It's kinda long, but no trig functions.
//vector3rotated=q*r*q^(-1)

vector3rotated=(8.2497292885,0.1015414636,64.8035204059)

//sum all the rotated vectors
machinecoordinates=vector1+vector2rotated+vector3rotated
machinecoordinates=(270.5733550611,623.8955414636,29.5971238424)

//I have no idea how to get linuxcnc to understand the infromation above.

pos->tran.x = joints[0]; //X component of machinecoordinates = 270.5733550611
pos->tran.y = joints[1]; //Y component of machinecoordinates = 623.8955414636
pos->tran.z = joints[2]; //Z component of machinecoordinates = 29.5971238424
pos->a = joints[3]; // unused in this kin-machine combination
pos->b = joints[4]; // B from above ACOS(K)*-SIGN(I)
pos->c = joints[5]; // C from above -ATAN(J/I)



// inverse kinematics

joints[0] = pos->tran.x;
joints[1] = pos->tran.y;
joints[2] = pos->tran.z;
joints[3] = pos->a;
joints[4] = pos->b;
joints[5] = pos->c;

// if someone wants to jog b or c, we can update ijk as follows below
// in other words, this will calculate ijk from ab
// this could be used to convert ab gcode to ijk gcode

I=SIN(B)*COS(C)
J=SIN(B)*SIN(C) 
K=COS(B)

//if someone wants to jog machine coordinates, we can update part coordinates this way
//under construction, but I sure it's not too bad.