Moving from using ODE solver to copying Randlov's impl.
Implementing Randlov based off his C code.