MGAircraftTrifilarPendulumDynamicsKaneEmbedded.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGAircraftTrifilarPendulumDynamicsKaneEmbedded.txt
   (2) % Copyright (c) 2016-18 Motion Genesis LLC.
   (3) %--------------------------------------------------------------------
   (4) SetAutoZ( OFF )
   (5) NewtonianFrame  N                    % Earth / aircraft hanger.
   (6) RigidBody       C                    % Aircraft.
   (7) Point           N1(N), N2(N), N3(N)  % End-points of cable on N.
   (8) Point           C1(C), C2(C), C3(C)  % End-points of cable on C.
   (9) %--------------------------------------------------------------------
   (10) Constant  L1 = 30 m                  % Length of cable between N1 and C1.
   (11) Constant  L2 = 30 m                  % Length of cable between N2 and C2.
   (12) Constant  L3 = 30 m                  % Length of cable between N3 and C3.
   (13) Constant  dN = 30 m                  % Distance between No and N1.
   (14) Constant  dC = 30 m                  % Distance between Co and C1.
   (15) Constant  wN = 20 m                  % Distance between No and N2.
   (16) Constant  wC = 20 m                  % Distance between Co and C2.
   (17) Constant dcm =  8 m                  % Distance between Co and Ccm.
   (18) Constant  g = 9.8 m/s^2              % Earth's gravitational acceleration.
   (19) C.SetMassInertia( m = 9000 kg, Ixx = 4.0E5 kg*m^2,  Iyy = 3.0E5 kg*m^2,  Izz = Ixx + Iyy )
-> (20) Izz = Ixx + Iyy

   (21) %--------------------------------------------------------------------
   (22) Constant  bT = 8.0E3 noUnits         % Torque damping constant.
   (23) Constant  bF = 6.0E3 noUnits         % Force  damping constant.
   (24) Constant  epsilonV   = 1.0E-5 m/s    % Small number to avoid divide by zero errors.
   (25) Constant  epsilonW   = 1.0E-5 rad/s  % Small number to avoid divide by zero errors.
   (26) Constant  expDamping = 0.25 noUnits  % Exponent used with damping force/torques.
   (27) %--------------------------------------------------------------------
   (28) Variable  x'',  y'',  z''            % Locates Ccm from No.
   (29) Variable  q1',  q2',  q3'            % BodyZYX Euler angles.
   (30) Variable  wx',  wy',  wz'            % Cxyz> measures of C's angular velocity in N.
   (31) SetGeneralizedSpeeds(  x',  y',  wz  )
   (32) %--------------------------------------------------------------------
   (33) %   Rotational kinematics.
   (34) C.SetAngularVelocityAcceleration(  N,  wx*Cx> + wy*Cy> + wz*Cz>  )
-> (35) w_C_N> = wx*Cx> + wy*Cy> + wz*Cz>
-> (36) alf_C_N> = wx'*Cx> + wy'*Cy> + wz'*Cz>

   (37) C.SetRotationMatrixODE( N, BodyZYX, q1, q2, q3 )
-> (38) C_N[1,1] = cos(q1)*cos(q2)
-> (39) C_N[1,2] = sin(q1)*cos(q2)
-> (40) C_N[1,3] = -sin(q2)
-> (41) C_N[2,1] = sin(q2)*sin(q3)*cos(q1) - sin(q1)*cos(q3)
-> (42) C_N[2,2] = cos(q1)*cos(q3) + sin(q1)*sin(q2)*sin(q3)
-> (43) C_N[2,3] = sin(q3)*cos(q2)
-> (44) C_N[3,1] = sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)
-> (45) C_N[3,2] = sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)
-> (46) C_N[3,3] = cos(q2)*cos(q3)
-> (47) q1' = (wy*sin(q3)+cos(q3)*wz)/cos(q2)
-> (48) q2' = wy*cos(q3) - sin(q3)*wz
-> (49) q3' = wx + tan(q2)*(wy*sin(q3)+cos(q3)*wz)

   (50) %--------------------------------------------------------------------
   (51) %   Translational kinematics.
   (52) CCm.Translate( No, x*Nx> + y*Ny> + z*Nz> )
-> (53) p_No_Ccm> = x*Nx> + y*Ny> + z*Nz>
-> (54) v_Ccm_N> = x'*Nx> + y'*Ny> + z'*Nz>
-> (55) a_Ccm_N> = x''*Nx> + y''*Ny> + z''*Nz>

   (56) Co.SetPosition( CCm, -dcm*Cx> )
-> (57) p_Ccm_Co> = -dcm*Cx>

   (58) C1.SetPosition( Co,    dC*Cx> )
-> (59) p_Co_C1> = dC*Cx>

   (60) C2.SetPosition( Co,   -wC*Cy> )
-> (61) p_Co_C2> = -wC*Cy>

   (62) C3.SetPosition( Co,    wC*Cy> )
-> (63) p_Co_C3> = wC*Cy>

   (64) N1.SetPosition( No,    dN*Nx> )
-> (65) p_No_N1> = dN*Nx>

   (66) N2.SetPosition( No,   -wN*Ny> )
-> (67) p_No_N2> = -wN*Ny>

   (68) N3.SetPosition( No,    wN*Ny> )
-> (69) p_No_N3> = wN*Ny>

   (70) %--------------------------------------------------------------------
   (71) %   Configuration constraints: Length of cables.
   (72) CableConstraint[1] = C1.GetDistanceSquared( N1 ) - L1^2
-> (73) CableConstraint[1] = (dC-dcm)^2 + y^2 + z^2 + (dN-x)^2 + 2*(dC-dcm)*y*
        sin(q1)*cos(q2) - L1^2 - 2*(dC-dcm)*z*sin(q2) - 2*(dC-dcm)*cos(q1)*cos(
        q2)*(dN-x)

   (74) CableConstraint[2] = C2.GetDistanceSquared( N2 ) - L2^2
-> (75) CableConstraint[2] = dcm^2 + wC^2 + x^2 + z^2 + (wN+y)^2 + 2*dcm*z*sin(q2)
        + 2*wC*x*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)) - L2^2 - 2*dcm*x*cos
        (q1)*cos(q2) - 2*wC*z*sin(q3)*cos(q2) - 2*dcm*sin(q1)*cos(q2)*(wN+y)
        - 2*wC*(wN+y)*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))

   (76) CableConstraint[3] = C3.GetDistanceSquared( N3 ) - L3^2
