% (********************************************)
% (** symoro+ : symbolic modelling of robots **)
% (**========================================**)
% (**      irccyn-ecn - 1, rue de la noe     **)
% (**      b.p.92101                         **)
% (**      44321 nantes cedex 3, france      **)
% (**      www.irccyn.ec-nantes.fr           **)
% (********************************************)


%    name of file : e:/mes_documents_boulot/boulot/recherche/travaux/recherche_personnelle/identification_dynamique/dycaro_orthoglide/matlab/modeles_pr_simulation/orthoglide.dyn




%      geometric parameters   


% j       ant     mu      sigma   gamma   b       alpha   d       theta   r


% 1       0       1       1       0       0       0       0       0       q1

%                                 pi              pi
% 2       0       1       1       --      a       --      0       0       -a + q2
%                                 2               2
%                                                 -pi             -pi
% 3       0       1       1       0       a       ---     0       ---     -a + q3
%                                                  2               2
%                                                 -pi
% 4       1       1       0       0       0       ---     0       q4      0
%                                                  2
%                                                 -pi
% 5       4       1       0       0       0       ---     0       q5      0
%                                                  2

% 6       5       1       0       0       0       0       d4      q6      0

%                                                 pi
% 7       6       1       0       0       0       --      0       q7      0
%                                                 2
%                                                 -pi
% 8       2       1       0       0       0       ---     0       q8      0
%                                                  2
%                                                 -pi
% 9       8       1       0       0       0       ---     0       q9      0
%                                                  2

% 10      9       1       0       0       0       0       d4      q10     0

%                                                 pi
% 11      10      1       0       0       0       --      0       q11     0
%                                                 2
%                                                 -pi
% 12      3       1       0       0       0       ---     0       q12     0
%                                                  2
%                                                 -pi
% 13      12      1       0       0       0       ---     0       q13     0
%                                                  2

% 14      13      1       0       0       0       0       d4      q14     0

%                                                 pi
% 15      14      1       0       0       0       --      0       q15     0
%                                                 2



%              inertial parameters

% j     xx    xy    xz    yy    yz    zz    mx    my    mz    m     ia

% 1     0     0     0     0     0     0     0     0     0     m1    0

% 2     0     0     0     0     0     0     0     0     0     m2    0

% 3     0     0     0     0     0     0     0     0     0     m3    ia3

% 4     0     0     0     0     0     zz4   0     0     0     0     0

% 5     0     0     0     0     0     zz5   mx5   0     0     0     0

% 6     0     0     0     0     0     0     0     0     0     0     0

% 7     0     0     0     0     0     0     0     0     0     0     0

% 8     0     0     0     0     0     0     0     0     0     0     0

% 9     0     0     0     0     0     0     0     0     0     0     0

% 10    0     0     0     0     0     0     0     0     0     0     0

% 11    0     0     0     0     0     0     0     0     0     0     0

% 12    0     0     0     0     0     0     0     0     0     0     0

% 13    0     0     0     0     0     0     0     0     0     0     0

% 14    0     0     0     0     0     0     0     0     0     0     0

% 15    0     0     0     0     0     0     0     0     0     0     0



%  external forces,friction parameters, joint velocities and accelerations

% j       fx      fy      fz      cx      cy      cz      fs      fv      qp      qdp

% 1       0       0       0       0       0       0       fs1     fv1     qd1     qdd1

% 2       0       0       0       0       0       0       fs2     fv2     qd2     qdd2

% 3       0       0       0       0       0       0       fs3     fv3     qd3     qdd3

% 4       0       0       0       0       0       0       0       0       qd4     qdd4

% 5       0       0       0       0       0       0       fs5     0       qd5     qdd5

% 6       0       0       0       0       0       0       0       0       qd6     qdd6

% 7       0       0       0       0       0       0       0       0       qd7     qdd7

% 8       0       0       0       0       0       0       0       0       qd8     qdd8

% 9       0       0       0       0       0       0       fs9     0       qd9     qdd9

% 10      0       0       0       0       0       0       0       0       qd10    qdd10

% 11      0       0       0       0       0       0       0       0       qd11    qdd11

% 12      0       0       0       0       0       0       0       0       qd12    qdd12

% 13      0       0       0       0       0       0       0       0       qd13    qdd13

% 14      0       0       0       0       0       0       0       0       qd14    qdd14

% 15      0       0       0       0       0       0       0       0       qd15    qdd15

% base velocity, base accelerations, and gravity

% j     w0    wp0   v0    vp0   g

% 1     0     0     0     0     0

% 2     0     0     0     0     g2

% 3     0     0     0     0     0

%  dynamic model: newton euler method
% equations:

% declaration of the function
function orthoglide_dyn()

% declaration of global input variables
global a g2 d4 d6
global m1 m2 m3 
global mx4 my4 m4 zz4 mx5 my5 mz5
global m5 yy5 zz5 xx5 xy5 xz5 yz5 
global mx6 my6 mz6 m6 yy6 zz6 xx6 xy6 xz6 yz6
global mx7 my7 mz7 m7 yy7 zz7 xx7 xy7
global xz7 yz7 mx8 my8 m8 zz8
global mx9 my9 mz9 m9 yy9 zz9 xx9 xy9 xz9 yz9
global mx10 my10 mz10 m10 yy10 zz10 xx10 xy10
global xz10 yz10 mx11 my11 mz11 m11 yy11 zz11
global xx11 xy11 xz11 yz11 mx12 my12 m12 zz12
global mx13 my13 mz13 m13 yy13 zz13 xx13 xy13
global xz13 yz13 mx14 my14 mz14 m14 yy14 zz14
global xx14 xy14 xz14 yz14 mx15 my15 mz15 m15
global yy15 zz15 xx15 xy15 xz15 yz15 fv1 ia1 fs1
global fv2 ia2 fs2 fv3 ia3 fs3 fv4 fs4
global fv5 fs5 fv6 fs6 fv7 fs7 fv8 fs8 fv9 fs9
global fv10 fs10 fv11 fs11 fv12 fs12 fv13 fs13 fv14 fs14
global fv15 fs15

% actuator position, velocity and acceleration
global q1 q2 q3
global qd1 qd2 qd3
global qdd1 qdd2 qdd3

% declaration of global output variables
global gam1 gam2 gam3


