00001 // This is core/vnl/vnl_quaternion.h 00002 #ifndef vnl_quaternion_h_ 00003 #define vnl_quaternion_h_ 00004 #ifdef VCL_NEEDS_PRAGMA_INTERFACE 00005 #pragma interface 00006 #endif 00007 //: 00008 // \file 00009 // \brief Unit quaternion represents rotation in 3D. 00010 // \author awf@robots.ox.ac.uk 00011 // \date 16 Mar 00 00012 // 00013 // \verbatim 00014 // Modifications 00015 // 20-05-2000 fsm. changed FLOAT to T since gcc will barf at 00016 // the very reasonable forward declaration 00017 // template <class FLOAT> class vnl_quaternion; 00018 // 23-3-2001 LSB (Manchester) Tidied documentation 00019 // 13-1-2003 Peter Vanroose - removed unimplemented method rotation_matrix() 00020 // 20-2-2006 Ian Scott - Added conversion to from Euler angles 00021 // 06-5-2006 Peter Vanroose - replaced all vnl_vector by vnl_vector_fixed 00022 // \endverbatim 00023 00024 #include <vnl/vnl_vector_fixed.h> 00025 #include <vnl/vnl_matrix_fixed.h> 00026 #include <vcl_iostream.h> 00027 00028 //: 4-element vector that represents rotation in 3D. 00029 // vnl_quaternion is a 4-element vector with 1 real and 3 imaginary 00030 // components: 00031 // \code 00032 // q = r + (i*x + j*y + k*z) 00033 // r = cos(theta/2) 00034 // (x, y, z) = sin(theta/2) (kx, ky, kz) 00035 // \endcode 00036 // where theta and k are respectively the angle and axis of rotation. 00037 // 00038 // 3D vectors can be thought of as pure imaginary quaternions, and so a 00039 // quaternion is represented as a vnl_vector_fixed<T,4> with the imaginary 00040 // part before the real part for 1-1 alignment. 00041 // 00042 // Unit quaternions (i.e., for which $x^2 + y^2 + z^2 + r^2 = 1$) 00043 // provide a more efficient representation for rotation 00044 // than the usual orthonormal matrix that has nine 00045 // parameters and six orthonormal constraints. The unit 00046 // quaternion has only one unit magnitude constraint. Composing 00047 // rotations with quaternions results in fewer multiplications 00048 // and less error. To insure valid rotation results, the 00049 // nearest unit quaternion is computed, and this is much easier 00050 // than finding the nearest orthonormal matrix. Transforming 00051 // vectors with a quaternion requires more operations compared 00052 // to multiplication with the equivalent orthonormal matrix. 00053 // 00054 // \sa 00055 // vnl_vector_fixed and vnl_matrix_fixed for basic operations on vectors and matrices. 00056 // \sa 00057 // Envelope for envelope-letter scheme that avoids deep copy on 00058 // return by value in arithmetic expressions like: q1 * q2 * q3 *... 00059 // 00060 00061 export template <class T> 00062 class vnl_quaternion : public vnl_vector_fixed<T, 4> 00063 { 00064 typedef vnl_vector_fixed<T,4> Base; 00065 public: 00066 00067 //: Constructor for null quaternion 00068 vnl_quaternion() {} 00069 00070 //: Construct quaternion from components x,y,z,r 00071 vnl_quaternion(T x, T y, T z, T r); 00072 00073 //: Construct quaternion from Euler Angles, 00074 // That is a rotation about the X axis, followed by Y, followed by 00075 // the Z axis, using a fixed reference frame. 00076 vnl_quaternion(T theta_X, T theta_Y, T theta_Z); 00077 00078 //: Construct quaternion from axis and angle of rotation. 00079 // \note If you specify an angle in [0, 2pi], then methods angle() and axis() will return the same values. 00080 // However, if you specify an angle in [-2pi, 0], then methods angle() and axis() will return values with opposite signs. 00081 // \sa vnl_quaternion::angle() 00082 // \sa vnl_quaternion::axis() 00083 vnl_quaternion(vnl_vector_fixed<T,3> const& axis, double angle); 00084 00085 //: Construct quaternion from 3x3 row-major matrix 00086 explicit vnl_quaternion(vnl_matrix_fixed<T,3,3> const& transform); 00087 00088 //: Construct quaternion from a 3D vector 00089 vnl_quaternion(vnl_vector_fixed<T,3> const& vec); 00090 00091 //: Construct quaternion from a 4D vector 00092 vnl_quaternion (vnl_vector_fixed<T,4> const& vec); 00093 00094 //: Copy constructor -- Creates a copy of from quaternion. 00095 inline vnl_quaternion(vnl_quaternion<T> const& from) : Base(from) {} 00096 00097 //: Free internal array 00098 inline ~vnl_quaternion() {} // vnl_vector_fixed will free data array 00099 00100 //: Overloads assignment operator to copy rhs quaternion into lhs quaternion. 00101 inline vnl_quaternion& operator= (vnl_quaternion<T> const& rhs) { Base::operator=(rhs); return *this; } 00102 00103 //: Imaginary component, parallel to axis of rotation. 00104 // Use this accessor to both get and set the component. 00105 inline T& x() { return this->operator()(0); } 00106 //: Imaginary component, parallel to axis of rotation. 00107 // Use this accessor to both get and set the component. 00108 inline T& y() { return this->operator()(1); } 00109 //: Imaginary component, parallel to axis of rotation. 00110 // Use this accessor to both get and set the component. 00111 inline T& z() { return this->operator()(2); } 00112 //: Real component. 00113 // Use this accessor to both get and set the component. 00114 inline T& r() { return this->operator()(3); } 00115 00116 //: Imaginary component, parallel to axis of rotation. 00117 // Use this accessor to get the component. 00118 inline T x() const { return this->operator()(0); } 00119 //: Imaginary component, parallel to axis of rotation. 00120 // Use this accessor to get the component. 00121 inline T y() const { return this->operator()(1); } 00122 //: Imaginary component, parallel to axis of rotation. 00123 // Use this accessor to get the component. 00124 inline T z() const { return this->operator()(2); } 00125 //: Real component. 00126 // Use this accessor to get the component. 00127 inline T r() const { return this->operator()(3); } 00128 00129 //: Copies and returns the real part. 00130 inline T real() const { return (*this)[3]; } 00131 00132 //: Copies and returns the imaginary part. 00133 inline vnl_vector_fixed<T,3> imaginary() const { return this->extract(3,0); } 00134 00135 //: Axis of rotation. 00136 // \note Axis not well defined for theta==0. In such a case (or if provided axis==(0,0,0)), this function returns (0,0,1). 00137 vnl_vector_fixed<T,3> axis() const; 00138 00139 //: Angle of rotation. 00140 // \note Returned angle lies in [0, 2*pi] 00141 double angle() const; 00142 00143 //: 3x3 rotation matrix 00144 // The orthonormal vectors are the rows of the matrix, not its columns 00145 vnl_matrix_fixed<T,3,3> rotation_matrix_transpose() const; 00146 00147 //: 4x4 rotation matrix 00148 vnl_matrix_fixed<T,4,4> rotation_matrix_transpose_4() const; 00149 00150 //: Same real, opposite img part 00151 vnl_quaternion<T> conjugate() const; 00152 00153 //: Inverse for nonzero quat 00154 vnl_quaternion<T> inverse() const; 00155 00156 vnl_quaternion<T> operator* (vnl_quaternion<T> const&) const; 00157 00158 //: Rotate 3D v 00159 // The quaternion must be normalised first. 00160 vnl_vector_fixed<T,3> rotate(vnl_vector_fixed<T,3> const&) const; 00161 00162 //: Rotation representation in Euler angles. 00163 // The angles returned will be [theta_X,theta_Y,theta_Z] 00164 // where the final rotation is found be first applying theta_X radians 00165 // about the X axis, then theta_Y about the Y-axis, etc. 00166 // The axes stay in a fixed reference frame. 00167 // The quaternion mut be normalised first. 00168 vnl_vector_fixed<T,3> rotation_euler_angles() const; 00169 }; 00170 00171 //: operator<< 00172 // \relatesalso vnl_quaternion 00173 template <class T> 00174 inline vcl_ostream& operator<< (vcl_ostream& os, vnl_quaternion<T> const& q) 00175 { 00176 return os << *((const vnl_vector_fixed<T,4>*) &q); 00177 } 00178 00179 #define VNL_QUATERNION_INSTANTIATE(T) extern "Please #include <vnl/vnl_quaternion.txx> first" 00180 00181 #endif // vnl_quaternion_h_