Code covered by the BSD License  

Highlights from
Sphere Collider

image thumbnail
from Sphere Collider by Ligong Han
This is a simple physics engine for simulating sphere collision.

SphereCollider(balls,ball_pos,ball_v,varargin)
function SphereCollider(balls,ball_pos,ball_v,varargin)
%SPHERECOLLIDER   Physics engine for multi-sphere collision.
%
%   Value Updating: compute acceleration at the current position ->
%   calculate velocity with the updated acceleration -> refresh position
%   with new velocity.
%
%   PROPERTIES and VARIABLES:
%   BALLs:
%    balls     : [r  m  q ]
%    ball_pos  : [x  y  z ]
%    ball_v    : [vx vy vz]
%    ball_color: [R G B alpha]
%   WALLs:
%    wall_plane : [A B C D]
%    wall_sphere: [R x y z]
%   FORCESOURCE:
%    GravitySrc : [M x y z]  (default: [])
%    CoulombSrc : [Q x y z]  (default: [])
%    GravityVec : [gx gy gz] (default: [ 0  0 -1])
%    GroundPoint: [x  y  z ] (default: [ 0  0  0])
%    grav_ball  : 1(on) OR {0(off)}
%   VARs:
%    dt        : time interval between two frames (default: 0.1)
%    tmax      : the total simulation time
%    debug_mode: graph of momentum and energy will be displayed in debug mode
%   CONSTANTs:
%    G : Newtonian constant of gravitation (default: 6.6738480e-11)
%    g : Gravitational acceleration        (default: 9.80665      )
%    k : Coulomb's constant                (default: 8.987551787e9)
%    Cr: Coefficient of restitution        (default: 1            )
%
%   NOTE:
%   Equations for collisions between elastic particles: (u for init, v for final)
%    va = (ma*ua+mb*ub+mb*Cr*(ub-ua))/(ma+mb);
%    vb = (ma*ua+mb*ub+ma*Cr*(ua-ub))/(ma+mb);
%   Distance from a point to a straight line:
%    l: (x-x1)/X == (y-y1)/Y == (z-z1)/Z; P: (x0,y0,z0)
%    d = sqrt((det([x0-x1,y0-y1;X,Y])^2+det([y0-y1,z0-z1;Y,Z])^2+det([z0-z1,x0-x1;Z,X])^2)/...
%        (X^2+Y^2+Z^2));
%
%   See also VARLIST, VARLIST2, SPHEREREPULSIONCOLLIDER, SPHERECOLLIDERST

%   Copyright Phymhan Studio.
%   $Revision: 0.2.5 $  $Date: 27-Mar-2013 14:55:00 $
%

%% PARAMETER
%Constants
G_newton  = 6.6738480e-11; %Newtonian constant of gravitation
g_grav    = 9.80665;       %Gravitational acceleration
k_coulomb = 8.987551787e9; %Coulomb's constant
Cr        = 1;             %Coefficient of restitution
dflt_axis_lim = [-10,10;-10,10;-10,10];

%Default Values
n_arg_in = nargin;
switch n_arg_in
    case 0
        var_add = {};
    case 1
        var_add = {balls};
    case 2
        var_add = {balls,ball_pos};
    otherwise
        var_add = {balls,ball_pos,ball_v};
end
k_var = 1;
while k_var <= length(var_add)
    if ischar(var_add{k_var})
        break
    end
    k_var = k_var+1;
