Refactoring physics code and adding some tests.