global q4 q5 q6 q7 q8 q9 q10 q11 q12 q13
global q14 q15 m1 qdd1 m2 qdd2 g2 qdd3 m3 qd4
global mx4 my4 qdd4 m4 zz4 qd5 qdd5 mx5 my5 mz5
global m5 yy5 zz5 xx5 xy5 xz5 yz5 qd6 qdd6 d4
global mx6 my6 mz6 m6 yy6 zz6 xx6 xy6 xz6 yz6
global qd7 qdd7 mx7 my7 mz7 m7 yy7 zz7 xx7 xy7
global xz7 yz7 qd8 mx8 my8 qdd8 m8 zz8 qd9 qdd9
global mx9 my9 mz9 m9 yy9 zz9 xx9 xy9 xz9 yz9
global qd10 qdd10 mx10 my10 mz10 m10 yy10 zz10 xx10 xy10
global xz10 yz10 qd11 qdd11 mx11 my11 mz11 m11 yy11 zz11
global xx11 xy11 xz11 yz11 qd12 mx12 my12 qdd12 m12 zz12
global qd13 qdd13 mx13 my13 mz13 m13 yy13 zz13 xx13 xy13
global xz13 yz13 qd14 qdd14 mx14 my14 mz14 m14 yy14 zz14
global xx14 xy14 xz14 yz14 qd15 qdd15 mx15 my15 mz15 m15
global yy15 zz15 xx15 xy15 xz15 yz15 fv1 qd1 ia1 fs1
global fv2 qd2 ia2 fs2 fv3 qd3 ia3 fs3 fv4 fs4
global fv5 fs5 fv6 fs6 fv7 fs7 fv8 fs8 fv9 fs9
global fv10 fs10 fv11 fs11 fv12 fs12 fv13 fs13 fv14 fs14
global fv15 fs15

% Function description:
    un = ones(length(q1),1);
    
% Forward geometric model
	rho1=q1;
	rho2=q2;
	rho3=q3;
	un=ones(length(rho1),1);
	termgd1=-rho1 - d6.*un;
	termgd2=-rho2 - (d6 - a).*un;
	termgd3=-rho3 - (d6 - a).*un;
	termgd4=(termgd1 + a.*un)./termgd2;
	termgd5=(termgd1.^2 - termgd2.^2 - a^2.*un)./(2.*termgd2);
	termgd6=(termgd1 + a.*un)./termgd3;
	termgd7=(termgd1.^2 - termgd3.^2 - a^2.*un)./(2.*termgd3);	
	termgd8=termgd4.^2 + termgd6.^2 + un;
	termgd9=2.*(termgd4.*termgd5 + termgd6.*termgd7 + termgd4.*termgd2 - a.*un);
	termgd10=termgd5.^2 + termgd7.^2 + termgd2.^2 + 2.*termgd5.*termgd2 + un.*(a^2-d4^2);
	posz=(-termgd9 + sqrt(termgd9.^2 - 4.*termgd8.*termgd10))./(2.*termgd8);	
	posx=termgd4.*posz + termgd5;
	posy=termgd6.*posz + termgd7;
    q4=atan(-(posz + termgd1)./posx);
	s4=sin(q4);
	c4=cos(q4);
    s5=-posy./d4;
    c5=posx./(d4.*c4);
	q5=atan2(s5,c5);
    s5=sin(q5);
    c5=cos(q5);
	q6=-q5;
	q7=-q4-pi/2.*un;
	s6=sin(q6);
	c6=cos(q6);
	s7=sin(q7);
	c7=cos(q7);
    q8=atan(-(posx + termgd2)./posy);
	s8=sin(q8);
	c8=cos(q8);
	s9=-(posz - a.*un)./d4;
	c9=posy./(d4.*c8);
    q9=atan2(s9,c9);
    s9=sin(q9);
    c9=cos(q9);
	q10=-q9;
	q11=-q8-pi/2.*un;
	s10=sin(q10);
	c10=cos(q10);
	s11=sin(q11);
	c11=cos(q11);
    q12=atan(-(posy + termgd3)./(posz - un.*a));
	s12=sin(q12);
	c12=cos(q12);
	s13=-posx./d4;
	c13=(posz - a.*un)./(d4.*c12);
    q13=atan2(s13,c13);
    s13=sin(q13);
    c13=cos(q13);
	q14=-q13;
	q15=-q12-pi/2.*un;
	s14=sin(q14);
	c14=cos(q14);
	s15=sin(q15);
	c15=cos(q15);
 
% Forward kinematic model of the first order
	rhod1=qd1;
	rhod2=qd2;
	rhod3=qd3;
	a11=posx;
	a12=posy;
	a13=posz + termgd1;
	a21=posx + termgd2;
	a22=posy;
	a23=posz - a.*un;
	a31=posx;
	a32=posy + termgd3;
	a33=posz - a.*un;
	deta = a11.*(a22.*a33 - a23.*a32) - a21.*a12.*a33 + a21.*a13.*a32 + a31.*a12.*a23 - a31.*a13.*a22;
	j11=(a22.*a33 - a23.*a32).*a13./deta;
	j12=-a21.*(a12.*a33 - a13.*a32)./deta;
	j13=(a12.*a23 - a13.*a22).*a32./deta;
	j21=-(a21.*a33 - a23.*a31).*a13./deta;
	j22=(a11.*a33 - a13.*a31).*a21./deta;
	j23=-(a11.*a23 - a13.*a21).*a32./deta;
	j31=(a21.*a32 - a22.*a31).*a13./deta;
	j32=-(a11.*a32 - a12.*a31).*a21./deta;
	j33=(a11.*a22 - a21.*a12).*a32./deta;
	posdx=j11.*rhod1 + j12.*rhod2 + j13.*rhod3;
	posdy=j21.*rhod1 + j22.*rhod2 + j23.*rhod3;
	posdz=j31.*rhod1 + j32.*rhod2 + j33.*rhod3;
	
	jc112=-d4.*s4.*c5;
	jc113=-d4.*c4.*s5;
	jc123=-d4.*c5;
	jc131=un;
	jc132=-d4.*c4.*c5;
	jc133=d4.*s4.*s5;
	dejc1=jc112.*jc123;
	ijc111=-jc123.*jc132./dejc1;
	ijc112=(-jc112.*jc133 + jc113.*jc132)./dejc1;
	ijc113=jc112.*jc123./dejc1;
	ijc121=jc123./dejc1;
	ijc122=-jc113./dejc1;
	ijc132=jc112./dejc1;
	qd4=ijc121.*posdx + ijc122.*posdy;
	qd5=ijc132.*posdy;
	qd6=-qd5;
	qd7=-qd4;
 	
	jc211=un;
	jc212=-d4.*c8.*c9;
	jc213=d4.*s8.*s9;
	jc222=-d4.*s8.*c9;
	jc223=-d4.*c8.*s9;
	jc233=-d4.*c9;
	dejc2=jc211.*jc222.*jc233;
	ijc211=jc222.*jc233./dejc2;
	ijc212=-jc212.*jc233./dejc2;
	ijc213=(jc212.*jc223 - jc213.*jc222)./dejc2;
	ijc222=jc211.*jc233./dejc2;
	ijc223=-jc211.*jc223./dejc2;
	ijc233=jc211.*jc222./dejc2;
	qd8=ijc222.*posdy + ijc223.*posdz;
	qd9=ijc233.*posdz;
	qd10=-qd9;
	qd11=-qd8;
	
	jc313=-d4.*c13;
	jc321=un;
	jc322=-d4.*c12.*c13;
	jc323=d4.*s12.*s13;
	jc332=-d4.*s12.*c13;
	jc333=-d4.*c12.*s13;
	dejc3=jc321.*jc313.*jc332;
	ijc311=(jc322.*jc333 - jc323.*jc332)./dejc3;
	ijc312=jc313.*jc332./dejc3;
	ijc313=- jc313.*jc322./dejc3;
	ijc321=-jc321.*jc333./dejc3;
	ijc323=jc313.*jc321./dejc3;
	ijc331=jc321.*jc332./dejc3;
	qd12=ijc321.*posdx + ijc323.*posdz;
	qd13=ijc331.*posdx;
	qd14=-qd13;
	qd15=-qd12;	
	
