function SphereCollider(balls,ball_pos,ball_v,varargin)
%SPHERECOLLIDER Physics engine for multi-sphere collision.
% Althogrim: scan all balls and walls after each 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;
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 = 10000;
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]);
f_h = figure('name','Sphere Collider','menubar','none','numbertitle','off',...
'position',[30 270 600 450],'color',[1 1 1]);
a_h = axes('parent',f_h);
set(a_h,'box','on','projection','perspective','dataaspectratio',[1 1 1])
set(a_h,'xlim',axis_lim(1,:),'ylim',axis_lim(2,:),'zlim',axis_lim(3,:))
set(a_h,'xlimmode','manual','ylimmode','manual','zlimmode','manual')
set(a_h,'xgrid','on','ygrid','on','zgrid','on')
xlabel(a_h,'x')
ylabel(a_h,'y')
zlabel(a_h,'z')
rotate3d(a_h)
%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]); %[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',a_h);
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',a_h,'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',a_h,'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
fprintf('Number of ERRORs: %d\n',cnt_error)
%% 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
end
