Caclucate Nautical = Cardan angles.

cardan_angles.cpp

//--------------------------------------------------------------

/**
 * @brief   Returns Cardan = roll/pitch/yaw angles for the given
 *          rotation.
 *
 * @param   q_ an ofQuaternion (unit quaternion) denoting a rotation.
 *
 * @details Cardan Angles == Tail-Bryan-A. == Nautical-A. == Euler
 *          Angle Sequence 1,2,3.
 *
 *          The angle sequence is relative to the object's current
 *          reference frame and to be applied in sequence. That is:
 *          roatation first around the object's x-axis, then its new
 *          y'-axis, then its new z''-axis. This is equivalent to the
 *          angle notation you'd expect for a plane or for a ship,
 *          thus the name 'Nautical Angles'.
 *
 *          Math implemented based on: Diebel, James: Representing
 *          Attitude: Euler Angles, Unit Quaternions, and Rotation
 *          Vectors, Stanford University, Stanford, California, 2006,
 *          p. 12.
 *          http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134
 *
 * @warning This method returns undefined results when approaching
 *          pitch PI/2 or -PI/2
 *
 * @author  tig
 */

static ofVec3f getCardanAngles(const ofQuaternion& q_){
	
	// tig: ok, so in math textbooks a quaternion is defined as:
	//
	// q = a + ib + jc + kd
	//
	// whereas openFrameworks stores quaternions as in:
	//
	// q = ix + jy + kz + w
	//
	// therefore, when textbooks uses quaternion indices, these will *NOT*
	// correspond to ofQuaternion's indices, but need to be shifted
	// by 1 to the left, which is what we do in the next step.
	
	float
	q0(q_[3]),	// w -> a
	q1(q_[0]),	// x -> b
	q2(q_[1]),  // y -> c
	q3(q_[2]);  // z -> d
	
	float sq0 = q0*q0;
	float sq1 = q1*q1;
	float sq2 = q2*q2;
	float sq3 = q3*q3;
	
	// we can now use the same terms as in the textbook.
	float roll	=	atan2f(2.0f * q2 * q3 + 2.0f * q0 * q1, sq3 - sq2 - sq1 + sq0);
	float pitch	=	-asin(2.0f * q1 * q3 - 2.0f * q0 * q2);
	float yaw	=	atan2f(2.0f * q1 * q2 + 2.0f * q0 * q3, sq1 + sq0 - sq3 - sq2);
	
	return (ofVec3f(roll,pitch,yaw) * RAD_TO_DEG);
}