% Forward kinematic model of the second order
	rhodd1=qdd1;
	rhodd2=qdd2;
	rhodd3=qdd3;	
	ad11=posdx;
	ad12=posdy;
	ad13=posdz - rhod1;
	ad21=posdx - rhod2;
	ad22=posdy;
	ad23=posdz;
	ad31=posdx;
	ad32=posdy - rhod3;
	ad33=posdz;
	bd11=ad13;
	bd22=ad21;
	bd33=ad32;
	m1mcd2o11=(-a22.*ad11.*a33 + a22.*ad31.*a13 + ad21.*a12.*a33 + ad11.*a23.*a32 - ad31.*a12.*a23 - ad21.*a13.*a32)./deta;
	m1mcd2o12=(-a22.*ad12.*a33 + a22.*ad32.*a13 + ad22.*a12.*a33 - ad32.*a12.*a23 + ad12.*a23.*a32 - ad22.*a13.*a32)./deta;
	m1mcd2o13=(-a22.*ad13.*a33 + a22.*ad33.*a13 + ad23.*a12.*a33 + ad13.*a23.*a32 - ad33.*a12.*a23 - ad23.*a13.*a32)./deta;
	m1mcd2o21=((-ad21.*a33 + ad31.*a23).*a11 + ad11.*a21.*a33 - ad31.*a13.*a21 - ad11.*a23.*a31 + ad21.*a13.*a31)./deta;
	m1mcd2o22=((-ad22.*a33 + ad32.*a23).*a11 + ad12.*a21.*a33 - ad12.*a23.*a31 - ad32.*a13.*a21 + ad22.*a13.*a31)./deta;
	m1mcd2o23=((-ad23.*a33 + ad33.*a23).*a11 + ad13.*a21.*a33 - ad13.*a23.*a31 + ad23.*a13.*a31 - ad33.*a13.*a21)./deta;
	m1mcd2o31=((-ad31.*a22 + ad21.*a32).*a11 + ad11.*a31.*a22 + ad31.*a12.*a21 - ad21.*a12.*a31 - ad11.*a21.*a32)./deta;
	m1mcd2o32=((-ad32.*a22 + ad22.*a32).*a11 + ad12.*a31.*a22 - ad12.*a21.*a32 - ad22.*a12.*a31 + ad32.*a12.*a21)./deta;
	m1mcd2o33=((-ad33.*a22 + ad23.*a32).*a11 + ad13.*a31.*a22 - ad23.*a12.*a31 + ad33.*a12.*a21 - ad13.*a21.*a32)./deta;
	m2mcd2o11=((a22.*a33 - a23.*a32).*ad13)./deta;
	m2mcd2o12=(-(a12.*a33 - a13.*a32).*ad21)./deta;
	m2mcd2o13=((a12.*a23 - a13.*a22).*ad32)./deta;
	m2mcd2o21=(-(a21.*a33 - a23.*a31).*ad13)./deta;
	m2mcd2o22=((a11.*a33 - a13.*a31).*ad21)./deta;
	m2mcd2o23=(-(a11.*a23 - a13.*a21).*ad32)./deta;
	m2mcd2o31=((a21.*a32 - a22.*a31).*ad13)./deta;
	m2mcd2o32=(-(a11.*a32 - a12.*a31).*ad21)./deta;
	m2mcd2o33=((a11.*a22 - a12.*a21).*ad32)./deta;
	m3mcd2o11=((a22.*a33 - a23.*a32).*a13)./deta;
	m3mcd2o12=(-(a12.*a33 - a13.*a32).*a21)./deta;
	m3mcd2o13=((a12.*a23 - a13.*a22).*a32)./deta;
	m3mcd2o21=(-(a21.*a33 - a23.*a31).*a13)./deta;
	m3mcd2o22=((a11.*a33 - a13.*a31).*a21)./deta;
	m3mcd2o23=(-(a11.*a23 - a13.*a21).*a32)./deta;
	m3mcd2o31=((a21.*a32 - a22.*a31).*a13)./deta;
	m3mcd2o32=(-(a11.*a32 - a12.*a31).*a21)./deta;
	m3mcd2o33=((a11.*a22 - a12.*a21).*a32)./deta;	
	posddx=(m1mcd2o11.*posdx + m1mcd2o12.*posdy + m1mcd2o13.*posdz) + (m2mcd2o11.*rhod1 + m2mcd2o12.*rhod2 + m2mcd2o13.*rhod3) + (m3mcd2o11.*rhodd1 + m3mcd2o12.*rhodd2 + m3mcd2o13.*rhodd3);
	posddy=(m1mcd2o21.*posdx + m1mcd2o22.*posdy + m1mcd2o23.*posdz) + (m2mcd2o21.*rhod1 + m2mcd2o22.*rhod2 + m2mcd2o23.*rhod3) + (m3mcd2o21.*rhodd1 + m3mcd2o22.*rhodd2 + m3mcd2o23.*rhodd3);
	posddz=(m1mcd2o31.*posdx + m1mcd2o32.*posdy + m1mcd2o33.*posdz) + (m2mcd2o31.*rhod1 + m2mcd2o32.*rhod2 + m2mcd2o33.*rhod3) + (m3mcd2o31.*rhodd1 + m3mcd2o32.*rhodd2 + m3mcd2o33.*rhodd3);
	
	jc1d12=-d4.*(qd4.*c4.*c5 - qd5.*s4.*s5);
	jc1d13=-d4.*(-qd4.*s4.*s5 + qd5.*c4.*c5);
	jc1d23=+d4.*qd5.*s5;
	jc1d32=-d4.*(-qd4.*s4.*c5 - qd5.*c4.*s5);
	jc1d33=d4.*(qd4.*c4.*s5 + qd5.*s4.*c5);
	mc1mcd2o12=(jc123.*(jc1d12.*jc132 - jc112.*jc1d32))./dejc1;
	mc1mcd2o13=(jc123.*jc132.*jc1d13 + jc133.*jc112.*jc1d23 - jc1d23.*jc113.*jc132 - jc112.*jc123.*jc1d33)./dejc1;
	mc1mcd2o22=(-jc1d12.*jc123)./dejc1;
	mc1mcd2o23=(-jc1d13.*jc123 + jc113.*jc1d23)./dejc1;
	mc1mcd2o33=(-jc112.*jc1d23)./dejc1;
	qdd4=(ijc121.*posddx + ijc122.*posddy) + (mc1mcd2o22.*qd4 + mc1mcd2o23.*qd5);
	qdd5=(ijc132.*posddy) + (mc1mcd2o33.*qd5);
	qdd6=-qdd5;
	qdd7=-qdd4;
	
	jc2d12=-d4.*(-qd8.*s8.*c9 - qd9.*c8.*s9);
	jc2d13=d4.*(qd8.*c8.*s9 + qd9.*s8.*c9);
	jc2d22=-d4.*(qd8.*c8.*c9 - qd9.*s8.*s9);
	jc2d23=-d4.*(-qd8.*s8.*s9 + qd9.*c8.*c9);
	jc2d33=+d4.*qd9.*s9;
	mc2mcd2o12=(-jc222.*jc233.*jc2d12 + jc233.*jc212.*jc2d22)./dejc2;
	mc2mcd2o13=((jc233.*jc2d23 - jc223.*jc2d33).*jc212 - jc222.*jc233.*jc2d13 + jc222.*jc2d33.*jc213)./dejc2;
	mc2mcd2o22=(-jc233.*jc2d22)./dejc2;
	mc2mcd2o23=(-jc233.*jc2d23 + jc223.*jc2d33)./dejc2;
	mc2mcd2o33=(-jc222.*jc2d33)./dejc2;
	qdd8=(ijc222.*posddy + ijc223.*posddz) + (mc2mcd2o22.*qd8 + mc2mcd2o23.*qd9);
	qdd9=(ijc233.*posddz) + (mc2mcd2o33.*qd9);
	qdd10=-qdd9;
	qdd11=-qdd8;
	
	jc3d13=+d4.*qd13.*s13;
	jc3d22=-d4.*(-qd12.*s12.*c13 - qd13.*c12.*s13);
	jc3d23=d4.*(qd12.*c12.*s13 + qd13.*s12.*c13);
	jc3d32=-d4.*(qd12.*c12.*c13 - qd13.*s12.*s13);
	jc3d33=-d4.*(-qd12.*s12.*s13 + qd13.*c12.*c13);
	mc3mcd2o12=(jc313.*(-jc332.*jc3d22 + jc322.*jc3d32))./dejc3;
	mc3mcd2o13=((-jc332.*jc3d23 + jc322.*jc3d33).*jc313 - jc3d13.*jc322.*jc333 + jc332.*jc3d13.*jc323)./dejc3;
	mc3mcd2o22=(-jc313.*jc3d32)./dejc3;
	mc3mcd2o23=(jc333.*jc3d13 - jc313.*jc3d33)./dejc3;
	mc3mcd2o33=(-jc332.*jc3d13)./dejc3;
	qdd12=(ijc321.*posddx + ijc323.*posddz) + (mc3mcd2o22.*qd12 + mc3mcd2o23.*qd13);
	qdd13=(ijc331.*posddx) + (mc3mcd2o33.*qd13);
	qdd14=-qdd13;
	qdd15=-qdd12;
	
