Bring structure into physics code

[meekee7]

The physics code is lacking structure, often using functions with 1000 LOC or more. Those super-long functions should be divided into one or two layers of smaller functions. Furthermore, the physics code could be shortened enormously be using loops. A lot of code is written in an "unrolled loop" style, but optimizers in compilers are perfectly capable of loop unrolling by themselves.