Everything you need to know to code a 6DOF in C++/MATLAB/Python
http://www.aerialsystems.org/p/tutorials.html
https://fastlabtutorials.blogspot.com/2015/03/everything-you-need-to-know-to-code.html?view=classic