Found cause of strange error to be typo in get positions function.
Integration with RK4 works!

Problem was the strange error when rewriting. Now that algorithm also is
easier to read it can be seen to be correctly implemented.