% Inverse dynamic model of the virtual tree structure

	s4=sin(q4);
	c4=cos(q4);
	s5=sin(q5);
	c5=cos(q5);
	s6=sin(q6);
	c6=cos(q6);
	s7=sin(q7);
	c7=cos(q7);
	s8=sin(q8);
	c8=cos(q8);
	s9=sin(q9);
	c9=cos(q9);
	s10=sin(q10);
	c10=cos(q10);
	s11=sin(q11);
	c11=cos(q11);
	s12=sin(q12);
	c12=cos(q12);
	s13=sin(q13);
	c13=cos(q13);
	s14=sin(q14);
	c14=cos(q14);
	s15=sin(q15);
	c15=cos(q15);
	f31=m1.*qdd1;
	f32=m2.*qdd2;
	vp33=-g2 + qdd3;
	f33=m3.*vp33;
	dv334=-qd4.^2;
	vp14=-(qdd1.*s4);
	vp24=-(c4.*qdd1);
	f14=dv334.*mx4 - my4.*qdd4 + m4.*vp14;
	f24=dv334.*my4 + mx4.*qdd4 + m4.*vp24;
	no34=qdd4.*zz4;
	wi15=-(qd4.*s5);
	wi25=-(c5.*qd4);
	wp15=-(qdd4.*s5) + qd5.*wi25;
	wp25=-(c5.*qdd4) - qd5.*wi15;
	dv115=-wi15.^2;
	dv225=-wi25.^2;
	dv335=-qd5.^2;
	dv125=wi15.*wi25;
	dv135=qd5.*wi15;
	dv235=qd5.*wi25;
	u115=dv225 + dv335;
	u125=dv125 - qdd5;
	u135=dv135 + wp25;
	u215=dv125 + qdd5;
	u225=dv115 + dv335;
	u235=dv235 - wp15;
	u315=dv135 - wp25;
	u325=dv235 + wp15;
	u335=dv115 + dv225;
	vp15=g2.*s5 + c5.*vp14;
	vp25=c5.*g2 - s5.*vp14;
	f15=mx5.*u115 + my5.*u125 + mz5.*u135 + m5.*vp15;
	f25=mx5.*u215 + my5.*u225 + mz5.*u235 + m5.*vp25;
	f35=mx5.*u315 + my5.*u325 + mz5.*u335 + m5.*vp24;
	pis15=-yy5 + zz5;
	pis25=xx5 - zz5;
	pis35=-xx5 + yy5;
	no15=dv235.*pis15 + wp15.*xx5 - u315.*xy5 + u215.*xz5 + (-dv225 + dv335).*yz5;
	no25=dv135.*pis25 + u325.*xy5 + (dv115 - dv335).*xz5 + wp25.*yy5 - u125.*yz5;
	no35=dv125.*pis35 + (-dv115 + dv225).*xy5 - u235.*xz5 + u135.*yz5 + qdd5.*zz5;
	wi16=c6.*wi15 + s6.*wi25;
	wi26=-(s6.*wi15) + c6.*wi25;
	w36=qd5 + qd6;
	wp16=qd6.*wi26 + c6.*wp15 + s6.*wp25;
	wp26=-(qd6.*wi16) - s6.*wp15 + c6.*wp25;
	wp36=qdd5 + qdd6;
	dv116=-wi16.^2;
	dv226=-wi26.^2;
	dv336=-w36.^2;
	dv126=wi16.*wi26;
	dv136=w36.*wi16;
	dv236=w36.*wi26;
	u116=dv226 + dv336;
	u126=dv126 - wp36;
	u136=dv136 + wp26;
	u216=dv126 + wp36;
	u226=dv116 + dv336;
	u236=dv236 - wp16;
	u316=dv136 - wp26;
	u326=dv236 + wp16;
	u336=dv116 + dv226;
	vsp16=d4.*u115 + vp15;
	vsp26=d4.*u215 + vp25;
	vsp36=d4.*u315 + vp24;
	vp16=c6.*vsp16 + s6.*vsp26;
	vp26=-(s6.*vsp16) + c6.*vsp26;
	f16=mx6.*u116 + my6.*u126 + mz6.*u136 + m6.*vp16;
	f26=mx6.*u216 + my6.*u226 + mz6.*u236 + m6.*vp26;
	f36=mx6.*u316 + my6.*u326 + mz6.*u336 + m6.*vsp36;
	pis16=-yy6 + zz6;
	pis26=xx6 - zz6;
	pis36=-xx6 + yy6;
	no16=dv236.*pis16 + wp16.*xx6 - u316.*xy6 + u216.*xz6 + (-dv226 + dv336).*yz6;
	no26=dv136.*pis26 + u326.*xy6 + (dv116 - dv336).*xz6 + wp26.*yy6 - u126.*yz6;
	no36=dv126.*pis36 + (-dv116 + dv226).*xy6 - u236.*xz6 + u136.*yz6 + wp36.*zz6;
	wi17=s7.*w36 + c7.*wi16;
	wi27=c7.*w36 - s7.*wi16;
	w37=qd7 - wi26;
	wp17=qd7.*wi27 + c7.*wp16 + s7.*wp36;
	wp27=-(qd7.*wi17) - s7.*wp16 + c7.*wp36;
	wp37=qdd7 - wp26;
	dv117=-wi17.^2;
	dv227=-wi27.^2;
	dv337=-w37.^2;
	dv127=wi17.*wi27;
	dv137=w37.*wi17;
	dv237=w37.*wi27;
	u117=dv227 + dv337;
	u127=dv127 - wp37;
	u137=dv137 + wp27;
	u217=dv127 + wp37;
	u227=dv117 + dv337;
	u237=dv237 - wp17;
	u317=dv137 - wp27;
	u327=dv237 + wp17;
	u337=dv117 + dv227;
	vp17=c7.*vp16 + s7.*vsp36;
	vp27=-(s7.*vp16) + c7.*vsp36;
	f17=mx7.*u117 + my7.*u127 + mz7.*u137 + m7.*vp17;
	f27=mx7.*u217 + my7.*u227 + mz7.*u237 + m7.*vp27;
	f37=mx7.*u317 + my7.*u327 + mz7.*u337 - m7.*vp26;
	pis17=-yy7 + zz7;
	pis27=xx7 - zz7;
	pis37=-xx7 + yy7;
	no17=dv237.*pis17 + wp17.*xx7 - u317.*xy7 + u217.*xz7 + (-dv227 + dv337).*yz7;
	no27=dv137.*pis27 + u327.*xy7 + (dv117 - dv337).*xz7 + wp27.*yy7 - u127.*yz7;
	no37=dv127.*pis37 + (-dv117 + dv227).*xy7 - u237.*xz7 + u137.*yz7 + wp37.*zz7;
	dv338=-qd8.^2;
	vp18=-(c8.*g2) - qdd2.*s8;
	vp28=-(c8.*qdd2) + g2.*s8;
	f18=dv338.*mx8 - my8.*qdd8 + m8.*vp18;
	f28=dv338.*my8 + mx8.*qdd8 + m8.*vp28;
	no38=qdd8.*zz8;
	wi19=-(qd8.*s9);
	wi29=-(c9.*qd8);
	wp19=-(qdd8.*s9) + qd9.*wi29;
	wp29=-(c9.*qdd8) - qd9.*wi19;
	dv119=-wi19.^2;
	dv229=-wi29.^2;
	dv339=-qd9.^2;
	dv129=wi19.*wi29;
	dv139=qd9.*wi19;
	dv239=qd9.*wi29;
	u119=dv229 + dv339;
	u129=dv129 - qdd9;
	u139=dv139 + wp29;
	u219=dv129 + qdd9;
	u229=dv119 + dv339;
	u239=dv239 - wp19;
	u319=dv139 - wp29;
	u329=dv239 + wp19;
	u339=dv119 + dv229;
	vp19=c9.*vp18;
	vp29=-(s9.*vp18);
	f19=mx9.*u119 + my9.*u129 + mz9.*u139 + m9.*vp19;
	f29=mx9.*u219 + my9.*u229 + mz9.*u239 + m9.*vp29;
	f39=mx9.*u319 + my9.*u329 + mz9.*u339 + m9.*vp28;
	pis19=-yy9 + zz9;
	pis29=xx9 - zz9;
	pis39=-xx9 + yy9;
	no19=dv239.*pis19 + wp19.*xx9 - u319.*xy9 + u219.*xz9 + (-dv229 + dv339).*yz9;
	no29=dv139.*pis29 + u329.*xy9 + (dv119 - dv339).*xz9 + wp29.*yy9 - u129.*yz9;
	no39=dv129.*pis39 + (-dv119 + dv229).*xy9 - u239.*xz9 + u139.*yz9 + qdd9.*zz9;
	wi110=c10.*wi19 + s10.*wi29;
	wi210=-(s10.*wi19) + c10.*wi29;
	w310=qd10 + qd9;
	wp110=qd10.*wi210 + c10.*wp19 + s10.*wp29;
	wp210=-(qd10.*wi110) - s10.*wp19 + c10.*wp29;
	wp310=qdd10 + qdd9;
	dv1110=-wi110.^2;
	dv2210=-wi210.^2;
	dv3310=-w310.^2;
	dv1210=wi110.*wi210;
	dv1310=w310.*wi110;
	dv2310=w310.*wi210;
	u1110=dv2210 + dv3310;
	u1210=dv1210 - wp310;
	u1310=dv1310 + wp210;
	u2110=dv1210 + wp310;
	u2210=dv1110 + dv3310;
	u2310=dv2310 - wp110;
	u3110=dv1310 - wp210;
	u3210=dv2310 + wp110;
	u3310=dv1110 + dv2210;
	vsp110=d4.*u119 + vp19;
	vsp210=d4.*u219 + vp29;
	vsp310=d4.*u319 + vp28;
	vp110=c10.*vsp110 + s10.*vsp210;
	vp210=-(s10.*vsp110) + c10.*vsp210;
	f110=mx10.*u1110 + my10.*u1210 + mz10.*u1310 + m10.*vp110;
	f210=mx10.*u2110 + my10.*u2210 + mz10.*u2310 + m10.*vp210;
	f310=mx10.*u3110 + my10.*u3210 + mz10.*u3310 + m10.*vsp310;
	pis110=-yy10 + zz10;
	pis210=xx10 - zz10;
	pis310=-xx10 + yy10;
	no110=dv2310.*pis110 + wp110.*xx10 - u3110.*xy10 + u2110.*xz10 + (-dv2210 + dv3310).*yz10;
	no210=dv1310.*pis210 + u3210.*xy10 + (dv1110 - dv3310).*xz10 + wp210.*yy10 - u1210.*yz10;
	no310=dv1210.*pis310 + (-dv1110 + dv2210).*xy10 - u2310.*xz10 + u1310.*yz10 + wp310.*zz10;
	wi111=s11.*w310 + c11.*wi110;
	wi211=c11.*w310 - s11.*wi110;
	w311=qd11 - wi210;
	wp111=qd11.*wi211 + c11.*wp110 + s11.*wp310;
	wp211=-(qd11.*wi111) - s11.*wp110 + c11.*wp310;
	wp311=qdd11 - wp210;
	dv1111=-wi111.^2;
	dv2211=-wi211.^2;
	dv3311=-w311.^2;
	dv1211=wi111.*wi211;
	dv1311=w311.*wi111;
	dv2311=w311.*wi211;
	u1111=dv2211 + dv3311;
	u1211=dv1211 - wp311;
	u1311=dv1311 + wp211;
	u2111=dv1211 + wp311;
	u2211=dv1111 + dv3311;
	u2311=dv2311 - wp111;
	u3111=dv1311 - wp211;
	u3211=dv2311 + wp111;
	u3311=dv1111 + dv2211;
	vp111=c11.*vp110 + s11.*vsp310;
	vp211=-(s11.*vp110) + c11.*vsp310;
	f111=mx11.*u1111 + my11.*u1211 + mz11.*u1311 + m11.*vp111;
	f211=mx11.*u2111 + my11.*u2211 + mz11.*u2311 + m11.*vp211;
	f311=mx11.*u3111 + my11.*u3211 + mz11.*u3311 - m11.*vp210;
	pis111=-yy11 + zz11;
	pis211=xx11 - zz11;
	pis311=-xx11 + yy11;
	no111=dv2311.*pis111 + wp111.*xx11 - u3111.*xy11 + u2111.*xz11 + (-dv2211 + dv3311).*yz11;
	no211=dv1311.*pis211 + u3211.*xy11 + (dv1111 - dv3311).*xz11 + wp211.*yy11 - u1211.*yz11;
	no311=dv1211.*pis311 + (-dv1111 + dv2211).*xy11 - u2311.*xz11 + u1311.*yz11 + wp311.*zz11;
	dv3312=-qd12.^2;
	vp112=-(s12.*vp33);
	vp212=-(c12.*vp33);
	f112=dv3312.*mx12 - my12.*qdd12 + m12.*vp112;
	f212=dv3312.*my12 + mx12.*qdd12 + m12.*vp212;
	no312=qdd12.*zz12;
	wi113=-(qd12.*s13);
	wi213=-(c13.*qd12);
	wp113=-(qdd12.*s13) + qd13.*wi213;
	wp213=-(c13.*qdd12) - qd13.*wi113;
	dv1113=-wi113.^2;
	dv2213=-wi213.^2;
	dv3313=-qd13.^2;
	dv1213=wi113.*wi213;
	dv1313=qd13.*wi113;
	dv2313=qd13.*wi213;
	u1113=dv2213 + dv3313;
	u1213=dv1213 - qdd13;
	u1313=dv1313 + wp213;
	u2113=dv1213 + qdd13;
	u2213=dv1113 + dv3313;
	u2313=dv2313 - wp113;
	u3113=dv1313 - wp213;
	u3213=dv2313 + wp113;
	u3313=dv1113 + dv2213;
	vp113=c13.*vp112;
	vp213=-(s13.*vp112);
	f113=mx13.*u1113 + my13.*u1213 + mz13.*u1313 + m13.*vp113;
	f213=mx13.*u2113 + my13.*u2213 + mz13.*u2313 + m13.*vp213;
	f313=mx13.*u3113 + my13.*u3213 + mz13.*u3313 + m13.*vp212;
	pis113=-yy13 + zz13;
	pis213=xx13 - zz13;
	pis313=-xx13 + yy13;
	no113=dv2313.*pis113 + wp113.*xx13 - u3113.*xy13 + u2113.*xz13 + (-dv2213 + dv3313).*yz13;
	no213=dv1313.*pis213 + u3213.*xy13 + (dv1113 - dv3313).*xz13 + wp213.*yy13 - u1213.*yz13;
	no313=dv1213.*pis313 + (-dv1113 + dv2213).*xy13 - u2313.*xz13 + u1313.*yz13 + qdd13.*zz13;
	wi114=c14.*wi113 + s14.*wi213;
	wi214=-(s14.*wi113) + c14.*wi213;
	w314=qd13 + qd14;
	wp114=qd14.*wi214 + c14.*wp113 + s14.*wp213;
	wp214=-(qd14.*wi114) - s14.*wp113 + c14.*wp213;
	wp314=qdd13 + qdd14;
	dv1114=-wi114.^2;
	dv2214=-wi214.^2;
	dv3314=-w314.^2;
	dv1214=wi114.*wi214;
	dv1314=w314.*wi114;
	dv2314=w314.*wi214;
	u1114=dv2214 + dv3314;
	u1214=dv1214 - wp314;
	u1314=dv1314 + wp214;
	u2114=dv1214 + wp314;
	u2214=dv1114 + dv3314;
	u2314=dv2314 - wp114;
	u3114=dv1314 - wp214;
	u3214=dv2314 + wp114;
	u3314=dv1114 + dv2214;
	vsp114=d4.*u1113 + vp113;
	vsp214=d4.*u2113 + vp213;
	vsp314=d4.*u3113 + vp212;
	vp114=c14.*vsp114 + s14.*vsp214;
	vp214=-(s14.*vsp114) + c14.*vsp214;
	f114=mx14.*u1114 + my14.*u1214 + mz14.*u1314 + m14.*vp114;
	f214=mx14.*u2114 + my14.*u2214 + mz14.*u2314 + m14.*vp214;
	f314=mx14.*u3114 + my14.*u3214 + mz14.*u3314 + m14.*vsp314;
	pis114=-yy14 + zz14;
	pis214=xx14 - zz14;
	pis314=-xx14 + yy14;
	no114=dv2314.*pis114 + wp114.*xx14 - u3114.*xy14 + u2114.*xz14 + (-dv2214 + dv3314).*yz14;
	no214=dv1314.*pis214 + u3214.*xy14 + (dv1114 - dv3314).*xz14 + wp214.*yy14 - u1214.*yz14;
	no314=dv1214.*pis314 + (-dv1114 + dv2214).*xy14 - u2314.*xz14 + u1314.*yz14 + wp314.*zz14;
	wi115=s15.*w314 + c15.*wi114;
	wi215=c15.*w314 - s15.*wi114;
	w315=qd15 - wi214;
	wp115=qd15.*wi215 + c15.*wp114 + s15.*wp314;
	wp215=-(qd15.*wi115) - s15.*wp114 + c15.*wp314;
	wp315=qdd15 - wp214;
	dv1115=-wi115.^2;
	dv2215=-wi215.^2;
	dv3315=-w315.^2;
	dv1215=wi115.*wi215;
	dv1315=w315.*wi115;
	dv2315=w315.*wi215;
	u1115=dv2215 + dv3315;
	u1215=dv1215 - wp315;
	u1315=dv1315 + wp215;
	u2115=dv1215 + wp315;
	u2215=dv1115 + dv3315;
	u2315=dv2315 - wp115;
	u3115=dv1315 - wp215;
	u3215=dv2315 + wp115;
	u3315=dv1115 + dv2215;
	vp115=c15.*vp114 + s15.*vsp314;
	vp215=-(s15.*vp114) + c15.*vsp314;
	f115=mx15.*u1115 + my15.*u1215 + mz15.*u1315 + m15.*vp115;
	f215=mx15.*u2115 + my15.*u2215 + mz15.*u2315 + m15.*vp215;
	f315=mx15.*u3115 + my15.*u3215 + mz15.*u3315 - m15.*vp214;
	pis115=-yy15 + zz15;
	pis215=xx15 - zz15;
	pis315=-xx15 + yy15;
	no115=dv2315.*pis115 + wp115.*xx15 - u3115.*xy15 + u2115.*xz15 + (-dv2215 + dv3315).*yz15;
	no215=dv1315.*pis215 + u3215.*xy15 + (dv1115 - dv3315).*xz15 + wp215.*yy15 - u1215.*yz15;
	no315=dv1215.*pis315 + (-dv1115 + dv2215).*xy15 - u2315.*xz15 + u1315.*yz15 + wp315.*zz15;
	n115=no115 - my15.*vp214 - mz15.*vp215;
	n215=no215 + mz15.*vp115 + mx15.*vp214;
	n315=no315 - my15.*vp115 + mx15.*vp215;
	fdi115=c15.*f115 - f215.*s15;
	fdi315=c15.*f215 + f115.*s15;
	e114=f114 + fdi115;
	e214=f214 - f315;
	e314=f314 + fdi315;
	n114=c15.*n115 + no114 - n215.*s15 - mz14.*vp214 + my14.*vsp314;
	n214=-n315 + no214 + mz14.*vp114 - mx14.*vsp314;
	n314=c15.*n215 + no314 + n115.*s15 - my14.*vp114 + mx14.*vp214;
	fdi114=c14.*e114 - e214.*s14;
	fdi214=c14.*e214 + e114.*s14;
	e113=f113 + fdi114;
	e213=f213 + fdi214;
	e313=e314 + f313;
	n113=c14.*n114 + no113 - n214.*s14 + my13.*vp212 - mz13.*vp213;
	n213=-(d4.*e314) + c14.*n214 + no213 + n114.*s14 + mz13.*vp113 - mx13.*vp212;
	n313=d4.*fdi214 + n314 + no313 - my13.*vp113 + mx13.*vp213;
	fdi113=c13.*e113 - e213.*s13;
	e112=f112 + fdi113;
	e212=e313 + f212;
	n312=-(c13.*n213) + no312 - n113.*s13 - my12.*vp112 + mx12.*vp212;
	fdi312=-(c12.*e212) - e112.*s12;
	n111=no111 - my11.*vp210 - mz11.*vp211;
	n211=no211 + mz11.*vp111 + mx11.*vp210;
	n311=no311 - my11.*vp111 + mx11.*vp211;
	fdi111=c11.*f111 - f211.*s11;
	fdi311=c11.*f211 + f111.*s11;
	e110=f110 + fdi111;
	e210=f210 - f311;
	e310=f310 + fdi311;
	n110=c11.*n111 + no110 - n211.*s11 - mz10.*vp210 + my10.*vsp310;
	n210=-n311 + no210 + mz10.*vp110 - mx10.*vsp310;
	n310=c11.*n211 + no310 + n111.*s11 - my10.*vp110 + mx10.*vp210;
	fdi110=c10.*e110 - e210.*s10;
	fdi210=c10.*e210 + e110.*s10;
	e19=f19 + fdi110;
	e29=f29 + fdi210;
	e39=e310 + f39;
	n19=c10.*n110 + no19 - n210.*s10 + my9.*vp28 - mz9.*vp29;
	n29=-(d4.*e310) + c10.*n210 + no29 + n110.*s10 + mz9.*vp19 - mx9.*vp28;
	n39=d4.*fdi210 + n310 + no39 - my9.*vp19 + mx9.*vp29;
	fdi19=c9.*e19 - e29.*s9;
	e18=f18 + fdi19;
	e28=e39 + f28;
	n38=-(c9.*n29) + no38 - n19.*s9 - my8.*vp18 + mx8.*vp28;
	fdi38=-(c8.*e28) - e18.*s8;
	n17=no17 - my7.*vp26 - mz7.*vp27;
	n27=no27 + mz7.*vp17 + mx7.*vp26;
	n37=no37 - my7.*vp17 + mx7.*vp27;
	fdi17=c7.*f17 - f27.*s7;
	fdi37=c7.*f27 + f17.*s7;
	e16=f16 + fdi17;
	e26=f26 - f37;
	e36=f36 + fdi37;
	n16=c7.*n17 + no16 - n27.*s7 - mz6.*vp26 + my6.*vsp36;
	n26=-n37 + no26 + mz6.*vp16 - mx6.*vsp36;
	n36=c7.*n27 + no36 + n17.*s7 - my6.*vp16 + mx6.*vp26;
	fdi16=c6.*e16 - e26.*s6;
	fdi26=c6.*e26 + e16.*s6;
	e15=f15 + fdi16;
	e25=f25 + fdi26;
	e35=e36 + f35;
	n15=c6.*n16 + no15 - n26.*s6 + my5.*vp24 - mz5.*vp25;
	n25=-(d4.*e36) + c6.*n26 + no25 + n16.*s6 + mz5.*vp15 - mx5.*vp24;
	n35=d4.*fdi26 + n36 + no35 - my5.*vp15 + mx5.*vp25;
	fdi15=c5.*e15 - e25.*s5;
	e14=f14 + fdi15;
	e24=e35 + f24;
	n34=-(c5.*n25) + no34 - n15.*s5 - my4.*vp14 + mx4.*vp24;
	fdi34=-(c4.*e24) - e14.*s4;
	e33=f33 + fdi312;
	e32=f32 + fdi38;
	e31=f31 + fdi34;
	gamint1=e31 + fv1.*qd1 + ia1.*qdd1 + fs1.*sign(qd1);
	gamint2=e32 + fv2.*qd2 + ia2.*qdd2 + fs2.*sign(qd2);
	gamint3=e33 + fv3.*qd3 + ia3.*qdd3 + fs3.*sign(qd3);
	gamint4=n34 + fv4.*qd4 + fs4.*sign(qd4);
	gamint5=n35 + fv5.*qd5 + fs5.*sign(qd5);
	gamint6=n36 + fv6.*qd6 + fs6.*sign(qd6);
	gamint7=n37 + fv7.*qd7 + fs7.*sign(qd7);
	gamint8=n38 + fv8.*qd8 + fs8.*sign(qd8);
	gamint9=n39 + fv9.*qd9 + fs9.*sign(qd9);
	gamint10=n310 + fv10.*qd10 + fs10.*sign(qd10);
	gamint11=n311 + fv11.*qd11 + fs11.*sign(qd11);
	gamint12=n312 + fv12.*qd12 + fs12.*sign(qd12);
	gamint13=n313 + fv13.*qd13 + fs13.*sign(qd13);
	gamint14=n314 + fv14.*qd14 + fs14.*sign(qd14);
	gamint15=n315 + fv15.*qd15 + fs15.*sign(qd15);
    
    % Adding the effect of the platform
	fxm16=m16.*posddx;
	fym16=m16.*(posddy - g2.*un);
	fzm16=m16.*posddz;
    
    % Adding the effect of the payload
	fxm17=m17.*posddx;
	fym17=m17.*(posddy - g2.*un);
	fzm17=m17.*posddz;
    
    %% Inverse dynamic model of the real parallel robot
