Refactoring

clean up the code and unit tests
Add getPenetrationVector for AABB's