% 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 = 7;

% 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