% matrix linking the efforts of the virtual chain 1 to the real input efforts
	jfc112=(jc123.*j11-j21.*jc113)./dejc1;
	jfc113=(jc112.*j21)./dejc1;
	jfc122=(jc123.*j12-j22.*jc113)./dejc1;
	jfc123=(jc112.*j22)./dejc1;
	jfc132=(jc123.*j13-j23.*jc113)./dejc1;
	jfc133=(jc112.*j23)./dejc1;

% matrix linking the efforts of the virtual chain 2 to the real input efforts
	jfc212=(jc233.*j21-j31.*jc223)./dejc2;
	jfc213=(jc222.*j31)./dejc2;
	jfc222=(jc233.*j22-j32.*jc223)./dejc2;
	jfc223=(jc222.*j32)./dejc2;
	jfc232=(jc233.*j23-j33.*jc223)./dejc2;
	jfc233=(jc222.*j33)./dejc2;

% matrix linking the efforts of the virtual chain 3 to the real input efforts
	jfc312=(-j11.*jc333+jc313.*j31)./dejc3;
	jfc313=(jc332.*j11)./dejc3;
	jfc322=(-j12.*jc333+jc313.*j32)./dejc3;
	jfc323=(jc332.*j12)./dejc3;
	jfc332=(-j13.*jc333+jc313.*j33)./dejc3;
	jfc333=(jc332.*j13)./dejc3;	
    
