function BR_test_bounded_motion
%

x_min = -6.25;
x_max = 6.25;
y_min = -6.25;
y_max = 6.25;
z_min = -6.25;
z_max = 6.25;
MIN_Y = y_min; %-6.25;
MAX_Y = y_max % 6.25;
MIN_X = x_min; %-6.25;
MAX_X = x_max; %6.25;
MIN_Z = z_min; %-6.25;
MAX_Z = z_max; %6.25;

box = [x_min,y_max,z_min; x_max,y_max,z_min;...
    x_max,y_min,z_min; x_min,y_min,z_min;...
    x_min,y_max,z_max; x_max,y_max,z_max;...
    x_max,y_min,z_max; x_min,y_min,z_max];

faces = zeros(6,3);
faces(1:3,:) = [MIN_X,MIN_Y,MIN_Z;MAX_X,MIN_Y,MIN_Z;MIN_X,MAX_Y,MIN_Z];
faces(4:6,:) = [MIN_X,MIN_Y,MIN_Z;MAX_X,MIN_Y,MIN_Z;MIN_X,MIN_Y,MAX_Z];
faces(7:9,:) = [MAX_X,MIN_Y,MIN_Z;MAX_X,MAX_Y,MIN_Z;MAX_X,MIN_Y,MAX_Z];
faces(10:12,:) = [MIN_X,MAX_Y,MIN_Z;MAX_X,MAX_Y,MIN_Z;MAX_X,MAX_Y,MAX_Z];
faces(13:15,:) = [MIN_X,MIN_Y,MIN_Z;MAX_X,MAX_Y,MIN_Z;MIN_X,MIN_Y,MAX_Z];
faces(16:18,:) = [MIN_X,MIN_Y,MAX_Z;MAX_X,MIN_Y,MAX_Z;MIN_X,MAX_Y,MAX_Z];

normals = zeros(6,3);
normals(1,:) = [0,0,1];
normals(2,:) = [0,1,0];
normals(3,:) = [-1,0,0];
normals(4,:) = [0,-1,0];
normals(5,:) = [1,0,0];
normals(6,:) = [0,0,-1];

p0 = [0,0,0];
v0 = [1,0,0];
del_t = 0.1;
[pf,vf] = BR_bounded_motion3D(box,p0,v0,del_t,faces,normals);

p0 = [-6,0,-6];
v0 = [1,0,-3];
del_t = 0.1;
[pf,vf] = BR_bounded_motion3D(box,p0,v0,del_t,faces,normals);
tch = 0;
