Fix in energy calculation formula

Samo Penic

2018-05-15 2d6875103844f7ff25c36ff0134506fc0aaaaa39

1
Original written by Dan Kroll in FORTRAN. Code was then largely adapted and commented by Miha Fosnaric-Fosni. Rewritten into C by Samo Penic-Peni, with help of Miha's extensive explanations.