core/vnl/vnl_quaternion.h
Go to the documentation of this file.
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_