% posRRR.m
% position RRR dyad 

function out = posRRR(rAi_, rAj_, li, lj)

% rAi_ is the position vector of Ai: rAi_ = [xAi yAi 0]
% rAj_ is the position vector of Aj: rAj_ = [xAj yAj 0]
% li is the length of link (i ): li=AiP
% lj is  length of link (j):  lj=AjP

% components of position of Ai
xAi = rAi_(1);
yAi = rAi_(2);
% components of position of Aj
xAj = rAj_(1);
yAj = rAj_(2);

syms xP yP
eqRRRi = (xAi-xP)^2+(yAi-yP)^2-li^2;
eqRRRj = (xAj-xP)^2+(yAj-yP)^2-lj^2;
solRRR = solve(eqRRRi, eqRRRj);
xPpos = eval(solRRR.xP);
yPpos = eval(solRRR.yP);
xP1 = xPpos(1); xP2 = xPpos(2); 
yP1 = yPpos(1); yP2 = yPpos(2); 

out = [xP1, yP1, xP2, yP2];
% rPI_  = [xP1 yP1 0] solution I  for P
% rPII_ = [xP2 yP2 0] solution II for P
end