Refactored some physics code