hMatrix Jacobian_hMatrix(double *theta, double *alpha, double *a, double *d) {
hMatrix T01(4,4), T02(4,4), T03(4,4), T04(4,4), T05(4,4), T06(4,4), T07(4,4);
T01 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 1);
T02 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 2);
T03 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 3);
T04 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 4);
T05 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 5);
T06 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 6);
T07 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 7);
double k[3] = {0,0,1};
double z1[3] = { T01.element(0,2),T01.element(1,2), T01.element(2,2)};
double z2[3] = { T02.element(0,2),T02.element(1,2), T02.element(2,2)};
double z3[3] = { T03.element(0,2),T03.element(1,2), T03.element(2,2)};
double z4[3] = { T04.element(0,2),T04.element(1,2), T04.element(2,2)};
double z5[3] = { T05.element(0,2),T05.element(1,2), T05.element(2,2)};
double z6[3] = { T06.element(0,2),T06.element(1,2), T06.element(2,2)};
double o1[3] = {T01.element(0,3), T01.element(1,3), T01.element(2,3)};
double o2[3] = {T02.element(0,3), T02.element(1,3), T02.element(2,3)};
double o3[3] = {T03.element(0,3), T03.element(1,3), T03.element(2,3)};
double o4[3] = {T04.element(0,3), T04.element(1,3), T04.element(2,3)};
double o5[3] = {T05.element(0,3), T05.element(1,3), T05.element(2,3)};
double o6[3] = {T06.element(0,3), T06.element(1,3), T06.element(2,3)};
double o7[3] = {T07.element(0,3), T07.element(1,3), T07.element(2,3)};
double O1[3] ={o7[0],o7[1],o7[2]};
double O2[3] ={o7[0]-o1[0],o7[1]-o1[1],o7[2]-o1[2]};
double O3[3] ={o7[0]-o2[0],o7[1]-o2[1],o7[2]-o2[2]};
double O4[3] ={o7[0]-o3[0],o7[1]-o3[1],o7[2]-o3[2]};
double O5[3] ={o7[0]-o4[0],o7[1]-o4[1],o7[2]-o4[2]};
double O6[3] ={o7[0]-o5[0],o7[1]-o5[1],o7[2]-o5[2]};
double O7[3] ={o7[0]-o6[0],o7[1]-o6[1],o7[2]-o6[2]};
hMatrix c1(1,3),c2(1,3),c3(1,3),c4(1,3),c5(1,3),c6(1,3),c7(1,3);
c1 = cross(&k[0],&O1[0]);
c2 = cross(&z1[0],&O2[0]);
c3 = cross(&z2[0],&O3[0]);
c4 = cross(&z3[0],&O4[0]);
c5 = cross(&z4[0],&O5[0]);
c6 = cross(&z5[0],&O6[0]);
c7 = cross(&z6[0],&O7[0]);
double J[42] = { c1.element(0,0), c2.element(0,0), c3.element(0,0), c4.element(0,0), c5.element(0,0), c6.element(0,0), c7.element(0,0),
c1.element(0,1), c2.element(0,1), c3.element(0,1), c4.element(0,1), c5.element(0,1), c6.element(0,1), c7.element(0,1),
c1.element(0,2), c2.element(0,2), c3.element(0,2), c4.element(0,2), c5.element(0,2), c6.element(0,2), c7.element(0,2),
k[0],z1[0],z2[0],z3[0],z4[0],z5[0],z6[0],
k[1],z1[1],z2[1],z3[1],z4[1],z5[1],z6[1],
k[2],z1[2],z2[2],z3[2],z4[2],z5[2],z6[2]};
hMatrix Jacobian(6,7);
Jacobian.SET(6,7,&J[0]);
return Jacobian;
}
enum parseType T()
{
char *save = next;
if (!T04()) {
next = save;
if (!T02()) {
next = save;
if (!T03()) {
next = save;
if (!T01()) {
return tERR;
} else return tT01;
} else return tT03;
} else return tT02;
} else return tT04;
return tERR;
}
