Sorted everything so the order in physics2d.cpp corresponds to the order in the header file, then added some functions for mass etc. Still have to add internal functions declarations at the top and make the old-ish code use the new mass functions (requires refactoring that is hinted at in the TODO comments already).