end
varargin = [var_add(k_var:end) varargin];
switch k_var
    case 1 %n_arg_in == 0 OR arguments start from propertyname
        balls = [1 1 0;1 1 0;1 1 0;1 1 0;1 1 0];
        ball_pos = [-8 -1 0;0 0 0;0 -4 0;6 2*sqrt(3) 0;6 4*sqrt(3) 0];
        ball_v = [2.5 0 0;0 0 0;0 0 0;0 0 0;0 0 0];
        N = size(balls,1);
    case 2
        N = size(balls,1);
        ball_pos = zeros(N,3);
        rmax = max(balls(:,1))+0.5;
        lx = dflt_axis_lim(1,1)+1+rmax:2*rmax:dflt_axis_lim(1,2)-1-rmax;
        ly = dflt_axis_lim(2,1)+1+rmax:2*rmax:dflt_axis_lim(2,2)-1-rmax;
        lz = dflt_axis_lim(3,1)+1+rmax:2*rmax:dflt_axis_lim(3,2)-1-rmax;
        [mx,my,mz] = meshgrid(lx,ly,lz);
        for kk = 1:N
            ball_pos(kk,:) = [mx(kk),my(kk),mz(kk)];
        end
        %random velocity in 3D
        ball_v = 2.5*rand(N,3);
    case 3
        N = size(balls,1);
        %random velocity in 3D
        ball_v = 2.5*rand(N,3);
    otherwise
        N = size(balls,1);
end
%Properties
ball_color = repmat([0 0.5 1 0.75],N,1);
pln_pnt = cell(1,6); %{[pnt1;pnt2;pnt3;pnt4]}
pln_pnt{1} = [-10 -10 10;-10 10 10;10 10 10;10 -10 10];
pln_pnt{2} = [-10 -10 -10;-10 10 -10;10 10 -10;10 -10 -10];
pln_pnt{3} = [-10 -10 -10;-10 -10 10;-10 10 10;-10 10 -10];
pln_pnt{4} = [10 -10 -10;10 -10 10;10 10 10;10 10 -10];
pln_pnt{5} = [-10 10 -10;-10 10 10;10 10 10;10 10 -10];
pln_pnt{6} = [-10 -10 -10;-10 -10 10;10 -10 10;10 -10 -10];
wall_pln = [0 0 1 10;0 0 1 -10;1 0 0 -10;1 0 0 10;0 1 0 -10;0 1 0 10];
wall_sph = [];
GravityVec  = [0 0 -1];
GravitySrc  = [];
CoulombSrc  = [];
GroundPoint = [0 0 0];
grav_ball = 0;
friction_factor = 0;
axis_lim = dflt_axis_lim;
dt = 0.1;
tmax = 100;
t_fig = 50;
debug_mode = true;
play_beep = false;
dflt_layout = '2d';
show_legend = false;
close_requested = 'none';
varlen = n_arg_in-k_var+1;
if rem(varlen,2) == 1
    fprintf('Not enough input arguments.\nValue of "%s" is not specified.\n',...
        varargin{end})
    return
