00001 //============================================================================= 00002 /*! return the imaginary part */ 00003 inline vec im(const quat& q) 00004 { 00005 return vec( q.x, q.y, q.z ); 00006 } 00007 00008 //============================================================================= 00009 /*! return vec from quat (|vec|=theta) */ 00010 inline vec q2vt(const quat& q) 00011 { 00012 double sin_theta_half; 00013 double theta( 2.*std::acos(q.r) ); 00014 00015 if(theta<M_PI){ 00016 sin_theta_half =std::sin(0.5*theta); 00017 } 00018 else{ 00019 theta -=2.*M_PI; 00020 sin_theta_half =-std::sin(0.5*theta); 00021 } 00022 00023 return vec( theta*q.x/sin_theta_half, 00024 theta*q.y/sin_theta_half, 00025 theta*q.z/sin_theta_half ); 00026 } 00027 00028 /////////////////////////////////////////////////////////////////////////////// 00029 /////////////////////////////////////////////////////////////////////////////// 00030 /////////////////////////////////////////////////////////////////////////////// 00031 00032 //============================================================================= 00033 /*! return the conjugate quat */ 00034 inline quat conj(const quat& q) 00035 { 00036 return quat( -q.x, -q.y, -q.z, q.r ); 00037 } 00038 00039 /////////////////////////////////////////////////////////////////////////////// 00040 /////////////////////////////////////////////////////////////////////////////// 00041 /////////////////////////////////////////////////////////////////////////////// 00042 00043 //============================================================================= 00044 /*! swap two quats */ 00045 inline void swap(quat& q1, quat& q2) 00046 { 00047 quat _q1(q1); 00048 q1 = q2; 00049 q2 =_q1; 00050 }