Added energy calulcation and gravitational interaction
More elaborate test with 4 bodies