Adding collision detection

Still need unit test for this.