% Real input effors
    gam1 = gamint1 + jfc112.*gamint4 + jfc113.*gamint5 - jfc113.*gamint6 - jfc112.*gamint7 + jfc212.*gamint8 + jfc213.*gamint9 -...
        jfc213.*gamint10 - jfc212.*gamint11 + jfc312.*gamint12 + jfc313.*gamint13 - jfc313.*gamint14 - jfc312.*gamint15 + j11.*(fxm16+fxm17) + j21.*(fym16+fym17) + j31.*(fzm16+fzm17);
    
    gam2 = gamint2 + jfc122.*gamint4 + jfc123.*gamint5 - jfc123.*gamint6 - jfc122.*gamint7 + jfc222.*gamint8 + jfc223.*gamint9 -...
        jfc223.*gamint10 - jfc222.*gamint11 + jfc322.*gamint12 + jfc323.*gamint13 - jfc323.*gamint14 - jfc322.*gamint15 + j12.*(fxm16+fxm17) + j22.*(fym16+fym17) + j32.*(fzm16+fzm17);
        
    gam3 = gamint3 + jfc132.*gamint4 + jfc133.*gamint5 - jfc133.*gamint6 - jfc132.*gamint7 + jfc232.*gamint8 + jfc233.*gamint9 -...
        jfc233.*gamint10 - jfc232.*gamint11 + jfc332.*gamint12 + jfc333.*gamint13 - jfc333.*gamint14 - jfc332.*gamint15 + j13.*(fxm16+fxm17) + j23.*(fym16+fym17) + j33.*(fzm16+fzm17);


