Fixed various logic problems preventing proper energy conservation for velocity verlet.