OpenGL - Robot arm kinematics 3D simulation