%Mechanism Kinematics
% Andy Ruina May 1, 2019
clear all
%Assume L>d. Then,  slot can go round and round.

syms theta phi L d r real

%Law of cosines
eq1= L^2 == d^2 + r^2  - 2*L*r*cos(theta);

%h = h  , kind of the law of sines
eq2 =  r*sin(theta) - L*sin(phi);

roots = solve(eq1,r); %solve quadratic eqn
r = roots(2); % Pick the bigger (positive) root

phi = solve(eq2,phi);
phi = phi(2);

d=1; L = sqrt(2); theta = pi/2;  % check a simple case


r = subs(r); phi = subs(phi);
eval(phi)/pi;  % should be 3/4.
%%%%%%%%%%%%%%%%%%%%%%%%

%VELOCITY
syms thetadot thetaddot phidot phiddot rdot rddot real
k = [ 0; 0; 1];
er     = [cos(theta); sin(theta); 0];   eth = cross(k,er);
lambda = [cos(phi);   sin(phi)  ; 0];   n   = cross(k,lambda);

vP1 = thetadot*er + rdot*eth;
vP2 = phidot*n;
eq3=  vP1 == vP2;  % Think about vel of P two different ways "let x = x".
[rdot,phidot] = (solve(eq3(1),eq3(2),rdot,phidot));

thetadot=1;
rdot = vpa(subs(rdot), 8)
phidot = vpa(subs(phidot), 8)



%Acceleration
syms thetaddot phiddot  rddot real

aP1 = (rddot - r*thetadot^2)*er  + (r*thetaddot + 2*rdot*thetadot)*eth; 
aP2 = (-L* phidot^2)*lambda      + (L*phiddot) * n;
eq4 = aP1-aP2;

[rddot,phiddot]  = solve(eq4(1),eq4(2), rddot, phiddot);

thetaddot = 0;
rddot     = vpa(subs(rddot),8)
phiddot   = vpa(subs(phiddot),8)


