Colony

function [ desX, desY, desPhi ] = desiredPosition( curX, curY, curPhi, desR, desPhi )

%function [ desX, desY, desTh ] = desiredPosition( curX, curY, curTh, desR, desPhi )

desX = zeros(size(curX));

desY = zeros(size(curX));

desTh = zeros(size(curX));

for i=2:size(curX,1)

     desX(i) = desR(i)*cos(desPhi(i) - curPhi(1)) + curX(1);

     desY(i) = desR(i)*sin(desPhi(i) - curPhi(1)) + curY(1);

%    desX(i) = desR(i)*cos(desPhi(i) - curTh(1)) + curX(1);

%    desY(i) = desR(i)*sin(desPhi(i) - curTh(1)) + curY(1);

    %desTh(i) = atan2(desY(i)-curY(i), desX(i)-curX(i));

    % rotate and translate so curr pos is 0,0 and cur theta = 0

    % rotate by -curTh(i)

%     x = (desX(i)-curX(i))*cos(-curTh(i)) - (desY(i)-curY(i))*sin(-curTh(i));

%     y = (desX(i)-curX(i))*sin(-curTh(i)) + (desY(i)-curY(i))*cos(-curTh(i));

%     

%     d = sqrt(x^2 + y^2)

%     p = atan2(y, x)

%     r = d/(2*sin(0.5*(pi-2*p)))

%    

%     (d*sin(p)-r)/r;

%     desTh(i) = asin((d*sin(p)-r)/r) + curTh(i);

    desPhi(i) = 0;

    %desTh(i) = 0; %for now, don't command theta

end

end