Verlet fix + refactoring

Fixed an implementation issue with Verlet integration. Some refactoring.