MATLAB collision part done