Preparing the code for the swept based double body collision.
The collision algorithm is still buggy. Needs to be fixed.