MGAircraftTrifilarPendulumConstraints.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGAircraftTrifilarPendulumConstraints.txt
   (2) % Copyright (c) 2016 Motion Genesis LLC.  Only for use with MotionGenesis.
   (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) Variable  x,   y,   z                % Locates Ccm from No.
   (19) Variable  q1,  q2,  q3               % BodyZYX Euler angles.
   (20) %--------------------------------------------------------------------
   (21) %   Rotational and translational kinematics.
   (22) C.SetRotationMatrix( N, BodyZYX, q1, q2, q3 )
-> (23) C_N[1,1] = cos(q1)*cos(q2)
-> (24) C_N[1,2] = sin(q1)*cos(q2)
-> (25) C_N[1,3] = -sin(q2)
-> (26) C_N[2,1] = sin(q2)*sin(q3)*cos(q1) - sin(q1)*cos(q3)
-> (27) C_N[2,2] = cos(q1)*cos(q3) + sin(q1)*sin(q2)*sin(q3)
-> (28) C_N[2,3] = sin(q3)*cos(q2)
-> (29) C_N[3,1] = sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)
-> (30) C_N[3,2] = sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)
-> (31) C_N[3,3] = cos(q2)*cos(q3)

   (32) CCm.SetPosition( No, x*Nx> + y*Ny> + z*Nz> )
-> (33) p_No_Ccm> = x*Nx> + y*Ny> + z*Nz>

   (34) Co.SetPosition( CCm, -dcm*Cx> )  
-> (35) p_Ccm_Co> = -dcm*Cx>

   (36) C1.SetPosition( Co,    dC*Cx> ) 
-> (37) p_Co_C1> = dC*Cx>

   (38) C2.SetPosition( Co,   -wC*Cy> ) 
-> (39) p_Co_C2> = -wC*Cy>

   (40) C3.SetPosition( Co,    wC*Cy> ) 
-> (41) p_Co_C3> = wC*Cy>

   (42) N1.SetPosition( No,    dN*Nx> ) 
-> (43) p_No_N1> = dN*Nx>

   (44) N2.SetPosition( No,   -wN*Ny> ) 
-> (45) p_No_N2> = -wN*Ny>

   (46) N3.SetPosition( No,    wN*Ny> ) 
-> (47) p_No_N3> = wN*Ny>

   (48) %--------------------------------------------------------------------
   (49) %   Configuration constraints: Length of cables.
   (50) CableConstraint[1] = C1.GetDistanceSquared( N1 ) - L1^2
-> (51) 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)

   (52) CableConstraint[2] = C2.GetDistanceSquared( N2 ) - L2^2
-> (53) 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))

   (54) CableConstraint[3] = C3.GetDistanceSquared( N3 ) - L3^2
-> (55) 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))

   (56) %--------------------------------------------------------------------
   (57) %   Calculate yaw, pitch and roll angles in terms of q1, q2, q3.
   (58) Yaw   = GetAngleBetweenUnitVectors( Nx>, Cy> )  -  pi/2 
-> (59) Yaw = -1.570796 + acos(sin(q2)*sin(q3)*cos(q1)-sin(q1)*cos(q3))

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

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

   (64) %--------------------------------------------------------------------
   (65) %   Situation when yaw and roll do not correspond directly to q1 and q3.
   (66) Constant  Yaw0 = 0 deg,  Pitch0 = 5 deg,  Roll0 = 5 deg
   (67) Solve( [ CableConstraint;  Yaw - Yaw0;  Pitch - Pitch0;  Roll - Roll0 ] = 0,   & 
         x = Input(dN)/2 m, y=0 m, z = 0.2*Input(L1) m,  q1 = 0 deg, q2 = 5 deg, q3 = 5 deg )
-> (68) x = 35.7853
-> (69) y = 11.1512
-> (70) z = -0.2130373
-> (71) q1 = 0.007654341       %  or  q1 = 0.4385614 deg.
-> (72) q2 = 0.08726646       %  or  q2 = 5 deg.
-> (73) q3 = 0.08760066       %  or  q3 = 5.019148 deg.

   (74) %--------------------------------------------------------------------
   (75) %   Calculate the distance between No and Ccm.
   (76) distance = EvaluateToNumber( Ccm.GetDistance( No ) )
-> (77) distance = 37.4831

   (78) %--------------------------------------------------------------------
Saved by Motion Genesis LLC.   Portions copyright (c) 2009-2017 Motion Genesis LLC. Rights reserved. Only for use with MotionGenesis.