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;
}
Exemple #2
0
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;
}