% posRRT.m
% position analysis RRT dyad: Ai->R  Aj->R Aj->T  
% slider (j) with CM at Aj=Cj
% As a point on the sliding direction

function out = posRRT(rAi_, rAs_, li, Theta)
% rAi_ is the position vector of Ai
% rAs_ is the position vector of a point As on the sliding direction
% li is the length of link (i) li=AiAj 
% Theta is the angle of the sliding direction with x-axis

% components of position of Ai
xAi = rAi_(1);
yAi = rAi_(2);
% components of position of As
xAs = rAs_(1);
yAs = rAs_(2);

% rAj_ = [xAj yAj 0] position vector of Aj=Cj on link (j)

% syms xAj yAj
if Theta==pi/2 || Theta==3*pi/2 
   xAj1 = xAs; 
   xAj2 = xAs;
   xAj = xAs;
   syms yAj
   eqRRT = (xAi-xAj)^2+(yAi-yAj)^2-li^2;
   solRRT = solve(eqRRT);
   yAj1 = eval(solRRT(1));
   yAj2 = eval(solRRT(2));
   
elseif Theta==0 || Theta==pi
    yAj1 = yAs; yAj2 = yAs;
    yAj = yAs;
    syms xAj
    eqRRT = (xAi-xAj)^2+(yAi-yAj)^2-li^2;
    solRRT = solve(eqRRT);
    xAj1 = eval(solRRT(1));
    xAj2 = eval(solRRT(2));
else
    syms xAj yAj
    eqRRT1 = (xAi-xAj)^2 + (yAi-yAj)^2 - li^2;
    eqRRT2 = tan(Theta) - (yAj-yAs)/(xAj-xAs);
    solRRT = solve(eqRRT1, eqRRT2);
    xPpos = eval(solRRT.xP);
    yPpos = eval(solRRT.yP);
    xAj1 = xPpos(1); xAj2 = xPpos(2); 
    yAj1 = yPpos(1); yAj2 = yPpos(2);
 end  
out = [xAj1 yAj1 xAj2 yAj2];
% rAjI_  = [xAj1 yAj1 0] solution I  for Aj
% rAjII_ = [xAj2 yAj2 0] solution II for Aj

end
