Full implementation of collision detection

Fully implemented the collision detection algorithm, it seems to work
based on the testing I've done thus far