Colony

% For now I am ignoring ThetaD and just computing an arc

% This function expects to NOT have the quees be at index 1

function [V,W] = computeTrajectories(X,Y,Theta,XD,YD,ThetaD)

Kp = 5;

% how far away are we

dists = sqrt((X-XD).^2 + (Y-YD) .^ 2);

for idx=1:size(X,1)

    %V(idx) = dists(idx)/Kp; % linear velocity is proportional to distance

    %W(idx) = (ThetaD(idx) - Theta(idx))/Kp;

    V(idx) = (cos(Theta(idx))*(XD(idx) - X(idx)) + sin(Theta(idx))*(YD(idx) - Y(idx)))/Kp;

    W(idx) = (-sin(Theta(idx))*(XD(idx) - X(idx)) + cos(Theta(idx))*(YD(idx) - Y(idx)))/Kp;

end

end