%
% Basic physical simulation example for multiple 2D particles
%
% Chad Jenkins (cjenkins@cs.brown.edu)
% Modified: January 18, 2009
%
% based on "Particle System Dynamics" by A. Witkin in 
% "Physically Based Modeling: Principles and Practice" (1997)
%

% select a set of simulation parameters to run
sim_case = 5;

% initialization: particle state, timestep, mass
% particle (p) concatenates particle position (x) and velocity (y)
if (sim_case == 1) % two particles with lower time step
    p = [[0.00; 20.00; 4.00; 0.00] ... % particle 1
        [-5.00; 5.00; 10.00*cos(pi/3); 10.00*sin(pi/3)]]; % particle 2
    dt = 0.01;
    t = 0;
    m = 100;
elseif (sim_case == 2) % two particles with higher time step
    p = [[0.00; 20.00; 4.00; 0.00] ... % particle 1
        [-5.00; 5.00; 10.00*cos(pi/3); 10.00*sin(pi/3)]]; % particle 2
    dt = 0.1;
    t = 0;
    m = 100;
elseif (sim_case == 3) % nine particles with higher time step
    p = [[0.00; 20.00; 8.00; 0.00] ...
        [0.00; 20.00; 0.00; 8.00] ...
        [0.00; 20.00; 8.00*sin(pi/6); 10.00*cos(pi/6)] ...
        [0.00; 20.00; 8.00*sin(-pi/6); 10.00*cos(-pi/6)] ...
        [0.00; 20.00; 8.00*sin(pi/3); 10.00*cos(pi/3)] ...
        [0.00; 20.00; 8.00*sin(-pi/3); 10.00*cos(-pi/3)] ...
        [0.00; 20.00; 8.00*sin(pi/4); 10.00*cos(pi/4)] ...
        [0.00; 20.00; 8.00*sin(-pi/4); 10.00*cos(-pi/4)] ...
        [0.00; 20.00; 8.00*sin(pi/2); 10.00*cos(pi/2)]];
    dt = 0.01;
    t = 0;
    m = 100;
elseif (sim_case == 4) % nine particles with lower time step
    p = [[0.00; 20.00; 8.00; 0.00] ...
        [0.00; 20.00; 0.00; 8.00] ...
        [0.00; 20.00; 8.00*sin(pi/6); 10.00*cos(pi/6)] ...
        [0.00; 20.00; 8.00*sin(-pi/6); 10.00*cos(-pi/6)] ...
        [0.00; 20.00; 8.00*sin(pi/3); 10.00*cos(pi/3)] ...
        [0.00; 20.00; 8.00*sin(-pi/3); 10.00*cos(-pi/3)] ...
        [0.00; 20.00; 8.00*sin(pi/4); 10.00*cos(pi/4)] ...
        [0.00; 20.00; 8.00*sin(-pi/4); 10.00*cos(-pi/4)] ...
        [0.00; 20.00; 8.00*sin(pi/2); 10.00*cos(pi/2)]];
    dt = 0.002;
    t = 0;
    m = 100;
elseif (sim_case == 5) % one particle connected to fixed point with light gravity
    p = [[0.00; 20.00; 0.00; 0.00] [1.00; 18.00; 10.00*cos(pi/3); 10.00*sin(pi/3)]];
    dt = 0.1;
    t = 0;
    m = 100;
end


% loop to update particles over time
while (1)
    %fprintf(1,'t = %f : x = %f, %f;  v = %f, %f\n',t,x(1,1),x(2,1),v(1,1),v(2,1));

    % draw bounding box
    figure(1);
    hold off;
    fill([-12 -12 -10 -10],[30 -5 -5 30],'g-');
    hold on;
    fill([12 12 10 10],[30 -5 -5 30],'g-');
    fill([-12 -12 12 12],[0 -5 -5 0],'g-');

    %update each particle
    for i = 1:size(p,2)
        plot(p(1,i),p(2,i),'b.','MarkerSize',25,'Color',[(mod(i,3)==2) (mod(i,3)==0) (mod(i,3)==1)]); % draw current position        %plot([p(1,i) p(1,i)+p(3,i)],[p(2,i) p(2,i)+p(4,i)],'r'); % draw current velocity
        axis('equal');

        % accumulate current forces
        if (sim_case ~= 5)
            F = (m * [0.00; -9.81]) + [0.00; 0.00]; % gravitational force (mass times gravity vector) plus external forces
        end

        % put spring between first two particles
        if ((sim_case == 5) & (i == 1))
            F = [0.00; -2.81] + (-0.99*(norm(p(1:2,1)-p(1:2,2))-1) * (p(1:2,1)-p(1:2,2))) + -(p(3:4,1)-p(3:4,2))/norm(p(1:2,1)-p(1:2,2));
        elseif (sim_case == 5)
            F = [0.00; -2.81] + (-0.99*(norm(p(1:2,1)-p(1:2,2))-1) * (p(1:2,2)-p(1:2,1))) + -(p(3:4,2)-p(3:4,1))/norm(p(1:2,1)-p(1:2,2));;
        end

        % hacky leaky momentum during collisions
        if (p(1,i) >= 10)
            p(3,i) = 0.95 * -p(3,i); % loss in horizontal momentum due to friction
        end
        if (p(1,i) <= -10)
            p(3,i) = 0.95 * -p(3,i); % loss in horizontal momentum due to friction
        end
        if (p(2,i) <= 0.01)
            p(3,i) = 0.98 * p(3,i); % loss in horizontal momentum due to friction
            p(4,i) = 0.9 * -p(4,i); % loss in vertical momentum due to friction
        end

        % compute position and velocity time deriatives
        dx = p(3:4,i);
        dv = F/m;

        % scale derivatives by discrete timestep duration
        dx = dx * dt;
        dv = dv * dt;

        % update position and velocity (using Euler's method) one timestep into the future
        if (sim_case ~= 5)
            p(1:2,i) = p(1:2,i) + dx;
            p(3:4,i) = p(3:4,i) + dv;
        elseif (i == 1)
            p(1:2,i) = p(1:2,i) + dx;
            p(3:4,i) = p(3:4,i) + dv;
        end

    end

    % update (simulation) time
    t = t + dt;

end