Refactored Custom6DOF  to use small angular aproximation