(1) % MotionGenesis file:  MGAircraftTrifilarPendulumConstraints.txt
   (2) % Copyright (c) 2016-18 Motion Genesis LLC.
   (3) %--------------------------------------------------------------------
   (4) NewtonianFrame  N                    % Earth / aircraft hanger.
   (5) RigidBody       C                    % Aircraft.
   (6) Point           N1(N), N2(N), N3(N)  % End-points of cable on N.
   (7) Point           C1(C), C2(C), C3(C)  % End-points of cable on C.
   (8) %--------------------------------------------------------------------
   (9) Constant  L1 = 30 m                  % Length of cable between N1 and C1.
   (10) Constant  L2 = 30 m                  % Length of cable between N2 and C2.
   (11) Constant  L3 = 30 m                  % Length of cable between N3 and C3.
   (12) Constant  dN = 30 m                  % Distance between No and N1.
   (13) Constant  dC = 30 m                  % Distance between Co and C1.
   (14) Constant  wN = 20 m                  % Distance between No and N2.
   (15) Constant  wC = 20 m                  % Distance between Co and C2.
   (16) Constant dcm =  8 m                  % Distance between Co and Ccm.
   (17) Variable  x,   y,   z                % Locates Ccm from No.
   (18) Variable  q1,  q2,  q3               % BodyZYX Euler angles.
   (19) %--------------------------------------------------------------------
   (20) %   Rotational and translational kinematics.
   (21) C.SetRotationMatrix( N, BodyZYX, q1, q2, q3 )
-> (22) C_N[1,1] = cos(q1)*cos(q2)
-> (23) C_N[1,2] = sin(q1)*cos(q2)
-> (24) C_N[1,3] = -sin(q2)
-> (25) C_N[2,1] = sin(q2)*sin(q3)*cos(q1) - sin(q1)*cos(q3)
-> (26) C_N[2,2] = cos(q1)*cos(q3) + sin(q1)*sin(q2)*sin(q3)
-> (27) C_N[2,3] = sin(q3)*cos(q2)
-> (28) C_N[3,1] = sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)
-> (29) C_N[3,2] = sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)
-> (30) C_N[3,3] = cos(q2)*cos(q3)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

   (63) %--------------------------------------------------------------------
   (64) %   Solve for values of q1, q2, q3 via yaw, pitch, roll angles.
   (65) Constant  Yaw0 = 0 deg,  Pitch0 = 5 deg,  Roll0 = 5 deg
   (66) Solve( [Yaw; Pitch; Roll] = [Yaw0; Pitch0; Roll0],  q1 = 0 deg, q2 = 5 deg, q3 = 5 deg )
-> (67) q1 = 0.007654341       %  or  q1 = 0.4385614 deg.
-> (68) q2 = 0.08726646       %  or  q2 = 5 deg.
-> (69) q3 = 0.08760066       %  or  q3 = 5.019148 deg.

   (70) %--------------------------------------------------------------------
   (71) %   Solve for values of x, y, z using the CableConstraint (and the values of q1, q2, q3).
   (72) Solve( CableConstraint = 0,  x = Input(dN)/2 m,  y=0 m,  z = 0.2*Input(L1) m )
-> (73) x = 35.7853
-> (74) y = 11.1512
-> (75) z = -0.2130373

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

   (80) %--------------------------------------------------------------------