end
for kk = 1:2:varlen
    switch upper(varargin{kk})
        case {'BALLS','BALL','BALL_PROPERTY','BALL_PROPERTIES',...
                'PROPERTY','PROPERTIES','PROP'}
            balls = varargin{kk+1};
        case {'BALL_POS','POS','POSITION'}
            ball_pos = varargin{kk+1};
        case {'BALL_V','BALL_VELOCITY','BALL_VELOCITIES',...
                'VELOCITY','VELOCITIES','V'}
            ball_v = varargin{kk+1};
        case {'BALL_COLOR','BALL_COLORS','COLOR','COLORS'}
            ball_color = varargin{kk+1};
        case {'WALL_PLANE','PLANE'}
            wall_pln = varargin{kk+1};
        case {'WALL_SPHERE','SPHERE'}
            wall_sph = varargin{kk+1};
        case 'GRAVITYVEC'
            GravityVec = varargin{kk+1};
        case {'GRAVITYSRC','GRAVITY SRC','GRAVITY_SRC',...
                'GRAVITYSOURCE','GRAVITY SOURCE','GRAVITY_SOURCE',...
                'GRAVSRC','GRAV SRC','GRAV_SRC'}
            GravitySrc = varargin{kk+1};
        case {'COULOMBSRC','COULOMB SRC','COULOMB_SRC',...
                'COULOMBSOURCE','COULOMB SOURCE','COULOMB_SOURCE'}
            CoulombSrc = varargin{kk+1};
        case {'GROUNDPOINT','GROUND POINT','GROUND_POINT'}
            GroundPoint = varargin{kk+1};
        case {'GRAV_BALL','BALL_GRAV','GRAV BALL','BALL GRAV',...
                'GRAVBALL','BALLGRAV'}
            if ischar(varargin{kk+1})
                switch upper(varargin{kk+1})
                    case 'ON'
                        grav_ball = 1;
                    case 'OFF'
                        grav_ball = 0;
                    otherwise
                        fprintf('''%s'' is not a property value of "debug_mode".\n',...
                            varargin{kk+1})
                        return
                end
            else
                grav_ball = 1;
            end
        case 'DT'
            dt = varargin{kk+1};
        case {'TMAX','T'}
            tmax = varargin{kk+1};
        case {'T_FIG','T FIG','TMAX_FIG','TMAX FIG','TFIG','TMAXFIG'}
            t_fig = varargin{kk+1};
        case {'DEBUG_MODE','DEBUG MODE','DEBUGMODE','DEBUG'}
            if ischar(varargin{kk+1})
                switch upper(varargin{kk+1})
                    case 'ON'
                        debug_mode = true;
                    case 'OFF'
                        debug_mode = false;
                    otherwise
                        fprintf('''%s'' is not a property value of "debug_mode".\n',...
                            varargin{kk+1})
                        return
                end
            else
                debug_mode = varargin{kk+1};
            end
        case 'G_NEWTON'
            G_newton = varargin{kk+1};
        case 'G_GRAV'
            g_grav = varargin{kk+1};
        case 'G'
            switch varargin{kk}
                case 'G'
                    G_newton = varargin{kk+1};
                case 'g'
                    g_grav = varargin{kk+1};
            end
        case 'K_COULOMB'
            k_coulomb = varargin{kk+1};
        case 'K'
            switch varargin{kk}
                case 'k'
                    k_coulomb = varargin{kk+1};
                case 'K'
                    fprintf('"K" is not a property name.\nDid you mean "k"?\n')
                    return
            end
        case {'CR','E'}
            Cr = varargin{kk+1};
        case {'FRICTION','FRICTION_FACTOR','FRICTION FACTOR','FRICT'}
            friction_factor = varargin{kk+1};
        case {'PLAY_BEEP','BEEP'}
            if ischar(varargin{kk+1})
                switch upper(varargin{kk+1})
                    case 'ON'
                        play_beep = true;
                    case 'OFF'
                        play_beep = false;
                    otherwise
                        fprintf('''%s'' is not a property value of "play_beep".\n',...
                            varargin{kk+1})
                        return
                end
            else
                play_beep = varargin{kk+1};
            end
        case {'DFLT_LAYOUT','DFLT LAYOUT','DEFAULT_LAYOUT',...
                'DEFAULT LAYOUT','LAYOUT'}
            if ischar(varargin{kk+1})
                switch upper(varargin{kk+1})
                    case {'2D','3D'}
                        dflt_layout = varargin{kk+1};
                    otherwise
                        fprintf('''%s'' is not a property value of "dflt_layout".\n',...
                            varargin{kk+1})
                        return
                end
            else
                fprintf('''%f'' is not a property value of "show_legend".\n',...
                    varargin{kk+1})
                return
            end
        case {'SHOW_LEGEND','LEGEND'}
            if ischar(varargin{kk+1})
                switch upper(varargin{kk+1})
                    case 'ON'
                        show_legend = true;
                    case 'OFF'
                        show_legend = false;
                    otherwise
                        fprintf('''%s'' is not a property value of "show_legend".\n',...
                            varargin{kk+1})
                        return
                end
            else
                show_legend = varargin{kk+1};
            end
        case {'AXIS_LIM','AXIS_LIMIT','LIM','LIMIT','AXIS'}
            if ischar(varargin{kk+1})
                switch upper(varargin{kk+1})
                    case {'AUTO','CALC'}
                        axis_lim = 'AUTO';
                    case {'DEFAULT','DFLT'}
                        axis_lim = 'DEFAULT';
                    otherwise
                        fprintf('''%s'' is not a property value of "axis_lim".\n',...
                            varargin{kk+1})
                        return
                end
            else
                axis_lim = varargin{kk+1};
            end
        otherwise
            fprintf('''%s'' is not a property name.\n',varargin{kk})
            return
    end
end
switch upper(dflt_layout)
    case '2D'
        ball_v(:,3) = 0;
    case '3D'
        if ball_v(:,3) == 0
            ball_v(:,3) = 2.5*rand(N,1);
        end
end
if ~debug_mode && show_legend
    fprintf('The property "show_legend" is only used in debug mode.\n')
end
%Variables
tlen = tmax/dt;
tlen_fig = t_fig/dt;
cldmax = 25000;
n_pln = size(wall_pln,1);
n_sph = size(wall_sph,1);
n_grav = size(GravitySrc,1);
n_clmb = size(CoulombSrc,1);
if ischar(axis_lim)
    switch axis_lim
        case 'AUTO'
            axis_lim = find_axislim;
        case 'DEFAULT'
            axis_lim = dflt_axis_lim;
    end
elseif isvector(axis_lim)
    axis_lim = reshape(axis_lim,2,3)';
end
%% MODIFY
if n_pln ~= 0
    plane_magn = sqrt(sum(wall_pln(:,1:3).^2,2));
    wall_pln = wall_pln./repmat(plane_magn,1,4); %normalize
end
ball_pos(:,4) = 1;
ball_pos_new = ball_pos;
ball_v_new = ball_v;
GravityVec = GravityVec/sqrt(GravityVec*GravityVec'); %normalize
GravityField = g_grav*GravityVec;
GravityPlane = [GravityVec -GravityVec*GroundPoint'];
%CHECK
[~,kc] = check_overlap;
if kc > 0
    fprintf(['ERROR: There is an overlap in the current sphere layout.'...
        '\nPlease check ''ball_pos'' and try again.\n\n'])
    return
end

%% PLOT and SIMULATION
%Generate Figure and Axes
t_dura = max([min([10^(round(log10(dt/N^2))),0.01]),1e-4]);
h_f = figure('name','Sphere Collider','menubar','none','numbertitle','off',...
    'position',[30 270 600 450],'color',[1 1 1],...
    'closerequestfcn',@my_closereq);
h_a = axes('parent',h_f);
set(h_a,'box','on','projection','perspective','dataaspectratio',[1 1 1])
set(h_a,'xlim',axis_lim(1,:),'ylim',axis_lim(2,:),'zlim',axis_lim(3,:))
set(h_a,'xlimmode','manual','ylimmode','manual','zlimmode','manual')
set(h_a,'xgrid','on','ygrid','on','zgrid','on')
xlabel(h_a,'x')
ylabel(h_a,'y')
zlabel(h_a,'z')
rotate3d(h_a)
%Debugging Figure
if debug_mode
    lin_color = [1 0 0;0 1 0;0 0 1;0 1 0;0 0 1;0 0 0];
    lin_style = {'-','-','-',':',':','-'};
    % figure1: Total Momentum and Energy
    % subplot(2,1,1): Momentum, subplot(2,1,2): Energy;
    h_fd1 = figure('name','DEBUGGING: Total Momentum and Energy','menubar','none',...
        'numbertitle','off','position',[728 270 600 450],...
        'closerequestfcn',@my_closereq); %[728 415 600 305]
    h_ad1 = zeros(1,2);
    h_lin1 = zeros(1,6);
    p_total = NaN(tlen_fig,3);
    E_total = NaN(tlen_fig,3);
    E_total(1,:) = total_energy;
    % Emax = max(E_total(1,:));
    % Emin = 0;
    % if Emax == 0
    %     Emax = 10; %!!
    % end
    h_ad1(2) = subplot(2,1,2,'parent',h_fd1,'xlim',[0 dt*tlen_fig]); %,'ylim',[Emin Emax*1.1]
    title(h_ad1(2),'Energy in Total');
    xlabel(h_ad1(2),'time interval')
    ylabel(h_ad1(2),'Energy')
    for dk = 4:6
        h_lin1(dk) = line('xdata',dt*(1:tlen_fig),'ydata',E_total(:,dk-3),'parent',h_ad1(2),...
            'color',lin_color(dk,:),'linestyle',lin_style{dk});
    end %energy
    p_total(1,:) = total_momentum;
    % pmax = max(abs(p_total(1,:)));
    % if pmax == 0
    %     pmax = sqrt(2*Emax);
    % end
    % pmin = -pmax;
    h_ad1(1) = subplot(2,1,1,'parent',h_fd1,'xlim',[0 dt*tlen_fig]); %,'ylim',[pmin pmax]
    title(h_ad1(1),'Momentum in Total')
    xlabel(h_ad1(1),'time interval')
    ylabel(h_ad1(1),'Momentum')
    for dk = 1:3
        h_lin1(dk) = line('xdata',dt*(1:tlen_fig),'ydata',p_total(:,dk),'parent',h_ad1(1),...
            'color',lin_color(dk,:),'linestyle',lin_style{dk});
    end %momentum
    if show_legend
        legend(h_ad1(2),'E_k','E_p','E');
        legend(h_ad1(1),'P_x','P_y','P_z');
    end
end
%Generate Balls
sph_n = 25;
[bx,by,bz] = sphere(sph_n);
xyzdata = cell(1,N); %{[(sph_n+1)x(sph_n+1)x3 matrix]}
sf_h = zeros(1,N);
%Initial xyz_data of balls
for ikb = 1:N
    xyzdata{ikb} = zeros(sph_n+1,sph_n+1,3);
    xyzdata{ikb}(:,:,1) = balls(ikb)*bx;
    xyzdata{ikb}(:,:,2) = balls(ikb)*by;
    xyzdata{ikb}(:,:,3) = balls(ikb)*bz;
    sf_h(ikb) = surface(xyzdata{ikb}(:,:,1),xyzdata{ikb}(:,:,2),xyzdata{ikb}(:,:,3),...
        'FaceColor',ball_color(ikb,1:3),'FaceAlpha',ball_color(ikb,4),...
        'linestyle','none','visible','off','parent',h_a);
end %Initial xyz_data of balls
%Initial plot
for ikb = 1:N
    set(sf_h(ikb),'xdata',xyzdata{ikb}(:,:,1)+ball_pos(ikb,1),...
        'ydata',xyzdata{ikb}(:,:,2)+ball_pos(ikb,2),...
        'zdata',xyzdata{ikb}(:,:,3)+ball_pos(ikb,3),'visible','on')
end %Initial plot
%Plot Walls
%plane
h_plane = zeros(1,n_pln);
for ikw = 1:n_pln
    h_plane(ikw) = patch(pln_pnt{ikw}(:,1),pln_pnt{ikw}(:,2),pln_pnt{ikw}(:,3),'k',...
        'parent',h_a,'facecolor',[0.85 0.4 0.24],'facealpha',0.2,'linestyle','none');
end
%sphere
for ikw = 1:n_sph
    surface(wall_sph(ikw,1)*bx+wall_sph(ikw,2),...
        wall_sph(ikw,1)*by+wall_sph(ikw,3),...
        wall_sph(ikw,1)*bz+wall_sph(ikw,4),...
        'parent',h_a,'facecolor',[0.85 0.4 0.24],'facealpha',0.2,...
        'linestyle','none')
end %plot wall_sphere

%% SIMULATION
%Start Simulation!
dt_pre = dt; % dt_p: time interval for prediction
dt_c = 0;  % dt_c: cumulative time of collision
cnt_t = 1; % counter for time
cnt_c = 0; % counter for collision
cnt_error = 0; % counter for errors
while cnt_c < cldmax && cnt_t < tlen
    cnt_c = cnt_c+1;
    %predict and update position
    ball_pos_new(:,1:3) = ball_pos(:,1:3)+dt_pre*ball_v;
    %Collision Detection
    [dt_int,kc] = check_overlap;
    if dt_int < inf %collide!
        %processing
        %update position and velocity
        dt_c = dt_c+dt_int;
        dt_pre = dt-dt_c;
        ball_pos(:,1:3) = ball_pos(:,1:3)+dt_int*ball_v;
        ball_v(kc,:) = ball_v_new(kc,:);
        if play_beep
            beep
        end
        %ACCELERATION NOT UPDATED
    else %No overlapping
        %#(ONE FRAME)#
        cnt_t = cnt_t+1;
        for kb1 = 1:N
            %update acceleration and velocity
            a = [0 0 0];
            for kb2 = [1:kb1-1,kb1+1:N]
                norm_balls = ball_pos(kb2,1:3)-ball_pos(kb1,1:3); %kb1 -> kb2
                dist_balls = sqrt(norm_balls*norm_balls');
                a_grav_magn = G_newton*balls(kb1,2)*balls(kb2,2)/dist_balls^2;
                a_coulomb_magn = -k_coulomb*balls(kb1,3)*balls(kb2,3)/dist_balls^2; %negative
                norm_balls = norm_balls/dist_balls; %normalize
                a = a+(a_grav_magn*grav_ball+a_coulomb_magn)*norm_balls;
            end %Interaction between balls
            for ksg = 1:n_grav
                norm_src = ball_pos(kb1,1:3)-GravitySrc(ksg,2:4);
                dist_src = sqrt(norm_src*norm_src');
                a_grav_magn = G_newton*balls(kb1,2)*GravitySrc(ksg,1)/dist_src^2;
                norm_src = norm_src/dist_src; %normalize
                a = a+a_grav_magn*norm_src;
            end %Gravity
            for ksc = 1:n_clmb
                norm_src = ball_pos(kb1,1:3)-CoulombSrc(ksc,2:4);
                dist_src = sqrt(norm_src*norm_src');
                a_coulomb_magn = k_coulomb*balls(kb1,3)*CoulombSrc(ksc,1)/dist_src^2;
                norm_src = norm_src/dist_src; %normalize
                a = a+a_coulomb_magn*norm_src;
            end %Coulomb force
            %Gravity Field
            a = a+GravityField;
            a_frict = -friction_factor/balls(kb1,2)*ball_v(kb1,:);
            ball_v(kb1,:) = ball_v(kb1,:)+dt*(a+a_frict);
            %DRAW
            set(sf_h(kb1),'xdata',xyzdata{kb1}(:,:,1)+ball_pos_new(kb1,1),...
                'ydata',xyzdata{kb1}(:,:,2)+ball_pos_new(kb1,2),...
                'zdata',xyzdata{kb1}(:,:,3)+ball_pos_new(kb1,3))
        end %update acceleration and velocity
        %update position
        ball_pos = ball_pos_new;
        pause(t_dura);
        %reset dt_p and dt_c
        dt_pre = dt; %this is important!
        dt_c = 0;
        
        %DEBUG
        if debug_mode
            %test for conservation of momentum
            idx = rem(cnt_t-1,tlen_fig)+1;
            p_total(idx,:) = total_momentum;
            E_total(idx,:) = total_energy;
            for dk = 1:3
                set(h_lin1(dk),'ydata',p_total([idx+1:tlen_fig,1:idx],dk));
            end %momentum
            for dk = 4:6
                set(h_lin1(dk),'ydata',E_total([idx+1:tlen_fig,1:idx],dk-3));
            end %energy
        end
    end
end %Simulation

%% REPORT
if strcmp(close_requested,'abort')
    delete(h_f)
    if debug_mode
        delete(h_fd1)
    end
end
fprintf('Number of ERROR: %d\n',cnt_error)
close_requested = 'done';

%% FUNCTIONS
    function p_total = total_momentum
        p_total = zeros(1,3);
        p_total(1) = balls(:,2)'*ball_v(:,1);
        p_total(2) = balls(:,2)'*ball_v(:,2);
        p_total(3) = balls(:,2)'*ball_v(:,3);
    end %compute total momentum

    function E_total = total_energy
        E_total = zeros(1,3);
        E_total(1) = balls(:,2)'*sum(ball_v.^2,2)/2;
        E_total(2) = balls(:,2)'*abs(ball_pos*GravityPlane')*g_grav;
        E_total(3) = E_total(1)+E_total(2);
    end %compute total energy

    function [dt_int,kc] = check_overlap
        dt_int = inf;
        kc = -1; %unnecessary, actually :)
        for fk1 = 1:N
            %update acceleration
            %detect possible collision between ball and walls
            for fkw = 1:n_pln
                dist_plane = abs(wall_pln(fkw,:)*ball_pos_new(fk1,:)');
                iscross = (wall_pln(fkw,:)*ball_pos_new(fk1,:)')*...
                    (wall_pln(fkw,:)*ball_pos(fk1,:)') < 0; %bug fixed
                if dist_plane < balls(fk1,1) || iscross %!!
                    vn = ball_v(fk1,:)*wall_pln(fkw,1:3)'*wall_pln(fkw,1:3);
                    dist_plane_old = abs(wall_pln(fkw,:)*ball_pos(fk1,:)');
                    dt_int_c = (dist_plane_old-balls(fk1,1))/sqrt(vn*vn');
                    if dt_int_c < dt_int
                        dt_int = dt_int_c;
                        ball_v_new(fk1,:) = ball_v(fk1,:)-(1+Cr)*vn;
                        kc = fk1;
                    end
                end
            end %detect possible collision between ball and walls
            for fkw = 1:n_sph
                sph_norm = ball_pos_new(fk1,1:3)-wall_sph(fkw,2:4);
                sph_norm_old = ball_pos(fk1,1:3)-wall_sph(fkw,2:4);
                dist_sphere = sqrt(sph_norm*sph_norm');
                dist_sphere_old = sqrt(sph_norm_old*sph_norm_old');
                iscross = ((dist_sphere-wall_sph(fkw,1))*...
                    (dist_sphere_old-wall_sph(fkw,1))) < 0; %bug fixed
                if abs(dist_sphere-wall_sph(fkw,1)) < balls(fk1,1) || iscross %!!
                    sph_norm = sph_norm/dist_sphere; %normalize
                    vn = (ball_v(fk1,:)*sph_norm')*sph_norm;
                    dt_int_c = (abs(dist_sphere_old-wall_sph(fkw,1))...
                        -balls(fk1,1))/sqrt(vn*vn');
                    if dt_int_c < dt_int
                        dt_int = dt_int_c;
                        ball_v_new(fk1,:) = ball_v(fk1,:)-(1+Cr)*vn;
                        kc = fk1;
                    end
                end
            end %detect possible collision between ball and walls
            for fk2 = fk1+1:N
                %detect possible collision between balls
                norm_balls = ball_pos_new(fk2,1:3)-ball_pos_new(fk1,1:3); %kb1 -> kb2
                if norm_balls*norm_balls' < (balls(fk1,1)+balls(fk2,1))^2
                    v2_1 = ball_v(fk2,:)-ball_v(fk1,:);         %relative velocity
                    r2_1 = ball_pos(fk2,1:3)-ball_pos(fk1,1:3); %relative position
                    d_sq = (det([r2_1([1,2]);v2_1([1,2])])^2+det([r2_1([2,3]);v2_1([2,3])])^2+...
                        det([r2_1([3,1]);v2_1([3,1])])^2)/(v2_1*v2_1');
                    d_c = sqrt(r2_1*r2_1'-d_sq)-sqrt((balls(fk1,1)+balls(fk2,1))^2-d_sq);
                    dt_int_c = abs(d_c)/sqrt(v2_1*v2_1');
                    %DEBUG
                    if d_sq >= (balls(fk1,1)+balls(fk2,1))^2
                        set(sf_h([fk1,fk2]),'facecolor',[0.75 0.25 0.75])
                        disp('ERROR: "d" >= R1+R2')
                        cnt_error = cnt_error+1;
                    end
                    if dt_int_c < 0
                        set(sf_h([fk1,fk2]),'facecolor',[0.75 0.25 0.75])
                        disp('ERROR: "dt_int_c" < 0')
                        cnt_error = cnt_error+1;
                    end
                    ball_pos_new(fk1,1:3) = ball_pos(fk1,1:3)+dt_int_c*ball_v(fk1,1:3);
                    ball_pos_new(fk2,1:3) = ball_pos(fk2,1:3)+dt_int_c*ball_v(fk2,1:3);
                    norm_balls_c = (ball_pos_new(fk2,1:3)-ball_pos_new(fk1,1:3))/...
                        (balls(fk1,1)+balls(fk2,1)); %normalize
                    if dt_int_c < dt_int
                        dt_int = dt_int_c;
                        vn1 = (ball_v(fk1,:)*norm_balls_c')*norm_balls_c;
                        vn2 = (ball_v(fk2,:)*norm_balls_c')*norm_balls_c;
                        va = (balls(fk1,2)*vn1+balls(fk2,2)*vn2+balls(fk2,2)*...
                            Cr*(vn2-vn1))/(balls(fk1,2)+balls(fk2,2));
                        vb = (balls(fk1,2)*vn1+balls(fk2,2)*vn2+balls(fk1,2)*...
                            Cr*(vn1-vn2))/(balls(fk1,2)+balls(fk2,2));
                        ball_v_new(fk1,:) = ball_v(fk1,:)-vn1+va;
                        ball_v_new(fk2,:) = ball_v(fk2,:)-vn2+vb;
                        kc = [fk1 fk2];
                    end
                end
            end %detect possible collision between balls
        end %loop contains every ball
    end %detect possible collisions

    function axislim = find_axislim
        %axislim: [xmin,xmax;ymin,ymax;zmin,zmax]
        pln_idx = combination_recs(1,n_pln,3);
        pt = zeros(size(pln_idx,1),3);
        for fkw = 1:size(pln_idx,1)
            pt(fkw,:) = -wall_pln(pln_idx(fkw,:),1:3)\wall_pln(pln_idx(fkw,:),4);
        end
        pt(isinf(pt) | isnan(pt)) = 0;
        axislim = [min(pt(:,1)),max(pt(:,1));min(pt(:,2)),max(pt(:,2));...
            min(pt(:,3)),max(pt(:,3))];
    end %compute axis limits

    function c = combination_recs(m,n,r)
        if (n-m+1) < r
            c = [];
            return;
        end
        if r == 1
            c = (m:n)';
            return;
        end
        nCr = factorial(n-m+1)/(factorial(r)*factorial(n-m+1-r));
        c = zeros(nCr,r);
        cstart = 1;
        for fk = m:(n-r+1)
            csub = combination_recs((fk+1),n,(r-1));
            cend = cstart+size(csub,1)-1;
            c(cstart:cend,1) = fk;
            c(cstart:cend,2:r) = csub;
            cstart = cend+1;
        end
    end

    function my_closereq(varargin)
        % User-defined close request function
        % to display a question dialog box
        if strcmp(close_requested,'none') %cnt_c < cldmax && cnt_t < tlen
            selection = questdlg('Abort Simulation?',...
                'Close Figure',...
                'Yes','No','Yes');
            switch selection,
                case 'Yes',
                    close_requested = 'abort';
                    fprintf('Simulation aborted.\nLoops: %d\nTime: %.2f\n',...
                        cnt_c,dt*cnt_t)
                    cnt_c = cldmax;
                case 'No'
                    return
            end
        else %close_requested == 'done'
            delete(h_f)
            if debug_mode
                delete(h_fd1)
            end
        end
    end

end

Contact us