Collision check

Collision check seems to be working. Needs a little testing before merging.