-> (77) CableConstraint[3] = dcm^2 + wC^2 + x^2 + z^2 + (wN-y)^2 + 2*dcm*z*sin(q2)
        + 2*wC*z*sin(q3)*cos(q2) + 2*dcm*sin(q1)*cos(q2)*(wN-y) - L3^2 - 2*dcm*
        x*cos(q1)*cos(q2) - 2*wC*x*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))
        - 2*wC*(wN-y)*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))

   (78) %--------------------------------------------------------------------
   (79) %   Embed constraints so tensions do not appear in statics equations.
   (80) %   Need to identify useful subset of generalized speed.  What worked
   (81) %   (from looking at system and using physical insight) was:  x', y', wz
   (82) %   Hence, need to solve  z', wx, wy  in terms of  x', y', wz.
   (83) SolveDt(  Dt(CableConstraint) = 0,   z',  wx,  wy )
-> (84) z' = -((dC-dcm)*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
        cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*
        cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(
        dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(x*x'+(wN+y)*y'+dcm*x*sin(q1)*cos
        (q3)*wz+wC*x*cos(q1)*cos(q2)*wz+wC*sin(q1)*cos(q2)*(wN+y)*wz+wC*(sin(
        q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'-wC*z*sin(q2)*wz-dcm*sin(q1)*cos
        (q2)*y'-dcm*cos(q1)*cos(q2)*x'-dcm*cos(q1)*cos(q3)*(wN+y)*wz-wC*(cos(
        q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y'-dcm*sin(q3)*(z*cos(q2)+x*sin(
        q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz)+(dC-dcm)*(z*cos(q2)*cos(q3)+x*(
        sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
        q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
        sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*
        (x*x'+wC*z*sin(q2)*wz+dcm*x*sin(q1)*cos(q3)*wz+dcm*cos(q1)*cos(q3)*(wN-
        y)*wz+wC*sin(q1)*cos(q2)*(wN-y)*wz+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*
        sin(q3))*y'-(wN-y)*y'-dcm*sin(q1)*cos(q2)*y'-dcm*cos(q1)*cos(q2)*x'-wC*
        x*cos(q1)*cos(q2)*wz-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'-
        dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz)+
        dcm*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
        wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*
        cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
        q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
        q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*
        (sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
        cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(y*y'+(dC-dcm)*sin(q1)
        *cos(q2)*y'+(dC-dcm)*cos(q1)*cos(q2)*x'+(dC-dcm)*y*cos(q1)*cos(q3)*wz+(
        dC-dcm)*z*sin(q3)*cos(q2)*wz+(dC-dcm)*y*sin(q1)*sin(q2)*sin(q3)*wz+(dC-
        dcm)*sin(q1)*cos(q3)*(dN-x)*wz-(dN-x)*x'-(dC-dcm)*sin(q2)*sin(q3)*cos(
        q1)*(dN-x)*wz))/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
        *sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
        q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
        cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
        *(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
        q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
        cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
        q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*
        cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos
        (q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(
        q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
        q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*
        cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)
        -sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))

-> (85) wx = -(dcm*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos
        (q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
        sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos
        (q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
        q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(y*y'+(dC-dcm)*sin(q1)*cos(q2)*y'+
        (dC-dcm)*cos(q1)*cos(q2)*x'+(dC-dcm)*y*cos(q1)*cos(q3)*wz+(dC-dcm)*z*
        sin(q3)*cos(q2)*wz+(dC-dcm)*y*sin(q1)*sin(q2)*sin(q3)*wz+(dC-dcm)*sin(
        q1)*cos(q3)*(dN-x)*wz-(dN-x)*x'-(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*(dN-x)
        *wz)-(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
        sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*
        (wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
        y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
        q2)*cos(q1)*cos(q3)*(dN-x)))*(x*x'+(wN+y)*y'+dcm*x*sin(q1)*cos(q3)*wz+
        wC*x*cos(q1)*cos(q2)*wz+wC*sin(q1)*cos(q2)*(wN+y)*wz+wC*(sin(q1)*cos(
        q3)-sin(q2)*sin(q3)*cos(q1))*x'-wC*z*sin(q2)*wz-dcm*sin(q1)*cos(q2)*y'-
        dcm*cos(q1)*cos(q2)*x'-dcm*cos(q1)*cos(q3)*(wN+y)*wz-wC*(cos(q1)*cos(
        q3)+sin(q1)*sin(q2)*sin(q3))*y'-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(
        q1)+sin(q1)*sin(q2)*(wN+y))*wz)-(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(
        q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
        q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*
        cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)
        -sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))*(x*x'+wC*z*sin
        (q2)*wz+dcm*x*sin(q1)*cos(q3)*wz+dcm*cos(q1)*cos(q3)*(wN-y)*wz+wC*sin(
        q1)*cos(q2)*(wN-y)*wz+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y'-(
        wN-y)*y'-dcm*sin(q1)*cos(q2)*y'-dcm*cos(q1)*cos(q2)*x'-wC*x*cos(q1)*cos
        (q2)*wz-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'-dcm*sin(q3)*(z*
        cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz))/(wC*(dcm*(z-(dC-
        dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
        (q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(
        q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
        sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(
        q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
        wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*
        sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
        q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
        q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*
        (dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
        cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))
        -(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(
        q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
        cos(q1)*cos(q3)*(dN-x)))))

-> (86) wy = -((z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
        q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*
        (x*x'+(wN+y)*y'+dcm*x*sin(q1)*cos(q3)*wz+wC*x*cos(q1)*cos(q2)*wz+wC*sin
        (q1)*cos(q2)*(wN+y)*wz+wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'-
        wC*z*sin(q2)*wz-dcm*sin(q1)*cos(q2)*y'-dcm*cos(q1)*cos(q2)*x'-dcm*cos(
        q1)*cos(q3)*(wN+y)*wz-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y'-
        dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz)+(z
        -(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
        q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*x'+
        wC*z*sin(q2)*wz+dcm*x*sin(q1)*cos(q3)*wz+dcm*cos(q1)*cos(q3)*(wN-y)*wz+
        wC*sin(q1)*cos(q2)*(wN-y)*wz+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(
        q3))*y'-(wN-y)*y'-dcm*sin(q1)*cos(q2)*y'-dcm*cos(q1)*cos(q2)*x'-wC*x*
        cos(q1)*cos(q2)*wz-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'-dcm*
        sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz)-((z+
        dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
        sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
        q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
        sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
        q2)*cos(q3))))*(y*y'+(dC-dcm)*sin(q1)*cos(q2)*y'+(dC-dcm)*cos(q1)*cos(
        q2)*x'+(dC-dcm)*y*cos(q1)*cos(q3)*wz+(dC-dcm)*z*sin(q3)*cos(q2)*wz+(dC-
        dcm)*y*sin(q1)*sin(q2)*sin(q3)*wz+(dC-dcm)*sin(q1)*cos(q3)*(dN-x)*wz-(
        dN-x)*x'-(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*(dN-x)*wz))/(dcm*(z-(dC-dcm)*
        sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
        -(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z
        *cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
        q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
        *(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*
        (sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)
        *sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
        q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
        q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-
        (dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
        cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
        dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
        sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
        q1)*cos(q3)*(dN-x))))

-> (87) z'' = -(dcm*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
        q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(
        q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
        sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
        sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
        q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin
        (q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(x'^2+y'^2+(dC-
        dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-
        dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-
        dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos
        (q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*
        y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*
        q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin
        (q3)*(dN-x)*wz*q1'+y*y''+(dC-dcm)*sin(q1)*cos(q2)*y''+(dC-dcm)*cos(q1)*
        cos(q2)*x''+(dC-dcm)*y*cos(q1)*cos(q3)*wz'+(dC-dcm)*z*sin(q3)*cos(q2)*
        wz'+(dC-dcm)*y*sin(q1)*sin(q2)*sin(q3)*wz'+(dC-dcm)*sin(q1)*cos(q3)*(
        dN-x)*wz'-(dC-dcm)*sin(q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*
        q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(
        dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-
        dcm)*z*sin(q2)*sin(q3)*wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(
        dC-dcm)*sin(q2)*cos(q1)*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*
        cos(q2)*(dN-x)*wz*q2'-(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*
        cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(
        q3)*q1'+y*sin(q1)*cos(q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin
        (q1)*sin(q2)*cos(q3)*(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(
        q3)*cos(q1)*y'-y*cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*
        cos(q2)*q3'-y*sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-
        sin(q3)*cos(q1)*(dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2')-(dN-x)*
        x''-(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*(dN-x)*wz')+(dC-dcm)*(z*cos(q2)*
        cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos
        (q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
        (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(
        dN-x))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+
        wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)
        *x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin
        (q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)
        *cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-
        wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(
        q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)
        *cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*
        sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)
        *q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(q1)*sin(
        q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(q1)*cos(
        q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')+x*x''+(wN+y)*y''+dcm*x*sin(
        q1)*cos(q3)*wz'+wC*x*cos(q1)*cos(q2)*wz'+wC*sin(q1)*cos(q2)*(wN+y)*wz'+
        wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x''-dcm*cos(q1)*cos(q3)*
        wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*q1'-
        dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*
        cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(q2)+
        x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*sin(
        q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)-sin
        (q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*
        (sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(
        q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(cos(
        q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)
        *cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(
        q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+
        y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(
        q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(
        q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*
        q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*
        cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)*
        (wN+y)*q1')-wC*z*sin(q2)*wz'-dcm*sin(q1)*cos(q2)*y''-dcm*cos(q1)*cos(
        q2)*x''-dcm*cos(q1)*cos(q3)*(wN+y)*wz'-wC*(cos(q1)*cos(q3)+sin(q1)*sin(
        q2)*sin(q3))*y''-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(
        q2)*(wN+y))*wz')+(dC-dcm)*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
        *cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*
        cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin
        (q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(x'^2+y'^2+dcm*sin(q1)*cos(
        q3)*wz*x'+wC*sin(q2)*wz*z'+wC*z*cos(q2)*wz*q2'+dcm*sin(q1)*sin(q2)*y'*
        q2'+dcm*sin(q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)
        *cos(q3)*wz*q1'+wC*x*sin(q1)*cos(q2)*wz*q1'+wC*x*sin(q2)*cos(q1)*wz*q2'
        +wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'+dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*
        sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin(
        q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+cos
        (q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(
        q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*
        cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-
        sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1')+
        x*x''+wC*z*sin(q2)*wz'+dcm*x*sin(q1)*cos(q3)*wz'+dcm*cos(q1)*cos(q3)*(
        wN-y)*wz'+wC*sin(q1)*cos(q2)*(wN-y)*wz'+wC*(cos(q1)*cos(q3)+sin(q1)*sin
        (q2)*sin(q3))*y''-dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q1)*cos(q2)*wz*y'-
        wC*cos(q1)*cos(q2)*wz*x'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(
        q3)*wz*q3'-dcm*sin(q1)*cos(q3)*(wN-y)*wz*q1'-dcm*sin(q3)*cos(q1)*(wN-y)
        *wz*q3'-wC*sin(q1)*sin(q2)*(wN-y)*wz*q2'-z'*(wC*sin(q2)*sin(q3)*q2'-z'-
        dcm*cos(q2)*q2'-wC*cos(q2)*cos(q3)*q3')-dcm*cos(q3)*(z*cos(q2)+x*sin(
        q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz*q3'-wC*x'*(cos(q1)*cos(q3)*q1'+
        sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)
        *q3'-sin(q3)*cos(q1)*cos(q2)*q2')-wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*
        cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin
        (q2)*sin(q3)*cos(q1)*q1')-dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*
        cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin
        (q2)*q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')-wC*wx*(
        (sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*
        sin(q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(
        q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)
        *q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(
        wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*
        q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-(wN-y)*
        y''-dcm*sin(q1)*cos(q2)*y''-dcm*cos(q1)*cos(q2)*x''-wC*x*cos(q1)*cos(
        q2)*wz'-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x''-dcm*sin(q3)*(z
        *cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz'))/(dcm*(z-(dC-
        dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
        (q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(
        q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
        sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(
        q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
        wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*
        sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
        q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
        q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*
        (dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
        cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))
        -(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(
        q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
        cos(q1)*cos(q3)*(dN-x))))

-> (88) wx' = -(dcm*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*
        cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
        q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)
        *cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
        cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*
        cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*
        sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*
        cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*
        wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin
        (q3)*cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*
        cos(q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*
        wz*q1'+y*y''+(dC-dcm)*sin(q1)*cos(q2)*y''+(dC-dcm)*cos(q1)*cos(q2)*x''+
        (dC-dcm)*y*cos(q1)*cos(q3)*wz'+(dC-dcm)*z*sin(q3)*cos(q2)*wz'+(dC-dcm)*
        y*sin(q1)*sin(q2)*sin(q3)*wz'+(dC-dcm)*sin(q1)*cos(q3)*(dN-x)*wz'-(dC-
        dcm)*sin(q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin
        (q1)*cos(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*
        cos(q3)*wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin
        (q3)*wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos
        (q1)*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*
        q2'-(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*
        cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*
        cos(q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(
        q3)*(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*
        cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(
        q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-
        x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2')-(dN-x)*x''-(dC-dcm)*sin(q2)*
        sin(q3)*cos(q1)*(dN-x)*wz')-(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(
        q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
        sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(
        q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin
        (q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))*(x'^2+y'^2+dcm*sin
        (q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+
        dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(
        q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin(q1)*cos(q3)*(wN+y)*wz*
        q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'+
        z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-wC*cos(q2)*cos(q3)*q3')+
        wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*
        q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin
        (q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(
        q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*
        sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*
        x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*
        cos(q1)*(wN+y)*q1')+x*x''+(wN+y)*y''+dcm*x*sin(q1)*cos(q3)*wz'+wC*x*cos
        (q1)*cos(q2)*wz'+wC*sin(q1)*cos(q2)*(wN+y)*wz'+wC*(sin(q1)*cos(q3)-sin(
        q2)*sin(q3)*cos(q1))*x''-dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*
        z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*
        q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*
        sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*
        sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
        *x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*
        sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*
        q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'
        -cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(
        q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)
        *cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'
        +z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos
        (q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*
        q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(
        q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-
        sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(
        q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1')-wC*z*sin(q2)
        *wz'-dcm*sin(q1)*cos(q2)*y''-dcm*cos(q1)*cos(q2)*x''-dcm*cos(q1)*cos(
        q3)*(wN+y)*wz'-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y''-dcm*sin
        (q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz')-(dcm*(z-
        (dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
        cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
        dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
        sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
        q1)*cos(q3)*(dN-x)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q2)*
        wz*z'+wC*z*cos(q2)*wz*q2'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(
        q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+wC*x
        *sin(q1)*cos(q2)*wz*q1'+wC*x*sin(q2)*cos(q1)*wz*q2'+wC*cos(q1)*cos(q2)*
        (wN-y)*wz*q1'+dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin
        (q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(
        q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*
        q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos
        (q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*
        sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*
        (wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1')+x*x''+wC*z*sin(q2)*wz'+
        dcm*x*sin(q1)*cos(q3)*wz'+dcm*cos(q1)*cos(q3)*(wN-y)*wz'+wC*sin(q1)*cos
        (q2)*(wN-y)*wz'+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y''-dcm*
        cos(q1)*cos(q3)*wz*y'-wC*sin(q1)*cos(q2)*wz*y'-wC*cos(q1)*cos(q2)*wz*
        x'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-dcm*sin(q1)*
        cos(q3)*(wN-y)*wz*q1'-dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'-wC*sin(q1)*sin(
        q2)*(wN-y)*wz*q2'-z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(
        q2)*cos(q3)*q3')-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(
        q2)*(wN-y))*wz*q3'-wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*
        q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos
        (q2)*q2')-wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(
        q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')
        -dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos
        (q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(
        wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')-wC*wx*((sin(q3)*cos(q1)-sin(q1)*
        sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin(
        q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'
        -sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+sin(
        q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+
        sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)
        *q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-(wN-y)*y''-dcm*sin(q1)*cos(q2)*y''-
        dcm*cos(q1)*cos(q2)*x''-wC*x*cos(q1)*cos(q2)*wz'-wC*(sin(q1)*cos(q3)-
        sin(q2)*sin(q3)*cos(q1))*x''-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-
        sin(q1)*sin(q2)*(wN-y))*wz'))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*
        cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos
        (q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
        sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*
        (wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
        x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin
        (q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
        sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-
        (z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
        sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(
        sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
        cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)
        -wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin
        (q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))))

-> (89) wy' = (((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(
        q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
        sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)
        +x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-
        sin(q1)*sin(q2)*cos(q3))))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(
        dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*
        wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+
        z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)
        *y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*
        wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(
        q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'+y*y''+
        (dC-dcm)*sin(q1)*cos(q2)*y''+(dC-dcm)*cos(q1)*cos(q2)*x''+(dC-dcm)*y*
        cos(q1)*cos(q3)*wz'+(dC-dcm)*z*sin(q3)*cos(q2)*wz'+(dC-dcm)*y*sin(q1)*
        sin(q2)*sin(q3)*wz'+(dC-dcm)*sin(q1)*cos(q3)*(dN-x)*wz'-(dC-dcm)*sin(
        q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(
        q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*
        wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*
        q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos
        (q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-
        dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*
        cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*
        cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)
        *q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*
        cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(
        q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-
        cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2')-(dN-x)*x''-(dC-dcm)*sin(q2)*sin(q3)
        *cos(q1)*(dN-x)*wz')-(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
        *sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
        q2)*cos(q3)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*
        wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*
        cos(q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+
        dcm*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*
        cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)
        *q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)
        *sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*
        cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin
        (q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*
        cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(
        q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(
        q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')+x*x''+(wN+y)*y''+dcm
        *x*sin(q1)*cos(q3)*wz'+wC*x*cos(q1)*cos(q2)*wz'+wC*sin(q1)*cos(q2)*(wN+
        y)*wz'+wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x''-dcm*cos(q1)*cos
        (q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*
        q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(
        q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(
        q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*
        sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)
        -sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*
        q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*
        cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(
        cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin
        (q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*
        cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*
        (wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+
        sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*
        cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(
        q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(
        q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(
        q3)*(wN+y)*q1')-wC*z*sin(q2)*wz'-dcm*sin(q1)*cos(q2)*y''-dcm*cos(q1)*
        cos(q2)*x''-dcm*cos(q1)*cos(q3)*(wN+y)*wz'-wC*(cos(q1)*cos(q3)+sin(q1)*
        sin(q2)*sin(q3))*y''-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*
        sin(q2)*(wN+y))*wz')-(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
        *sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
        q2)*cos(q3)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q2)*wz*z'+wC*
        z*cos(q2)*wz*q2'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*q1'+
        dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+wC*x*sin(q1)*
        cos(q2)*wz*q1'+wC*x*sin(q2)*cos(q1)*wz*q2'+wC*cos(q1)*cos(q2)*(wN-y)*
        wz*q1'+dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*
        cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*
        cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+
        sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)
        *q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(
        q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-
        y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1')+x*x''+wC*z*sin(q2)*wz'+dcm*x
        *sin(q1)*cos(q3)*wz'+dcm*cos(q1)*cos(q3)*(wN-y)*wz'+wC*sin(q1)*cos(q2)*
        (wN-y)*wz'+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y''-dcm*cos(q1)
        *cos(q3)*wz*y'-wC*sin(q1)*cos(q2)*wz*y'-wC*cos(q1)*cos(q2)*wz*x'-dcm*
        cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-dcm*sin(q1)*cos(q3)
        *(wN-y)*wz*q1'-dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'-wC*sin(q1)*sin(q2)*(
        wN-y)*wz*q2'-z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(q2)*
        cos(q3)*q3')-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(
        wN-y))*wz*q3'-wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-
        sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)
        *q2')-wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*
        cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')-
        dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos(
        q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(
        wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')-wC*wx*((sin(q3)*cos(q1)-sin(q1)*
        sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin(
        q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'
        -sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+sin(
        q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+
        sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)
        *q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-(wN-y)*y''-dcm*sin(q1)*cos(q2)*y''-
        dcm*cos(q1)*cos(q2)*x''-wC*x*cos(q1)*cos(q2)*wz'-wC*(sin(q1)*cos(q3)-
        sin(q2)*sin(q3)*cos(q1))*x''-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-
        sin(q1)*sin(q2)*(wN-y))*wz'))/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(
        q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)
        -sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(
        q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-
        y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(
        sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
        q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
        sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-
        (z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
        sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(
        sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
        cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)
        -wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin
        (q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))

   (90) %--------------------------------------------------------------------
   (91) %   Add forces from gravity.  It is unnecessary to add tension because tension does
   (92) %   not contribute to generalized forces (force is perpendicular to velocity).
   (93) Ccm.AddForce(  m*g*Nz>  )
-> (94) Force_Ccm> = m*g*Nz>

   (95) %--------------------------------------------------------------------
   (96) %   Add force to damp translational motion.
   (97) vMag = Ccm.GetSpeed( N )
-> (98) vMag = sqrt(x'^2+y'^2+z'^2)

   (99) unitVectorVelocity> = CCm.GetVelocity( N ) / ( vMag + epsilonV )
-> (100) unitVectorVelocity> = x'/(epsilonV+vMag)*Nx> + y'/(epsilonV+vMag)*Ny>
         + z'/(epsilonV+vMag)*Nz>

   (101) dampingForce> = -bF *  vMag^expDamping * unitVectorVelocity>
-> (102) dampingForce> = -bF*x'*vMag^expDamping/(epsilonV+vMag)*Nx> - bF*y'*vMag
         ^expDamping/(epsilonV+vMag)*Ny> - bF*z'*vMag^expDamping/(epsilonV+vMag)*Nz>

   (103) CCm.AddForce( dampingForce> )
-> (104) Force_Ccm> = -bF*x'*vMag^expDamping/(epsilonV+vMag)*Nx> - bF*y'*vMag^expDamping
         /(epsilonV+vMag)*Ny> + (m*g-bF*z'*vMag^expDamping/(epsilonV+vMag))*Nz>

   (105) %--------------------------------------------------------------------
   (106) %   Add torque to damp rotational motion.
   (107) wMag = C.GetAngularSpeed( N )
-> (108) wMag = sqrt(wx^2+wy^2+wz^2)

   (109) unitVectorAngularVelocity> = C.GetAngularVelocity( N ) / ( wMag + epsilonW )
-> (110) unitVectorAngularVelocity> = wx/(epsilonW+wMag)*Cx> + wy/(epsilonW+wM
         ag)*Cy> + wz/(epsilonW+wMag)*Cz>

   (111) dampingTorque> =  -bT *  wMag^expDamping * unitVectorAngularVelocity>
-> (112) dampingTorque> = -bT*wx*wMag^expDamping/(epsilonW+wMag)*Cx> - bT*wy*
         wMag^expDamping/(epsilonW+wMag)*Cy> - bT*wz*wMag^expDamping/(epsilonW+
         wMag)*Cz>

   (113) C.AddTorque( dampingTorque> )
-> (114) Torque_C> = -bT*wx*wMag^expDamping/(epsilonW+wMag)*Cx> - bT*wy*wMag^expDamping
         /(epsilonW+wMag)*Cy> - bT*wz*wMag^expDamping/(epsilonW+wMag)*Cz>

   (115) %--------------------------------------------------------------------
   (116) %   Form dynamic equations with Kane's method.
   (117) Dynamics = System.GetDynamicsKane()
-> (118) Dynamics[1] = bT*(wy*((z-(dC-dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-wC*(
         sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(
         q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(sin(
         q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))-(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(
         q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
         (q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)
         -wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
         (q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*wMag
         ^expDamping/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
         cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
         *(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
         q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
         cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
         q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*
         cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
         cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin
         (q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
         cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))-wx*((
         dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(
         dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(
         q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(
         wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
         y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin
         (q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x
         *sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
         q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)
         -x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)
         *sin(q2)*cos(q3)*(wN+y)))-(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-
         sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(
         wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
         sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos
         (q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
         sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*wMag^expDamping
         /(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
         sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm
         *sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
         (q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
         q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+
         x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-
         sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(
         wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
         sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos
         (q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
         sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))))/(epsilonW+
         wMag) + ((dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*
         cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(
         q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*
         cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)
         *cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(
         dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(
         q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*
         cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
         q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(
         q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
         q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(x-dcm*cos(q1)*cos(q2)-wC*(sin(
         q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(
         q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(
         q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-
         wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin
         (q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*
         ((Iyy-Izz)*wy*wz-Ixx*((dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*
         cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
         q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
         *(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
         q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))*(x'^2+y'^2+dcm*sin
         (q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+
         dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(
         q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin(q1)*cos(q3)*(wN+y)*wz*
         q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'
         +z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-wC*cos(q2)*cos(q3)*q3')
         +wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(
         q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*
         y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*
         q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(
         q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)
         *cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*
         q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*
         wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin
         (q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-
         wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(
         q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos
         (q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
         cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin
         (q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(
         q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*
         q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*
         cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z
         *sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+
         x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*
         sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*
         y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'
         -x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*
         cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)
         *(wN+y)*q1'))-dcm*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(
         q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
         -sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(
         sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
         cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(x'^2+y'^2+(dC-dcm)*
         cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*
         sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*
         cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)
         *cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*
         sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*
         q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*
         sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*
         sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(
         q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-(dC-dcm)*y*sin(q3)*cos(
         q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*
         (dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*
         sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+
         sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*
         z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos(q3)*q2'+y*sin(q2)*cos(
         q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*q1'+sin(q2)*sin(q3)*cos
         (q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*cos(q3)*q3'-z*sin(q2)*cos
         (q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*
         cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*
         (dN-x)*q2'))-(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
         q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
         q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos
         (q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
         q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))*(dcm*cos(q1)*cos(q3)*wz*
         y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*cos(q1)*cos(
         q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(q1)*cos(q3)*(wN-y)*wz*
         q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)*sin(q2)*(wN-y)*wz*q2'
         +z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(q2)*cos(q3)*q3')
         +dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz*
         q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin
         (q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*
         y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*
         q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(
         q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos
         (q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*
         q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin(q1)
         *sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-
         sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'
         +sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(
         q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2-dcm*sin(q1)*cos(q3)*
         wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*sin(q1)*sin(q2)*y'*q2'-
         dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*x'*q2'-dcm*x*cos(q1)*
         cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'
         -wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*
         sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin
         (q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+
         cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*
         cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(
         q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*
         q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*
         q1')))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin
         (q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
         cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(
         q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z
         +dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
         +sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
         sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(
         q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
         q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)
         *cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
         q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))))/(wC*(
         dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
         *cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x
         *sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
         cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)
         +wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
         (q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos
         (q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
         q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(
         q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*
         sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
         sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z
         *cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))) + bF*x'*vMag^expDamping
         /(epsilonV+vMag) + (Iyy*((z-(dC-dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-
         wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(
         sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(
         q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))-(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*
         sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
         *cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin
         (q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
         *cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))^2
         /(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(
         q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin
         (q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
         *cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z
         *cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)
         -x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)
         *sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
         z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2+Ixx*((dcm*cos(q1)*
         cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-
         dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
         cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
         *(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(
         q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x)))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)-
         wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos
         (q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z
         +dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin
         (q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
         q3)*(wN+y)))-(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(
         q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
         q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
         q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos
         (q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
         q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2/(wC^2*(dcm*(z-(dC-dcm)*
         sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         -(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+
         z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin
         (q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(
         q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
         wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*
         sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
         q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z
         *cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(
         wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
         y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin
         (q2)*cos(q1)*cos(q3)*(dN-x))))^2)+m*(1+((dC-dcm)*(x-dcm*cos(q1)*cos(
         q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x
         *(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin
         (q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
         sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))
         -(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*
         cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
         q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(
         q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
         -sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*(
         (z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*
         (sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(
         q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
         sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
         sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
         cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))^2/(dcm*(z-(dC-dcm)*
         sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         -(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+
         z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin
         (q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(
         q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
         wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*
         sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
         q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z
         *cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(
         wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
         y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin
         (q2)*cos(q1)*cos(q3)*(dN-x))))^2))*x'' + (m*((dC-dcm)*(x-dcm*cos(q1)*
         cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))-(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*
         sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
         q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(
         q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
         q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(dN-x-(dC-dcm)*cos(q1)*
         cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
         q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(
         q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
         -sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
         +sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*
         sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))*(dcm*((z*cos
         (q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos
         (q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*
         cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*
         cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
         cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*
         cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*
         sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*
         cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)
         *wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*
         sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-
         dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(
         dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*
         y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'
         -(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(
         dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*
         q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*
         cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*
         sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin
         (q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(
         q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(
         dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*
         q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(
         q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-
         x)*q2'))+(dC-dcm)*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
         q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(
         q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
         q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(x'^2+y'^2+dcm*sin(q1)*cos(
         q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(
         q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*
         q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm
         *sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(
         z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*
         x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*
         q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(
         sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-
         sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*
         wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos
         (q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-
         sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*
         z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(
         q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*
         sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)
         +sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos(
         q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
         cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin
         (q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(
         q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*
         q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*
         cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z
         *sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+
         x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*
         sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*
         y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'
         -x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*
         cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)
         *(wN+y)*q1'))-(dC-dcm)*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*
         cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(dcm*cos(q1)*cos(q3)*
         wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*cos(q1)*
         cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(q1)*cos(q3)*(wN-y)
         *wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)*sin(q2)*(wN-y)*
         wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(q2)*cos(
         q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-
         y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(
         q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*
         q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*
         cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+
         dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos
         (q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(
         wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin
         (q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*
         q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)
         *q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*
         cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2-dcm*sin(q1)*cos(
         q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*sin(q1)*sin(q2)*y'*
         q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*x'*q2'-dcm*x*cos(
         q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*
         wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(q1)*sin(q3)*x'+sin
         (q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+
         x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*
         q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(
         q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*
         sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-
         y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-
         y)*q1')))+((z-(dC-dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos
         (q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))-(z-(dC-dcm)*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-
         sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         -(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(
         z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*
         cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*((Ixx-Izz)*wx*(
         dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
         *cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x
         *sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
         cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)
         +wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
         (q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos
         (q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
         q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(
         q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*
         sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
         sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z
         *cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*wz-Iyy*((z-(dC-dcm)*
         sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x'^2+y'^2+dcm*sin(
         q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+
         dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(
         q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin(q1)*cos(q3)*(wN+y)*wz*
         q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'
         +z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-wC*cos(q2)*cos(q3)*q3')
         +wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(
         q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*
         y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*
         q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(
         q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)
         *cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*
         q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*
         wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin
         (q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-
         wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(
         q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos
         (q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
         cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin
         (q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(
         q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*
         q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*
         cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z
         *sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+
         x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*
         sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*
         y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'
         -x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*
         cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)
         *(wN+y)*q1'))-((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x
         *(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin
         (q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3))))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)
         *wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)
         *cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)
         *y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'
         +(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*
         cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(
         q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*
         q1'-(dC-dcm)*sin(q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(
         dC-dcm)*sin(q1)*cos(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)
         *y*sin(q1)*cos(q3)*wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z
         *sin(q2)*sin(q3)*wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)
         *sin(q2)*cos(q1)*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(
         q2)*(dN-x)*wz*q2'-(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(
         q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)
         *q1'+y*sin(q1)*cos(q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(
         q1)*sin(q2)*cos(q3)*(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(
         q3)*cos(q1)*y'-y*cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*
         cos(q2)*q3'-y*sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-
         sin(q3)*cos(q1)*(dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))-(z-(
         dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
         *cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*cos(
         q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+
         dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(q1)*
         cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)*sin
         (q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*
         cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*
         sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(
         q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(
         q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(
         q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*
         cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos
         (q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(
         q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin(q3)*cos
         (q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(
         q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin
         (q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin
         (q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(
         cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-
         sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2-
         dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*sin
         (q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*x'*
         q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(
         q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(q1)*
         sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(
         q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*
         cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)
         *(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)
         *q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-sin(
         q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*cos(
         q1)*cos(q3)*(wN-y)*q1'))))+(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
         sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
         *(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
         sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
         *sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))*((dC-dcm)*(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin
         (q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*
         cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(dcm*cos(q1)*
         cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos
         (q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin
         (q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(
         q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*
         sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
         sin(q2)*cos(q3)*(wN+y))))*(m*g-bF*z'*vMag^expDamping/(epsilonV+vMag))-
         (Iyy*((z-(dC-dcm)*sin(q2))*(wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(
         q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(
         q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
         *(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)
         *(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*
         cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*((z-(dC-dcm)*sin(
         q2))*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(
         q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(
         wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*
         (x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(
         z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(dN-x-(dC-dcm)*cos(q1)*cos(
         q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
         *sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*
         (sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))))+Ixx*((wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos
         (q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*
         sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
         wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(
         q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
         cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(y
         +(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(
         q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
         q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*
         cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
         *sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(wN+dcm*sin(
         q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(
         dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
         cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
         dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
         sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
         q1)*cos(q3)*(dN-x))))*((dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(
         q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*
         cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
         q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
         *(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
         q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(dN-x-(dC-dcm)
         *cos(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(
         q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
         -sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(
         sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
         cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(x-dcm*cos(q1)*cos(
         q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin
         (q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*
         sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))/wC^2+m*((dC-dcm)*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*
         cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
         q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
         cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
         q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(
         wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))
         *(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
         *(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(
         q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
         cos(q1)*cos(q3)*(dN-x))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
         +x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
         q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
         q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(
         q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
         q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))*((dC-dcm)*(x-dcm*cos(q1)*cos(q2)
         -wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
         sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))
         -(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*
         cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
         q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(
         q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
         -sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*(
         (z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*
         (sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(
         q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
         sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
         sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
         cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))))*y''-(Iyy*((z-(dC-
         dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(
         q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)
         *sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*
         cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
         q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(dN-x-(dC-dcm)*
         cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*((z-(dC-dcm)*sin(q2))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*x*sin(q1)*cos(q3)+wC*x*
         cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
         (q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
         wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
         (q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin
         (q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
         q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin
         (q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*
         sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)
         *cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
         z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))-m*(dC-dcm)*((dC-dcm)*(x-
         dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)
         *sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
         q1)*cos(q3)*(dN-x))-(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(
         q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
         sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(dN-x-(dC-
         dcm)*cos(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*
         sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
         cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(
         q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))
         *((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
         *(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(
         q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
         cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+
         wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*
         sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
         (q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(
         q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*
         (z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))+dcm*(y*cos(q1)*
         cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(
         dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
         cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
         q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
         q2)*cos(q3)*(wN+y))))-Ixx*((dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-
         sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)
         +z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
         sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos
         (q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
         sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(dN-x-(dC-
         dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*
         sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
         wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(
         q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin
         (q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(x-dcm*cos(q1)*
         cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)
         *sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-
         x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))*(dcm*(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin
         (q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(
         dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(
         q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
         sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*
         cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
         cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*sin(q1)*cos(q3)+wC*x*
         cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
         (q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
         wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
         +x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
         q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
         (q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
         -sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+
         dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(
         q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))*
         (dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
         cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+
         y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*
         sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
         q2)*cos(q1)*cos(q3)*(dN-x))))/wC^2)*wz')/(dcm*(z-(dC-dcm)*sin(q2))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)
         *cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
         q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos
         (q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
         (q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(
         dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
         cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
         dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
         sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
         q1)*cos(q3)*(dN-x))))^2

-> (119) Dynamics[2] = bF*y'*vMag^expDamping/(epsilonV+vMag) + (Iyy*((z-(dC-dcm)
         *sin(q2))*(wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(
         q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)
         *sin(q2))*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(
         q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(y+(dC-dcm)
         *sin(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))^2/(dcm*(z-(dC-dcm)*sin(q2))
         *(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)
         *(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(
         q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
         sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z
         *cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)
         *sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
         q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
         q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z
         -(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)
         *cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(
         dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(
         q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
         cos(q1)*cos(q3)*(dN-x))))^2+Ixx*((wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)
         *cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(
         q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
         q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*
         sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
         q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-
         dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
         x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
         cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(
         q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(
         q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(wN+dcm
         *sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*
         (z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(
         q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-
         (dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(
         q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
         cos(q1)*cos(q3)*(dN-x))))^2/(wC^2*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)
         *cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
         +x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
         q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
         (q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
         sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
         *sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))^2)+m*(1+((dC-dcm)*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*
         cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
         q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
         cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
         q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(
         wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))
         *(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
         *(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(
         q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
         cos(q1)*cos(q3)*(dN-x))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
         +x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
         q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
         q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(
         q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
         q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))^2/(dcm*(z-(dC-dcm)*sin(q2))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)
         *cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
         q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos
         (q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
         (q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(
         dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
         cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
         dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
         sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
         q1)*cos(q3)*(dN-x))))^2))*y'' - bT*(wy*((z-(dC-dcm)*sin(q2))*(wN+y-dcm
         *sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm*sin
         (q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))*((z
         +dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
         +sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
         *sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))))*wMag^expDamping/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
         +x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
         q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
         (q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
         sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
         *sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))-wx*((wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(
         q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*
         cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
         q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
         *(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
         q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(y+(dC-dcm)*
         sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(
         q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
         -sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(
         sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
         cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(wN+dcm*sin(q1)*cos(
         q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*
         sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
         *sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))*wMag^expDamping/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)
         *cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
         +x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
         q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
         (q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
         sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
         *sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))))/(epsilonW+wMag) - ((wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(
         q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin
         (q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
         q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*
         sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
         q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-
         dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
         x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
         cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(
         q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(
         q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(wN+dcm
         *sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*
         (z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(
         q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-
         (dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(
         q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
         cos(q1)*cos(q3)*(dN-x))))*((Iyy-Izz)*wy*wz-Ixx*((dcm*(z-(dC-dcm)*sin(
         q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
         sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm
         *sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(
         q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(
         dN-x)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+
         wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(
         q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm
         *sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*
         cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(
         q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin
         (q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(
         q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*
         q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*
         sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*
         q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)
         *q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(
         q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos
         (q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-
         wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(
         q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*
         ((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(
         q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)
         *q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*
         q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*
         sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-
         dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'
         +cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*
         sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)
         *x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(
         q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(
         wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)
         *q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-dcm*((z+dcm*sin(q2)-wC*sin(
         q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
         cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*
         sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-
         z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(
         wN+y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin
         (q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin
         (q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos
         (q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)
         *cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*
         sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+
         (dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(
         q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*
         q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-
         (dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(
         dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)
         *(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)
         *wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos
         (q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos
         (q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*
         q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*
         cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(
         q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-
         cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))-(dcm*(z-(dC-dcm)*sin(q2))*(sin(
         q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(
         q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-
         wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin
         (q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))*(
         dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*
         wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(
         q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)
         *sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-
         wC*cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(
         q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*
         sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*
         cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-
         sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(
         q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*
         x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*
         q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin
         (q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(
         q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)
         *q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*
         q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(
         wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)
         *q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2
         -dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*
         sin(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*
         x'*q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*
         sin(q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(
         q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+
         cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(
         q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin
         (q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos
         (q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-
         sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*
         cos(q1)*cos(q3)*(wN-y)*q1')))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)
         *cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
         +x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
         q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
         (q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
         sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
         *sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*
         cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))
         +(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin
         (q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
         q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(
         sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
         cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(
         q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y
         *sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))))
         - (m*((dC-dcm)*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*
         sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
         q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(
         q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
         q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(wN+y-dcm*sin(q1)*
         cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(
         q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(
         q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*
         sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
         sin(q2)*cos(q3)*(wN+y))))*(dcm*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
         sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(
         q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
         q3)*(wN+y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(
         q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-
         dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-
         dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)
         *sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-
         dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*
         wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)
         *cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(
         q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*
         wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*
         wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)
         *cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-
         (dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos
         (q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos
         (q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)
         *(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*
         cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(
         q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(
         dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))+(dC-dcm)*(z*cos(q2)*cos
         (q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+
         wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(
         q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm
         *sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*
         cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(
         q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin
         (q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(
         q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*
         q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*
         sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*
         q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)
         *q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(
         q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos
         (q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-
         wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(
         q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*
         ((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(
         q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)
         *q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*
         q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*
         sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-
         dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'
         +cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*
         sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)
         *x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(
         q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(
         wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)
         *q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-(dC-dcm)*(z*cos(q2)*cos(q3)+
         x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-
         sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)
         -y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-
         x))*(dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos
         (q2)*wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm
         *sin(q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*
         sin(q1)*sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(
         q2)*q2'-wC*cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(
         q1)-sin(q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*
         sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-
         sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(
         q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(
         q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*
         cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*
         sin(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*
         wx*((sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'
         +z*sin(q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*
         cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*
         cos(q3)*q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(
         q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)
         *sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))
         -x'^2-y'^2-dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*
         q2'-dcm*sin(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*
         cos(q1)*x'*q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*
         q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*
         wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(
         q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+
         x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(
         q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(
         q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(
         q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-
         sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1')))+((z-(dC-dcm)*sin(q2))*(wN+y-dcm*
         sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm*sin
         (q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))*((z
         +dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
         +sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
         *sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))))*((Ixx-Izz)*wx*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos
         (q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
         sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
         *(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
         sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
         *sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))*wz-Iyy*((z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(
         q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(
         q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(
         q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*
         wz*q1'+dcm*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*
         wz*q3'+wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(
         q2)*sin(q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin
         (q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*
         q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*
         cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-
         sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*
         sin(q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)
         *cos(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-
         dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos
         (q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*
         wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-
         dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*
         q3'-wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(
         q3)*z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*
         q2'-z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(
         q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(
         q2)*cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*
         q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*
         cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(
         q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*
         q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-
         sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*
         x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(
         q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*
         cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-((z+dcm*sin(
         q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3))))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(
         q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(
         q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(
         q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*
         cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*
         sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+
         (dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(
         q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*
         q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-
         (dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(
         dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)
         *(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)
         *wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos
         (q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos
         (q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*
         q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*
         cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(
         q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-
         cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))-(z-(dC-dcm)*sin(q2))*(z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(
         q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+
         dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin
         (q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)*sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin
         (q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(q2)*cos(q3)*q3')+dcm*cos(
         q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*
         (cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-
         sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(
         q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(
         q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(
         sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'
         -z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(
         q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)
         *cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*
         cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+sin(q2)*cos(
         q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)
         *sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-
         sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2-dcm*sin(q1)*cos(q3)*wz*x'-wC*
         sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*sin(q1)*sin(q2)*y'*q2'-dcm*sin(
         q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*x'*q2'-dcm*x*cos(q1)*cos(q3)*
         wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*cos(
         q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*
         cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos
         (q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*
         cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*
         y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(
         q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(
         q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1'))))+
         (dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(
         q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin
         (q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
         *cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z
         *cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)
         -x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)
         *sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
         z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*((dC-dcm)*(wN+dcm*sin
         (q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
         (q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x))-(dC-dcm)*(wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)
         +sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
         q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(y+(dC-dcm)*sin
         (q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*
         sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
         wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(
         q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))*(m*g-
         bF*z'*vMag^expDamping/(epsilonV+vMag))+(Iyy*((z-(dC-dcm)*sin(q2))*(wN+
         y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z
         *cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm
         *sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))
         *((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin
         (q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
         cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin
         (q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))))*((z-(dC-dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-wC*(
         sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(
         q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(sin(
         q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))-(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(
         q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
         (q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)
         -wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
         (q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))+Ixx*
         ((wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))
         *(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(
         q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(
         wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
         y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin
         (q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm
         *sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
         sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
         *(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*
         sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
         sin(q2)*cos(q3)*(wN+y)))-(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)
         +sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*
         (wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
         sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos
         (q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
         sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*((dcm*cos(q1)
         *cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-
         dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
         cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
         *(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(
         q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x)))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)-
         wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos
         (q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z
         +dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin
         (q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
         q3)*(wN+y)))-(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(
         q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
         q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
         q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos
         (q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
         q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))/wC^2+m*((dC-dcm)*(wN+dcm*
         sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos
         (q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
         (q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x))-(dC-dcm)*(wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)
         +sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
         q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(y+(dC-dcm)*sin
         (q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*
         sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
         wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(
         q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))*((dC-
         dcm)*(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(
         q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
         wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*
         sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
         q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(
         q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
         cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(
         dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
         sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(
         q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
         q3)*(wN+y)))))*x''-(Iyy*((z-(dC-dcm)*sin(q2))*(wN+y-dcm*sin(q1)*cos(
         q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x
         *(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin
         (q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm*sin(q1)*cos(q2)-y-
         wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)+wC*
         sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
         *cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin
         (q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
         *cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*
         ((z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
         dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-
         wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(
         q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+
         dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(
         q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-
         (dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(
         q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin
         (q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
         *cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z
         +dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
         +sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))))-m*(dC-dcm)*((dC-dcm)*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos
         (q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
         sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(wN+y-
         dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)
         *sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
         q1)*cos(q3)*(dN-x))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
         sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
         *(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(
         wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
         sin(q1)*sin(q2)*cos(q3)*(wN+y))))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
         q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
         cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
         q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(
         q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(
         q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(
         q1)+sin(q1)*sin(q2)*(wN+y)))+(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
         (q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
         q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x
         *sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-
         wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*
         sin(q2)*(wN-y)))+dcm*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*
         sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))
         *((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)
         *(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(
         q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
         sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
         sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
         cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))-Ixx*((wN+y-dcm*sin(
         q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-
         dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
         cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
         *(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(
         q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x)))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*
         sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(
         q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+
         dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(
         q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
         q3)*(wN+y)))-(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin
         (q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin
         (q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
         q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos
         (q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
         q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*(dcm*(dC-dcm)*(y*cos(q1)*
         cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(
         dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(
         q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
         sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+
         wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(
         q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))
         -(dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)
         -wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(
         q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(
         q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
         q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*
         sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
         q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(
         wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(
         q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
         q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(
         q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(
         q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-
         wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin
         (q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))/wC^2)
         *wz')/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
         sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm
         *sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
         (q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
         q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+
         x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-
         sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(
         wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
         sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos
         (q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
         sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2

-> (120) Dynamics[3] = bT*(wz*wMag^expDamping-wy*((z-(dC-dcm)*sin(q2))*(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*x*sin(q1)*cos(q3)+wC*x*cos(
         q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)
         *(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))
         +(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
         wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(
         q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
         q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin
         (q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*
         sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)
         *cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
         z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*wMag^expDamping/(dcm*(z-(
         dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
         *cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)
         *sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
         wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(
         q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
         (q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos
         (q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
         -sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
         +sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(
         q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
         q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
         (q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
         -sin(q2)*cos(q1)*cos(q3)*(dN-x))))-wx*(dcm*(dC-dcm)*(y*cos(q1)*cos(q3)
         +z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-
         sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x
         *sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
         cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(
         q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(
         q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*
         sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*
         sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*
         cos(q1)+sin(q1)*sin(q2)*(wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*
         sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
         wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(
         q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
         cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*
         sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*
         cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*
         cos(q1)-sin(q1)*sin(q2)*(wN-y)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*
         cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
         cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin
         (q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
         cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*wMag^
         expDamping/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
         *sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
         cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
         *(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
         q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
         cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
         q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*
         cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
         cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin
         (q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
         cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))))/(
         epsilonW+wMag) + (dcm*(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*
         sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)
         *(dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(
         q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
         sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*
         cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
         cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*sin(q1)*cos(q3)+wC*x*
         cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
         (q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
         wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
         +x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
         q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
         (q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
         -sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+
         dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(
         q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))*
         (dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
         cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+
         y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*
         sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
         q2)*cos(q1)*cos(q3)*(dN-x))))*((Iyy-Izz)*wy*wz-Ixx*((dcm*(z-(dC-dcm)*
         sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(
         q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z
         +dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*
         wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*
         cos(q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'
         +dcm*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+
         wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin
         (q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*
         sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-
         sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(
         q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(
         q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(
         q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos
         (q2)*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*
         cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)
         *cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*
         q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*
         cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-
         wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*
         z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-
         z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*
         cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*
         cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-
         sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(
         q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*
         cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x
         *sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(
         q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-
         cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)
         *sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(
         q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-dcm*((z+dcm*sin(
         q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)
         *cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))
         +(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*
         sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*
         cos(q3)*(wN+y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*
         sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(
         dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(
         dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(
         q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(
         dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-
         x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(
         q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos
         (q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)
         *wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*
         wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)
         *cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-
         (dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos
         (q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos
         (q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)
         *(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*
         cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(
         q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(
         dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))-(dcm*(z-(dC-dcm)*sin(
         q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin
         (q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*
         sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(
         q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(
         dN-x)))*(dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)
         *cos(q2)*wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'
         +dcm*sin(q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+
         wC*sin(q1)*sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos
         (q2)*q2'-wC*cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(
         q1)-sin(q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*
         sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-
         sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(
         q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(
         q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*
         cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*
         sin(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*
         wx*((sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'
         +z*sin(q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*
         cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*
         cos(q3)*q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(
         q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)
         *sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))
         -x'^2-y'^2-dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*
         q2'-dcm*sin(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*
         cos(q1)*x'*q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*
         q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*
         wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(
         q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+
         x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(
         q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(
         q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(
         q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-
         sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1')))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z
         *cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)
         *cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
         q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos
         (q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
         (q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(
         dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
         cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
         dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
         sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
         q1)*cos(q3)*(dN-x))))))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
         sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
         *(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(
         q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
         (q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
         (dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
         sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
         *sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))) + (Izz+Ixx*(dcm*(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*
         cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(
         q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin
         (q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
         -sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(
         sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
         cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*sin(q1)*cos(
         q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*
         cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(
         q1)*sin(q2)*(wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*
         cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
         q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
         *(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
         q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*sin(q2)+dcm*
         x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)
         -wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)
         *sin(q2)*(wN-y)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*
         sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
         sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z
         *cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2/(wC^2*(dcm*(z-(dC-
         dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*
         sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
         wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(
         q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
         (q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos
         (q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
         -sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
         +sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(
         q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
         q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
         (q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
         -sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2)+(Iyy*((z-(dC-dcm)*sin(q2))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*x*sin(q1)*cos(q3)+wC*x*
         cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
         (q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
         wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
         (q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin
         (q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
         q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin
         (q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*
         sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)
         *cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
         z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))^2+m*(dC-dcm)^2*((z*cos(q2)
         *cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)
         *cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))*(dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos
         (q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos
         (q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
         sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))
         *(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin
         (q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
         q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))+dcm*(y*cos(q1)*cos(q3)+z*sin(q3)*
         cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(
         q3)*cos(q1)*(dN-x))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
         (q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin
         (q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
         q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(
         q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
         cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))^2)
         /(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(
         q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin
         (q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
         *cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z
         *cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)
         -x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)
         *sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
         z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2)*wz' + (m*(dC-dcm)*
         ((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
         *(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(
         q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
         cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+
         wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*
         sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
         (q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(
         q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*
         (z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))+dcm*(y*cos(q1)*
         cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(
         dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
         cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
         q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
         q2)*cos(q3)*(wN+y))))*(dcm*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(
         q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+
         x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-
         sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
         cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+
         y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(
         q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(
         q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(
         q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*
         cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*
         sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+
         (dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(
         q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*
         q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-
         (dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(
         dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)
         *(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)
         *wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos
         (q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos
         (q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*
         q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*
         cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(
         q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-
         cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))+(dC-dcm)*(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
         sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))
         *(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(
         q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*
         q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin(
         q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)
         *cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-
         wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin
         (q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(
         q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(
         q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*
         cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(
         q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin
         (q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(q1)*cos(
         q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*
         q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(
         q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos
         (q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(
         q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin
         (q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+
         y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*
         q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(
         sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)
         *cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*
         cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(
         q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x
         *sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-
         x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(
         q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-(dC-dcm)*(z*cos(q2)*cos(q3)+x*(sin(
         q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
         q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(
         dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*
         wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(
         q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)
         *sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-
         wC*cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(
         q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*
         sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*
         cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-
         sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(
         q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*
         x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*
         q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin
         (q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(
         q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)
         *q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*
         q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(
         wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)
         *q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2
         -dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*
         sin(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*
         x'*q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*
         sin(q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(
         q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+
         cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(
         q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin
         (q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos
         (q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-
         sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*
         cos(q1)*cos(q3)*(wN-y)*q1')))+(dC-dcm)*(dcm*(z-(dC-dcm)*sin(q2))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)
         *cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
         q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos
         (q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
         (q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(
         dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
         cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
         dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
         sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
         q1)*cos(q3)*(dN-x))))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*
         cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(q1)*cos(q3)+
         wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(
         q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*
         sin(q2)*(wN+y)))+(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
         *cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)
         *cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(
         dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(
         q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*
         cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))
         +dcm*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+
         sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z*cos(q2)*cos
         (q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
         q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
         sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
         *(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         +(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(
         wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
         sin(q1)*sin(q2)*cos(q3)*(wN+y))))*(m*g-bF*z'*vMag^expDamping/(epsilonV
         +vMag))+(Iyy*((z-(dC-dcm)*sin(q2))*(wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(
         q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(
         q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*
         cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         -(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*
         sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
         *cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*((z-(dC-
         dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*x*sin(
         q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(
         q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(
         q1)+sin(q1)*sin(q2)*(wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x
         *(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin
         (q1)*sin(q2)*cos(q3)))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)
         *cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin
         (q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-(dC-dcm)*(y
         *cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*
         cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)+wC*sin(
         q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
         (q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)
         -wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
         (q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))-m*(
         dC-dcm)*((dC-dcm)*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(
         q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*
         cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
         sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(wN+y-dcm*sin(
         q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)
         *cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*
         cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))
         -(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
         *(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*
         sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
         sin(q2)*cos(q3)*(wN+y))))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
         q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
         q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(q1)*cos(
         q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*
         cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(
         q1)*sin(q2)*(wN+y)))+(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
         (q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos
         (q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
         q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x*sin(q1)
         *cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos
         (q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*
         (wN-y)))+dcm*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*
         sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z*cos
         (q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos
         (q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*
         cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*
         cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
         cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))-Ixx*((wN+y-dcm*sin(q1)*cos(
         q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin
         (q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
         sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm
         *sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(
         q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(
         dN-x)))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*
         cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(
         q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(
         q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
         cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+
         y)))-(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin
         (q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin
         (q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
         q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
         (q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
         -sin(q2)*cos(q1)*cos(q3)*(dN-x))))*(dcm*(dC-dcm)*(y*cos(q1)*cos(q3)+z*
         sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(
         q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin
         (q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
         q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*
         cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
         *sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*sin(
         q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(
         q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(
         q1)+sin(q1)*sin(q2)*(wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(
         q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
         -sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*
         cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
         q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*sin(
         q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(
         q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(
         q1)-sin(q1)*sin(q2)*(wN-y)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(
         q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
         q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)
         *cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
         q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))/wC^2)*y''
         -((z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
         dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-
         wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(
         q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+
         dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(
         q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-
         (dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(
         q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin
         (q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
         *cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z
         +dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
         +sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))))*((Ixx-Izz)*wx*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*
         cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))
         +(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin
         (q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
         sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
         q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(
         sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
         cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(
         q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y
         *sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))
         *wz-Iyy*((z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+
         wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(
         q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm
         *sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*
         cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(
         q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin
         (q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(
         q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*
         q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*
         sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*
         q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)
         *q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(
         q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos
         (q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-
         wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(
         q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*
         ((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(
         q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)
         *q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*
         q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*
         sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-
         dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'
         +cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*
         sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)
         *x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(
         q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(
         wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)
         *q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-((z+dcm*sin(q2)+wC*sin(q3)*
         cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
         -(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*
         sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
         *cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))))*(x'^2+y'^2
         +(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*
         y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*
         z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-
         dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*
         q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(
         q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin
         (q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(q3)*wz*x'-(dC-
         dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*q1'-(dC-dcm)*
         sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-(dC-dcm)*y*
         sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(dC-dcm)*sin(
         q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)*(dN-x)*wz*
         q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)*wy*(sin(
         q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+
         cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos(q3)*q2'
         +y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*q1'+sin(
         q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*cos(q3)*
         q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(q2)*sin(
         q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-cos(q1)*
         cos(q2)*cos(q3)*(dN-x)*q2'))-(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x
         *(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin
         (q1)*sin(q2)*cos(q3)))*(dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*
         wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(
         q1)*sin(q3)*wz*q3'+dcm*sin(q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(
         q1)*(wN-y)*wz*q3'+wC*sin(q1)*sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(
         q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(
         q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*
         cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*
         cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(
         q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(
         q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*
         sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(
         q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(
         q1)*(wN-y)*q1')+wC*wx*((sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*
         sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*
         q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*
         q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
         q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)
         *sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*
         cos(q1)*cos(q3)*q1'))-x'^2-y'^2-dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*
         wz*z'-wC*z*cos(q2)*wz*q2'-dcm*sin(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(
         q2)*x'*q1'-dcm*sin(q2)*cos(q1)*x'*q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*
         x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(
         q2)*(wN-y)*wz*q1'-dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*
         y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'
         +x*sin(q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(
         wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(
         q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*
         sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)
         *cos(q3)*(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1'))))-(Iyy*((z-(
         dC-dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*
         sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
         q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-
         dcm)*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(
         q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(dN-x-(dC-
         dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*
         cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
         z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*((z-(dC-dcm)*sin(q2))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*x*sin(q1)*cos(q3)+wC*x*
         cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
         (q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
         wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
         (q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
         *(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin
         (q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
         q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin
         (q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*
         sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)
         *cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
         cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
         z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))-m*(dC-dcm)*((dC-dcm)*(x-
         dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)
         *sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
         q1)*cos(q3)*(dN-x))-(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(
         q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
         sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
         q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
         sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(dN-x-(dC-
         dcm)*cos(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
         cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*
         sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
         cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(
         sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
         q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(
         q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))
         *((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
         *(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(
         q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
         cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+
         wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*
         sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
         (q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(
         q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*
         (z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))+dcm*(y*cos(q1)*
         cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(
         dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z*cos(q2)*cos(q3)+x*(sin(q1)*
         sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
         q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
         cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(
         q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
         q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
         q2)*cos(q3)*(wN+y))))-Ixx*((dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-
         sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)
         +z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
         sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos
         (q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
         sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(dN-x-(dC-
         dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*
         sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
         wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(
         q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin
         (q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(x-dcm*cos(q1)*
         cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)
         *sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-
         x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
         dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
         cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
         q3)*(dN-x))))*(dcm*(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin
         (q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(
         dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(
         q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
         sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*
         cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
         cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*sin(q1)*cos(q3)+wC*x*
         cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
         (q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
         wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
         +x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
         q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
         (q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
         -sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+
         dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(
         q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))*
         (dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
         cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+
         y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*
         sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
         q2)*cos(q1)*cos(q3)*(dN-x))))/wC^2)*x'')/(dcm*(z-(dC-dcm)*sin(q2))*(z*
         cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
         sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)
         *cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
         q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos
         (q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
         q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
         (q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
         cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
         cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(
         dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
         cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
         dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
         sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
         q1)*cos(q3)*(dN-x))))^2 - (Ixx-Iyy)*wx*wy

   (121) %--------------------------------------------------------------------
   (122) %   Integration parameters and initial values for variables.
   (123) Input  tFinal = 120 sec,  tStep = 0.05 sec,  absError = 1.0E-07
   (124) Input  x' = 0 m/s,  y' = 0 m/s,  wz = 0 rad/sec
   (125) %--------------------------------------------------------------------
   (126) %   Calculate yaw, pitch and roll angles in terms of q1, q2, q3.
   (127) Yaw   = GetAngleBetweenUnitVectors( Nx>, Cy> )  -  pi/2
-> (128) Yaw = -1.570796 + acos(sin(q2)*sin(q3)*cos(q1)-sin(q1)*cos(q3))

   (129) Pitch = GetAngleBetweenUnitVectors( Nz>, Cx> )  -  pi/2
-> (130) Pitch = 1.570796 - acos(sin(q2))

   (131) Roll  = pi/2  -  GetAngleBetweenUnitVectors( Nz>, Cy> )
-> (132) Roll = 1.570796 - acos(sin(q3)*cos(q2))

   (133) %--------------------------------------------------------------------
   (134) %   Solve for initial values of q1, q2, q3 via initial yaw, pitch, roll angles.
   (135) Constant  Yaw0 = 60 deg,  Pitch0 = 0 deg,  Roll0 = 0 deg
   (136) SolveSetInput( [Yaw; Pitch; Roll] = [Yaw0; Pitch0; Roll0],  q1 = 60 deg, q2 = 0 deg, q3 = 0 deg )

->    %  INPUT has been assigned as follows:
->    %   q1                        60                      deg
->    %   q2                        0                       deg
->    %   q3                        0                       deg

   (137) %--------------------------------------------------------------------
   (138) %   Solve for initial values of x, y, z using the CableConstraint (and initial values of q1, q2, q3).
   (139) SolveSetInput( CableConstraint = 0,  x = Input(dN)/2 m,  y=0 m,  z = 0.8*Input(L1) m )

->    %  INPUT has been assigned as follows:
->    %   x                         8.16666666666667        m
->    %   y                        -0.2886751345948167      m
->    %   z                         20.74983266331456       m

   (140) %--------------------------------------------------------------------
   (141) %   List output quantities and solve ODEs.
   (142) Distance = Co.GetDistance( No )
-> (143) Distance = sqrt(dcm^2+x^2+y^2+z^2+2*dcm*z*sin(q2)-2*dcm*x*cos(q1)*cos(
         q2)-2*dcm*y*sin(q1)*cos(q2))

   (144) Output  t sec,  Distance m,  Yaw deg,  Pitch deg,  Roll deg,  vMag m/s,  wMag rad/sec, CableConstraint[1] m, CableConstraint[2] m, CableConstraint[3] m
   (145) Output  t sec,  x m,  y m,  z m,  x' m/s,  y' m/s,  z' m/s,  q1 deg,  q2 deg,  q3 deg,  wx rad/sec,  wy rad/sec,  wz rad/sec
   (146) ODE( Dynamics = 0,  x'',  y'',  wz' )  MGAircraftTrifilarPendulumDynamicsKaneEmbedded.m

   (147) %--------------------------------------------------------------------
Saved by Motion Genesis LLC.   Copyright (c) 2009-2018 Motion Genesis LLC on command names and syntax. All rights reserved.