Code cleanup, physics fixes

In this commit I cleaned some duplicate code and also commented more
parts of library. And hopefully also fixed some glitches.