% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% function_CalculateCentroid returns the "centroid" of a set of points.    %
% This centroid is not a geometrical centroid but the point where          %
% every other point will calculate its angle from.                         %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %

%  INPUTS:   Name           | Type    | Dimensions | Description
%            type           | int     | 1x1        | determines the type of calculation
%            Points1        | nPoints | nx2        | Points in t-1
%            Points2        | nPoints | nx2        | Points in t
%
% OTUPUTS:   Name           | Type    | Dimensions | Description
%            Centroid       | Point   | 2x2        | Centroid in t-1 and t

function Centroid = function_calculateCentroid(type, Points1, Points2)
    % Tipe can be one of two values
    %      0 ---> the calculated centroid will be the first known point
    %      1 ---> the calculated centroid will be the average of all points
    
    Centroid(1,:) = Points1(1,:) - (Points1(1,:) - sum(Points1)/size(Points1,1))* type ;
    Centroid(2,:) = Points2(1,:) - (Points2(1,:) - sum(Points2)/size(Points2,1))* type ;
end