Implemented rudimentary collision detection

Added a simple routine for collision detection, has some issues but a
good start point