[ VIGRA Homepage | Function Index | Class Index | Namespaces | File List | Main Page ]
vigra/quaternion.hxx | ![]() |
00001 /************************************************************************/ 00002 /* */ 00003 /* Copyright 2004-2010 by Hans Meine und Ullrich Koethe */ 00004 /* */ 00005 /* This file is part of the VIGRA computer vision library. */ 00006 /* The VIGRA Website is */ 00007 /* http://hci.iwr.uni-heidelberg.de/vigra/ */ 00008 /* Please direct questions, bug reports, and contributions to */ 00009 /* ullrich.koethe@iwr.uni-heidelberg.de or */ 00010 /* vigra@informatik.uni-hamburg.de */ 00011 /* */ 00012 /* Permission is hereby granted, free of charge, to any person */ 00013 /* obtaining a copy of this software and associated documentation */ 00014 /* files (the "Software"), to deal in the Software without */ 00015 /* restriction, including without limitation the rights to use, */ 00016 /* copy, modify, merge, publish, distribute, sublicense, and/or */ 00017 /* sell copies of the Software, and to permit persons to whom the */ 00018 /* Software is furnished to do so, subject to the following */ 00019 /* conditions: */ 00020 /* */ 00021 /* The above copyright notice and this permission notice shall be */ 00022 /* included in all copies or substantial portions of the */ 00023 /* Software. */ 00024 /* */ 00025 /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND */ 00026 /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES */ 00027 /* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ 00028 /* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT */ 00029 /* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, */ 00030 /* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING */ 00031 /* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR */ 00032 /* OTHER DEALINGS IN THE SOFTWARE. */ 00033 /* */ 00034 /************************************************************************/ 00035 00036 #ifndef VIGRA_QUATERNION_HXX 00037 #define VIGRA_QUATERNION_HXX 00038 00039 #include "config.hxx" 00040 #include "numerictraits.hxx" 00041 #include "tinyvector.hxx" 00042 #include "matrix.hxx" 00043 #include "mathutil.hxx" 00044 #include <iosfwd> // ostream 00045 00046 00047 namespace vigra { 00048 00049 /** Quaternion class. 00050 00051 Quaternions are mainly used as a compact representation for 3D rotations because 00052 they are much less prone to round-off errors than rotation matrices, especially 00053 when many rotations are concatenated. In addition, the angle/axis interpretation 00054 of normalized quaternions is very intuitive. Read the 00055 <a href="http://en.wikipedia.org/wiki/Quaternion">Wikipedia entry on quaternions</a> 00056 for more information on the mathematics. 00057 00058 See also: \ref QuaternionOperations 00059 */ 00060 template<class ValueType> 00061 class Quaternion { 00062 public: 00063 typedef TinyVector<ValueType, 3> Vector; 00064 00065 /** the quaternion's valuetype 00066 */ 00067 typedef ValueType value_type; 00068 00069 /** reference (return of operator[]). 00070 */ 00071 typedef ValueType & reference; 00072 00073 /** const reference (return of operator[] const). 00074 */ 00075 typedef ValueType const & const_reference; 00076 00077 /** the quaternion's squared norm type 00078 */ 00079 typedef typename NormTraits<ValueType>::SquaredNormType SquaredNormType; 00080 00081 /** the quaternion's norm type 00082 */ 00083 typedef typename NormTraits<ValueType>::NormType NormType; 00084 00085 /** Construct a quaternion with explicit values for the real and imaginary parts. 00086 */ 00087 Quaternion(ValueType w = 0, ValueType x = 0, ValueType y = 0, ValueType z = 0) 00088 : w_(w), v_(x, y, z) 00089 {} 00090 00091 /** Construct a quaternion with real value and imaginary vector. 00092 00093 Equivalent to <tt>Quaternion(w, v[0], v[1], v[2])</tt>. 00094 */ 00095 Quaternion(ValueType w, const Vector &v) 00096 : w_(w), v_(v) 00097 {} 00098 00099 /** Copy constructor. 00100 */ 00101 Quaternion(const Quaternion &q) 00102 : w_(q.w_), v_(q.v_) 00103 {} 00104 00105 /** Copy assignment. 00106 */ 00107 Quaternion & operator=(Quaternion const & other) 00108 { 00109 w_ = other.w_; 00110 v_ = other.v_; 00111 return *this; 00112 } 00113 00114 /** Assign \a w to the real part and set the imaginary part to zero. 00115 */ 00116 Quaternion & operator=(ValueType w) 00117 { 00118 w_ = w; 00119 v_.init(0); 00120 return *this; 00121 } 00122 00123 /** 00124 * Creates a Quaternion which represents the operation of 00125 * rotating around the given axis by the given angle. 00126 * 00127 * The angle should be in the range -pi..3*pi for sensible 00128 * results. 00129 */ 00130 static Quaternion 00131 createRotation(double angle, const Vector &rotationAxis) 00132 { 00133 // the natural range would be -pi..pi, but the reflective 00134 // behavior around pi is too unexpected: 00135 if(angle > M_PI) 00136 angle -= 2.0*M_PI; 00137 double t = VIGRA_CSTD::sin(angle/2.0); 00138 double norm = rotationAxis.magnitude(); 00139 return Quaternion(VIGRA_CSTD::sqrt(1.0-t*t), t*rotationAxis/norm); 00140 } 00141 00142 /** Read real part. 00143 */ 00144 ValueType w() const { return w_; } 00145 /** Access real part. 00146 */ 00147 ValueType &w() { return w_; } 00148 /** Set real part. 00149 */ 00150 void setW(ValueType w) { w_ = w; } 00151 00152 /** Read imaginary part. 00153 */ 00154 const Vector &v() const { return v_; } 00155 /** Access imaginary part. 00156 */ 00157 Vector &v() { return v_; } 00158 /** Set imaginary part. 00159 */ 00160 void setV(const Vector & v) { v_ = v; } 00161 /** Set imaginary part. 00162 */ 00163 void setV(ValueType x, ValueType y, ValueType z) 00164 { 00165 v_[0] = x; 00166 v_[1] = y; 00167 v_[2] = z; 00168 } 00169 00170 ValueType x() const { return v_[0]; } 00171 ValueType y() const { return v_[1]; } 00172 ValueType z() const { return v_[2]; } 00173 ValueType &x() { return v_[0]; } 00174 ValueType &y() { return v_[1]; } 00175 ValueType &z() { return v_[2]; } 00176 void setX(ValueType x) { v_[0] = x; } 00177 void setY(ValueType y) { v_[1] = y; } 00178 void setZ(ValueType z) { v_[2] = z; } 00179 00180 /** Access entry at index (0 <=> w(), 1 <=> v[0] etc.). 00181 */ 00182 value_type & operator[](int index) 00183 { 00184 return (&w_)[index]; 00185 } 00186 00187 /** Read entry at index (0 <=> w(), 1 <=> v[0] etc.). 00188 */ 00189 value_type operator[](int index) const 00190 { 00191 return (&w_)[index]; 00192 } 00193 00194 /** Magnitude. 00195 */ 00196 NormType magnitude() const 00197 { 00198 return VIGRA_CSTD::sqrt((NormType)squaredMagnitude()); 00199 } 00200 00201 /** Squared magnitude. 00202 */ 00203 SquaredNormType squaredMagnitude() const 00204 { 00205 return w_*w_ + v_.squaredMagnitude(); 00206 } 00207 00208 /** Add \a w to the real part. 00209 00210 If the quaternion represents a rotation, the rotation angle is 00211 increased by \a w. 00212 */ 00213 Quaternion &operator+=(value_type const &w) 00214 { 00215 w_ += w; 00216 return *this; 00217 } 00218 00219 /** Add assigment. 00220 */ 00221 Quaternion &operator+=(Quaternion const &other) 00222 { 00223 w_ += other.w_; 00224 v_ += other.v_; 00225 return *this; 00226 } 00227 00228 /** Subtract \a w from the real part. 00229 00230 If the quaternion represents a rotation, the rotation angle is 00231 decreased by \a w. 00232 */ 00233 Quaternion &operator-=(value_type const &w) 00234 { 00235 w_ -= w; 00236 return *this; 00237 } 00238 00239 /** Subtract assigment. 00240 */ 00241 Quaternion &operator-=(Quaternion const &other) 00242 { 00243 w_ -= other.w_; 00244 v_ -= other.v_; 00245 return *this; 00246 } 00247 00248 /** Addition. 00249 */ 00250 Quaternion operator+() const 00251 { 00252 return *this; 00253 } 00254 00255 /** Subtraction. 00256 */ 00257 Quaternion operator-() const 00258 { 00259 return Quaternion(-w_, -v_); 00260 } 00261 00262 /** Multiply assignment. 00263 00264 If the quaternions represent rotations, the rotations of <tt>this</tt> and 00265 \a other are concatenated. 00266 */ 00267 Quaternion &operator*=(Quaternion const &other) 00268 { 00269 value_type newW = w_*other.w_ - dot(v_, other.v_); 00270 v_ = w_ * other.v_ + other.w_ * v_ + cross(v_, other.v_); 00271 w_ = newW; 00272 return *this; 00273 } 00274 00275 /** Multiply all entries with the scalar \a scale. 00276 */ 00277 Quaternion &operator*=(double scale) 00278 { 00279 w_ *= scale; 00280 v_ *= scale; 00281 return *this; 00282 } 00283 00284 /** Divide assignment. 00285 */ 00286 Quaternion &operator/=(Quaternion const &other) 00287 { 00288 (*this) *= conj(other) / squaredNorm(other); 00289 return *this; 00290 } 00291 00292 /** Devide all entries by the scalar \a scale. 00293 */ 00294 Quaternion &operator/=(double scale) 00295 { 00296 w_ /= scale; 00297 v_ /= scale; 00298 return *this; 00299 } 00300 00301 /** Equal. 00302 */ 00303 bool operator==(Quaternion const &other) const 00304 { 00305 return (w_ == other.w_) && (v_ == other.v_); 00306 } 00307 00308 /** Not equal. 00309 */ 00310 bool operator!=(Quaternion const &other) const 00311 { 00312 return (w_ != other.w_) || (v_ != other.v_); 00313 } 00314 00315 /** 00316 * Fill the first 3x3 elements of the given matrix with a 00317 * rotation matrix performing the same 3D rotation as this 00318 * quaternion. If matrix is in column-major format, it should 00319 * be pre-multiplied with the vectors to be rotated, i.e. 00320 * matrix[0][0-3] will be the rotated X axis. 00321 */ 00322 template<class MatrixType> 00323 void fillRotationMatrix(MatrixType &matrix) const 00324 { 00325 // scale by 2 / norm 00326 typename NumericTraits<ValueType>::RealPromote s = 00327 2 / (typename NumericTraits<ValueType>::RealPromote)squaredMagnitude(); 00328 00329 Vector 00330 vs = v_ * s, 00331 wv = w_ * vs, 00332 vv = vs * v_; 00333 value_type 00334 xy = vs[0] * v_[1], 00335 xz = vs[0] * v_[2], 00336 yz = vs[1] * v_[2]; 00337 00338 matrix[0][0] = 1 - (vv[1] + vv[2]); 00339 matrix[0][1] = ( xy - wv[2]); 00340 matrix[0][2] = ( xz + wv[1]); 00341 00342 matrix[1][0] = ( xy + wv[2]); 00343 matrix[1][1] = 1 - (vv[0] + vv[2]); 00344 matrix[1][2] = ( yz - wv[0]); 00345 00346 matrix[2][0] = ( xz - wv[1]); 00347 matrix[2][1] = ( yz + wv[0]); 00348 matrix[2][2] = 1 - (vv[0] + vv[1]); 00349 } 00350 00351 void fillRotationMatrix(Matrix<value_type> &matrix) const 00352 { 00353 // scale by 2 / norm 00354 typename NumericTraits<ValueType>::RealPromote s = 00355 2 / (typename NumericTraits<ValueType>::RealPromote)squaredMagnitude(); 00356 00357 Vector 00358 vs = v_ * s, 00359 wv = w_ * vs, 00360 vv = vs * v_; 00361 value_type 00362 xy = vs[0] * v_[1], 00363 xz = vs[0] * v_[2], 00364 yz = vs[1] * v_[2]; 00365 00366 matrix(0, 0) = 1 - (vv[1] + vv[2]); 00367 matrix(0, 1) = ( xy - wv[2]); 00368 matrix(0, 2) = ( xz + wv[1]); 00369 00370 matrix(1, 0) = ( xy + wv[2]); 00371 matrix(1, 1) = 1 - (vv[0] + vv[2]); 00372 matrix(1, 2) = ( yz - wv[0]); 00373 00374 matrix(2, 0) = ( xz - wv[1]); 00375 matrix(2, 1) = ( yz + wv[0]); 00376 matrix(2, 2) = 1 - (vv[0] + vv[1]); 00377 } 00378 00379 protected: 00380 ValueType w_; 00381 Vector v_; 00382 }; 00383 00384 template<class T> 00385 struct NormTraits<Quaternion<T> > 00386 { 00387 typedef Quaternion<T> Type; 00388 typedef typename NumericTraits<T>::Promote SquaredNormType; 00389 typedef typename SquareRootTraits<SquaredNormType>::SquareRootResult NormType; 00390 }; 00391 00392 00393 /** \defgroup QuaternionOperations Quaternion Operations 00394 */ 00395 //@{ 00396 00397 /// Create conjugate quaternion. 00398 template<class ValueType> 00399 Quaternion<ValueType> conj(Quaternion<ValueType> const & q) 00400 { 00401 return Quaternion<ValueType>(q.w(), -q.v()); 00402 } 00403 00404 /// Addition. 00405 template<typename Type> 00406 inline Quaternion<Type> 00407 operator+(const Quaternion<Type>& t1, 00408 const Quaternion<Type>& t2) 00409 { 00410 return Quaternion<Type>(t1) += t2; 00411 } 00412 00413 /// Addition of a scalar on the right. 00414 template<typename Type> 00415 inline Quaternion<Type> 00416 operator+(const Quaternion<Type>& t1, 00417 const Type& t2) 00418 { 00419 return Quaternion<Type>(t1) += t2; 00420 } 00421 00422 /// Addition of a scalar on the left. 00423 template<typename Type> 00424 inline Quaternion<Type> 00425 operator+(const Type& t1, 00426 const Quaternion<Type>& t2) 00427 { 00428 return Quaternion<Type>(t1) += t2; 00429 } 00430 00431 /// Subtraction. 00432 template<typename Type> 00433 inline Quaternion<Type> 00434 operator-(const Quaternion<Type>& t1, 00435 const Quaternion<Type>& t2) 00436 { 00437 return Quaternion<Type>(t1) -= t2; 00438 } 00439 00440 /// Subtraction of a scalar on the right. 00441 template<typename Type> 00442 inline Quaternion<Type> 00443 operator-(const Quaternion<Type>& t1, 00444 const Type& t2) 00445 { 00446 return Quaternion<Type>(t1) -= t2; 00447 } 00448 00449 /// Subtraction of a scalar on the left. 00450 template<typename Type> 00451 inline Quaternion<Type> 00452 operator-(const Type& t1, 00453 const Quaternion<Type>& t2) 00454 { 00455 return Quaternion<Type>(t1) -= t2; 00456 } 00457 00458 /// Multiplication. 00459 template<typename Type> 00460 inline Quaternion<Type> 00461 operator*(const Quaternion<Type>& t1, 00462 const Quaternion<Type>& t2) 00463 { 00464 return Quaternion<Type>(t1) *= t2; 00465 } 00466 00467 /// Multiplication with a scalar on the right. 00468 template<typename Type> 00469 inline Quaternion<Type> 00470 operator*(const Quaternion<Type>& t1, 00471 double t2) 00472 { 00473 return Quaternion<Type>(t1) *= t2; 00474 } 00475 00476 /// Multiplication with a scalar on the left. 00477 template<typename Type> 00478 inline Quaternion<Type> 00479 operator*(double t1, 00480 const Quaternion<Type>& t2) 00481 { 00482 return Quaternion<Type>(t1) *= t2; 00483 } 00484 00485 /// Division 00486 template<typename Type> 00487 inline Quaternion<Type> 00488 operator/(const Quaternion<Type>& t1, 00489 const Quaternion<Type>& t2) 00490 { 00491 return Quaternion<Type>(t1) /= t2; 00492 } 00493 00494 /// Division by a scalar. 00495 template<typename Type> 00496 inline Quaternion<Type> 00497 operator/(const Quaternion<Type>& t1, 00498 double t2) 00499 { 00500 return Quaternion<Type>(t1) /= t2; 00501 } 00502 00503 /// Division of a scalar by a Quaternion. 00504 template<typename Type> 00505 inline Quaternion<Type> 00506 operator/(double t1, 00507 const Quaternion<Type>& t2) 00508 { 00509 return Quaternion<Type>(t1) /= t2; 00510 } 00511 00512 /// squared norm 00513 template<typename Type> 00514 inline 00515 typename Quaternion<Type>::SquaredNormType 00516 squaredNorm(Quaternion<Type> const & q) 00517 { 00518 return q.squaredMagnitude(); 00519 } 00520 00521 /// norm 00522 template<typename Type> 00523 inline 00524 typename Quaternion<Type>::NormType 00525 abs(Quaternion<Type> const & q) 00526 { 00527 return norm(q); 00528 } 00529 00530 //@} 00531 00532 } // namespace vigra 00533 00534 namespace std { 00535 00536 template<class ValueType> 00537 inline 00538 ostream & operator<<(ostream & os, vigra::Quaternion<ValueType> const & q) 00539 { 00540 os << q.w() << " " << q.x() << " " << q.y() << " " << q.z(); 00541 return os; 00542 } 00543 00544 template<class ValueType> 00545 inline 00546 istream & operator>>(istream & is, vigra::Quaternion<ValueType> & q) 00547 { 00548 ValueType w, x, y, z; 00549 is >> w >> x >> y >> z; 00550 q.setW(w); 00551 q.setX(x); 00552 q.setY(y); 00553 q.setZ(z); 00554 return is; 00555 } 00556 00557 } // namespace std 00558 00559 #endif // VIGRA_QUATERNION_HXX
© Ullrich Köthe (ullrich.koethe@iwr.uni-heidelberg.de) |
html generated using doxygen and Python
|