% Exam 2, Problem 3
% N1> downward, N2> to the right, and N3> = N1> x N2>

% Set up problem
OVERWRITE ON
DEGREES ON
DIGITS 3
FRAMES N
BODIES C,F,S,T
POINTS O,P,A,K,H
CONSTANTS LC,LF,LS,LT
CONSTANTS X,Y
VARIABLES Q{4}''

% Rotation matrices
SIMPROT(N,C,-3,Q1)
SIMPROT(N,F,3,Q2)
SIMPROT(N,S,-3,Q3)
SIMPROT(N,T,3,Q4)

% Position vector loop equations
P_H_TO>=0.5*LT*T1>
P_TO_K>=P_H_TO>
P_K_SO>=0.5*LS*S1>
P_SO_A>=P_K_SO>
P_A_FO>=0.5*LF*F1>
P_FO_P>=P_A_FO>
P_P_CO>=0.5*LC*C1>
P_CO_O>=P_P_CO>
LOOP>=P_O_H>+Y*N1>+X*N2>
EXPRESS(LOOP>,N)

% Nonlinear equation solution for Q3 and Q4 as a function of
% Q1 and Q2
LOOP=DOT(LOOP>,[N1>;N2>])
UNITS [Q1,Q2,Q3,Q4]=DEG
INPUT Q1=45,Q2=38,Q3=30,Q4=45
INPUT LC=0.170,LF=0.198,LS=0.438,LT=0.432
INPUT X=0.246,Y=0.804
CODE NONLINEAR(LOOP,Q3,Q4) exam1pr3.m,subs

% Linear equation solution for Q3' and Q4' as a function of
% Q1', Q2', and Q1, Q2, Q3, Q4
LOOPDOT>=DT(LOOP>,N)
LOOPDOT=DOT(LOOPDOT>,[N1>;N2>])
SOLVE(LOOPDOT,Q3',Q4')
Q3P=EVALUATE(Q3',Q1=45,Q2=38,Q3=22.7729,Q4=73.3319,Q1'=441,Q2'=137,LC=0.170,LF=0.198,LS=0.438,LT=0.432,X=0.246,Y=0.804)
Q4P=EVALUATE(Q4',Q1=45,Q2=38,Q3=22.7729,Q4=73.3319,Q1'=441,Q2'=137,LC=0.170,LF=0.198,LS=0.438,LT=0.432,X=0.246,Y=0.804)
%Q3P=EVALUATE(Q3',Q1=45,Q2=38,Q3=-72.4526,Q4=-23.6522,Q1'=441,Q2'=137,LC=0.170,LF=0.198,LS=0.438,LT=0.432,X=0.246,Y=0.804)
%Q4P=EVALUATE(Q4',Q1=45,Q2=38,Q3=-72.4526,Q4=-23.6522,Q1'=441,Q2'=137,LC=0.170,LF=0.198,LS=0.438,LT=0.432,X=0.246,Y=0.804)