Refreshed collision algo. But still needs improvements in corner cases