Clean up physics code, ready to be merged back