Did you do all the math yourself?

yup, derived most of the equations myself then double checked them before implementing them. That being said all of the positining is done through time steps and not so much equations