function [ output_args ] = plotFormation( initial, state_vector)
%PLOTFORMATION Plots an iniital and a transformed formation for the robot.
%Calculates the transformed formation based on the initial formation and
%the state vector describing the transformed formation. Primarily used for debugging.

t = state_vector(1:3);
s = state_vector(4);
q = state_vector(5:8);

initial_prime = [zeros(size(initial,1),1), initial];
final = [];

for i=1:size(initial_prime,1) 
    temp = t+[s*quatRotateVec(q',initial(i,:)')']
    final = [final; temp];
end

figure(2);
hold on;
colours = ['r', 'g', 'b', 'c'];
for i=1:size(initial_prime,1) 
    scatter3(initial(i,1), initial(i,2), initial(i,3), colours(i));
    scatter3(final(i,1), final(i,2), final(i,3), colours(i));
end

end

