centralPotentialODE(stateVect,charges,p,polarity,masses)

function s = centralPotentialODE(stateVect,charges,p,polarity,masses)
% centralPotentialODE : ODE for multi-body 2D central potential motion
% stateVect = [x coordinates; y coordinates; x velocity; y velocity]
%   where each of these subcomponents has (number of particles) elements
% charges = column vector of charge on each particle (eg mass or electric
%   charge)
% p = exponent of force eg 2 is inverse square law
% polarity is 1 for like charges attracting, -1 for repelling
% masses is column vector of particle masses.  If undefined then the
%   charges are used as the particle masses.

%   Copyright 2008-2009 The MathWorks, Inc.
%   $Revision: 35 $  $Date: 2009-05-29 15:27:34 +0100 (Fri, 29 May 2009) $

if nargin<2
    % 2D motion so 4 stateVect elements for each charge
    chargeCount = numel(stateVect)/4;
    % Define the charge vector
    charges = ones(chargeCount,1);
else
    chargeCount = numel(charges);
end

if nargin<3
    % Inverse square law if p undefined
    p=2;
end

if nargin<4
    % Attractive potential if polarity undefined.  Polarity = 1 for
    % attraction between like charges (eg gravity), -1 for repulsion between like
    % charges (eg electric charges)
    polarity = 1;
end

if nargin<5
    % The particle mass gives the scaling between the force on the particle
    % and the acceleration.  If left undefined then we assume that the
    % potential we are modelling is gravitational, so the 'mass charge' is
    % equal to the mass.    
    masses = charges;
end

% Extract the position and velocity components from the state vector
x = stateVect(1:chargeCount);
y = stateVect(chargeCount+(1:chargeCount));
vx = stateVect(2*chargeCount+(1:chargeCount));
vy = stateVect(3*chargeCount+(1:chargeCount));

%% Calculate the distances
% First determine the displacement matrices for the x and y components of
% the particle positions

% % Fancy way to calculate this using array fun:
% dispXMat = cell2mat(arrayfun(@(thisX) x-thisX,x,'UniformOutput',false)');
% dispYMat = cell2mat(arrayfun(@(thisY) y-thisY,y,'UniformOutput',false)');

% Simpler way of calculating the displacement matrices:

% Preallocate storage for the displacement matrices
dispXMat = zeros(chargeCount, chargeCount);
dispYMat = zeros(chargeCount, chargeCount);

for chargeIndex = 1:chargeCount
    dispXMat(:,chargeIndex) = x - x(chargeIndex);
    dispYMat(:,chargeIndex) = y - y(chargeIndex);
end

% Calculate the distance matrix
distMat = sqrt(dispXMat.^2 + dispYMat.^2);

%% Calculate the particle forces

% Preallocate the force vectors
forceXVect = zeros(chargeCount,1);
forceYVect = zeros(chargeCount,1);

% Create an index vector that will be used to extract only those elements
% of the displacement and distance matrices that aren't associated with the
% currently selected charge
indexVect = 1:chargeCount;

for chargeIndex = 1:chargeCount
    % Indices of the other charges
    otherIndices = indexVect(indexVect ~= chargeIndex);
    otherCharges = charges(otherIndices);
    thisCharge = charges(chargeIndex);
    thisMass = masses(chargeIndex);
    
    forceXVect(chargeIndex) = thisCharge*sum(polarity*...
        dispXMat(otherIndices,chargeIndex).*otherCharges...
        ./(distMat(otherIndices,chargeIndex).^(p+1)))/thisMass;
    forceYVect(chargeIndex) = thisCharge*sum(polarity*...
        dispYMat(otherIndices,chargeIndex).*otherCharges...
        ./(distMat(otherIndices,chargeIndex).^(p+1)))/thisMass;
end

% New state vect
s = [vx; vy; forceXVect; forceYVect];