https://arnoldpretoriusuct.github.io/Introduction-to-Robotics/Notes/HTML Exports/MEC4127F_Chapter2_RigidBodyMotion.html