Further refactoring: energy functions now use the acceleration structure for calculating energy
Updated tests + fixed bug where crossings where counted twice