function fermats_room
%FERMATS_ROOM Just a game
%
%% PARAMETER
%Constants
g_grav = 0;
k_coulomb = 0.5;
Cr = 1; %Coefficient of restitution
%Properties
balls = repmat([0.5 1 0],1,1);
ball_pos = [-1 -1 0;-1 1 0;1 1 0;1 -1 0];
N = size(balls,1);
ball_v = 1*rand(N,3);
ball_v(:,3) = 0;
ball_pos = ball_pos(1:N,:);
ball_v = ball_v(1:N,:);
ball_color = repmat([0 0.5 1 0.75],N,1);
w_pln_v = {...
{@(x) 0,@(x) 0,@(x) -0.2},...
{@(x) 0,@(x) 0,@(x) 0.2},...
{@(x) 0.2,@(x) 0,@(x) 0},...
{@(x) -0.2,@(x) 0,@(x) 0},...
{@(x) 0,@(x) -0.2,@(x) 0},...
{@(x) 0,@(x) 0.2,@(x) 0}};
w_sph_v = {};
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_sph = [];
GravityVec = [0 0 -1];
GroundPoint = [0 0 0];
friction_factor = 0;
axis_lim = [-10,10;-10,10;-10,10];
dt = 0.1;
tmax = 100;
debug_mode = true;
%Variables
tlen = tmax/dt;
tlen_fig = 50/dt;
cldmax = 10000;
n_pln = length(pln_pnt);
n_sph = size(wall_sph,1);
wall_pln = generate_walls;
%% 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','Fermat''s Room','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;
fd1_h = figure('name','Momentum and Energy','menubar','none',...
'numbertitle','off','position',[728 270 600 450]); %[728 415 600 305]
ad1_h = zeros(1,2);
lin1_h = zeros(1,6);
p_total = NaN(tlen_fig,3);
E_total = NaN(tlen_fig,3);
E_total(1,:) = total_energy;
ad1_h(2) = subplot(2,1,2,'parent',fd1_h,'xlim',[0 dt*tlen_fig]);
title(ad1_h(2),'Energy in Total');
xlabel(ad1_h(2),'time interval')
ylabel(ad1_h(2),'Energy')
for dk = 4:6
lin1_h(dk) = line('xdata',dt*(1:tlen_fig),'ydata',E_total(:,dk-3),'parent',ad1_h(2),...
'color',lin_color(dk,:),'linestyle',lin_style{dk});
end %energy
p_total(1,:) = total_momentum;
ad1_h(1) = subplot(2,1,1,'parent',fd1_h,'xlim',[0 dt*tlen_fig]);
title(ad1_h(1),'Momentum in Total')
xlabel(ad1_h(1),'time interval')
ylabel(ad1_h(1),'Momentum')
for dk = 1:3
lin1_h(dk) = line('xdata',dt*(1:tlen_fig),'ydata',p_total(:,dk),'parent',ad1_h(1),...
'color',lin_color(dk,:),'linestyle',lin_style{dk});
end %momentum
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
%Sphere
h_sphere = zeros(1,n_sph);
for ikw = 1:n_sph
h_sphere(ikw) = 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.8 0.5 0.25],'facealpha',0.25,...
'linestyle','none');
end %plot wall_sphere
%Plane
h_pln = zeros(1,n_pln);
for ikw = 1:n_pln
h_pln(ikw) = patch(pln_pnt{ikw}(:,1),pln_pnt{ikw}(:,2),pln_pnt{ikw}(:,3),'k',...
'parent',a_h,'facecolor',[0.8 0.5 0.25],'facealpha',0.25,'linestyle','none');
end
%% SIMULATION
%Start Simulation!
dt_p = dt; % dt_p: time interval for prediction
dt_c = 0; % dt_c: cumulative time of collision
t = dt; % time
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_p*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_p = dt-dt_c;
ball_pos(:,1:3) = ball_pos(:,1:3)+dt_int*ball_v;
ball_v(kc,:) = ball_v_new(kc,:);
%ACCELERATION NOT UPDATED
else %No overlapping
%#(ONE FRAME)#
cnt_t = cnt_t+1;
t = t+dt;
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_coulomb_magn = -k_coulomb*balls(kb1,3)*balls(kb2,3)/dist_balls^2; %negative
norm_balls = norm_balls/dist_balls; %normalize
a = a+a_coulomb_magn*norm_balls;
end %Interaction between balls
%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;
%move walls
for kwv = 1:n_pln
vw = [w_pln_v{kwv}{1}(t),w_pln_v{kwv}{2}(t),w_pln_v{kwv}{3}(t)];
pln_pnt{kwv} = pln_pnt{kwv}+([1;1;1;1]*vw)*dt;
set(h_pln(kwv),...
'xdata',pln_pnt{kwv}(:,1),...
'ydata',pln_pnt{kwv}(:,2),...
'zdata',pln_pnt{kwv}(:,3))
wall_pln(kwv,4) = wall_pln(kwv,4)-(vw*wall_pln(kwv,1:3)')*dt;
end
for kwv = 1:n_sph
end
pause(t_dura);
%reset dt_p and dt_c
dt_p = 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(lin1_h(dk),'ydata',p_total([idx+1:tlen_fig,1:idx],dk));
end %momentum
for dk = 4:6
set(lin1_h(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,kc_w] = check_overlap
dt_int = inf;
kc = -1; %unnecessary, actually :)
kc_w = -1;
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 %!!
vbn = ball_v(fk1,:)*wall_pln(fkw,1:3)'*wall_pln(fkw,1:3);
vwn = ([w_pln_v{fkw}{1}(t),w_pln_v{fkw}{2}(t),w_pln_v{fkw}{3}(t)]*...
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((vbn-vwn)*(vbn-vwn)');
if dt_int_c < dt_int
dt_int = dt_int_c;
ball_v_new(fk1,:) = ball_v(fk1,:)-(1+Cr)*vbn+2*vwn;
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
vbn = (ball_v(fk1,:)*sph_norm')*sph_norm;
vwn = ([w_sph_v{fkw}{1}(t),w_sph_v{fkw}{2}(t),w_sph_v{fkw}{3}(t)]*...
sph_norm')*sph_norm;
dt_int_c = (abs(dist_sphere_old-wall_sph(fkw,1))...
-balls(fk1,1))/sqrt((vbn-vwn)*(vbn-vwn)');
if dt_int_c < dt_int
dt_int = dt_int_c;
ball_v_new(fk1,:) = ball_v(fk1,:)-(1+Cr)*vbn+2*vwn;
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 wall_pln = generate_walls
wall_pln = zeros(n_pln,4);
for fkw = 1:n_pln
vec1 = pln_pnt{fkw}(2,:)-pln_pnt{fkw}(1,:);
vec2 = pln_pnt{fkw}(3,:)-pln_pnt{fkw}(1,:);
vecn = cross(vec1,vec2);
vecn = vecn/sqrt(vecn*vecn'); %normalize
wall_pln(fkw,:) = [vecn -vecn*pln_pnt{fkw}(1,:)'];
end
end %generate walls(wall_pln) from plane_pnt
end
