Implemented SAP in collisionSystem.
Minor cleaning.