Colony

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

desX = zeros(size(curX),1);

desY = zeros(size(curX),1);

desTh = zeros(size(curX),1);

for i=1:size(curX),

    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));

end

end