[ VIGRA Homepage | Function Index | Class Index | Namespaces | File List | Main Page ]

quaternion.hxx
1/************************************************************************/
2/* */
3/* Copyright 2004-2010 by Hans Meine und Ullrich Koethe */
4/* */
5/* This file is part of the VIGRA computer vision library. */
6/* The VIGRA Website is */
7/* http://hci.iwr.uni-heidelberg.de/vigra/ */
8/* Please direct questions, bug reports, and contributions to */
9/* ullrich.koethe@iwr.uni-heidelberg.de or */
10/* vigra@informatik.uni-hamburg.de */
11/* */
12/* Permission is hereby granted, free of charge, to any person */
13/* obtaining a copy of this software and associated documentation */
14/* files (the "Software"), to deal in the Software without */
15/* restriction, including without limitation the rights to use, */
16/* copy, modify, merge, publish, distribute, sublicense, and/or */
17/* sell copies of the Software, and to permit persons to whom the */
18/* Software is furnished to do so, subject to the following */
19/* conditions: */
20/* */
21/* The above copyright notice and this permission notice shall be */
22/* included in all copies or substantial portions of the */
23/* Software. */
24/* */
25/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND */
26/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES */
27/* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */
28/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT */
29/* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, */
30/* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING */
31/* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR */
32/* OTHER DEALINGS IN THE SOFTWARE. */
33/* */
34/************************************************************************/
35
36#ifndef VIGRA_QUATERNION_HXX
37#define VIGRA_QUATERNION_HXX
38
39#include "config.hxx"
40#include "numerictraits.hxx"
41#include "tinyvector.hxx"
42#include "matrix.hxx"
43#include "mathutil.hxx"
44#include <iosfwd> // ostream
45
46
47 namespace vigra {
48
49/** Quaternion class.
50
51 Quaternions are mainly used as a compact representation for 3D rotations because
52 they are much less prone to round-off errors than rotation matrices, especially
53 when many rotations are concatenated. In addition, the angle/axis interpretation
54 of normalized quaternions is very intuitive. Read the
55 <a href="http://en.wikipedia.org/wiki/Quaternion">Wikipedia entry on quaternions</a>
56 for more information on the mathematics.
57
58 See also: \ref QuaternionOperations
59*/
60 template<class ValueType>
61 class Quaternion {
62 public:
63 typedef TinyVector<ValueType, 3> Vector;
64
65 /** the quaternion's valuetype
66 */
67 typedef ValueType value_type;
68
69 /** reference (return of operator[]).
70 */
71 typedef ValueType & reference;
72
73 /** const reference (return of operator[] const).
74 */
75 typedef ValueType const & const_reference;
76
77 /** the quaternion's squared norm type
78 */
79 typedef typename NormTraits<ValueType>::SquaredNormType SquaredNormType;
80
81 /** the quaternion's norm type
82 */
83 typedef typename NormTraits<ValueType>::NormType NormType;
84
85 /** Construct a quaternion with explicit values for the real and imaginary parts.
86 */
87 Quaternion(ValueType w = 0, ValueType x = 0, ValueType y = 0, ValueType z = 0)
88 : w_(w), v_(x, y, z)
89 {}
90
91 /** Construct a quaternion with real value and imaginary vector.
92
93 Equivalent to <tt>Quaternion(w, v[0], v[1], v[2])</tt>.
94 */
95 Quaternion(ValueType w, const Vector &v)
96 : w_(w), v_(v)
97 {}
98
99 /** Copy constructor.
100 */
101 Quaternion(const Quaternion &q)
102 : w_(q.w_), v_(q.v_)
103 {}
104
105 /** Copy assignment.
106 */
107 Quaternion & operator=(Quaternion const & other)
108 {
109 w_ = other.w_;
110 v_ = other.v_;
111 return *this;
112 }
113
114 /** Assign \a w to the real part and set the imaginary part to zero.
115 */
116 Quaternion & operator=(ValueType w)
117 {
118 w_ = w;
119 v_.init(0);
120 return *this;
121 }
122
123 /**
124 * Creates a Quaternion which represents the operation of
125 * rotating around the given axis by the given angle.
126 *
127 * The angle should be in the range -pi..3*pi for sensible
128 * results.
129 */
130 static Quaternion
131 createRotation(double angle, const Vector &rotationAxis)
132 {
133 // the natural range would be -pi..pi, but the reflective
134 // behavior around pi is too unexpected:
135 if(angle > M_PI)
136 angle -= 2.0*M_PI;
137 double t = VIGRA_CSTD::sin(angle/2.0);
138 double norm = rotationAxis.magnitude();
139 return Quaternion(VIGRA_CSTD::sqrt(1.0-t*t), t*rotationAxis/norm);
140 }
141
142 /** Read real part.
143 */
144 ValueType w() const { return w_; }
145 /** Access real part.
146 */
147 ValueType &w() { return w_; }
148 /** Set real part.
149 */
150 void setW(ValueType w) { w_ = w; }
151
152 /** Read imaginary part.
153 */
154 const Vector &v() const { return v_; }
155 /** Access imaginary part.
156 */
157 Vector &v() { return v_; }
158 /** Set imaginary part.
159 */
160 void setV(const Vector & v) { v_ = v; }
161 /** Set imaginary part.
162 */
163 void setV(ValueType x, ValueType y, ValueType z)
164 {
165 v_[0] = x;
166 v_[1] = y;
167 v_[2] = z;
168 }
169
170 ValueType x() const { return v_[0]; }
171 ValueType y() const { return v_[1]; }
172 ValueType z() const { return v_[2]; }
173 ValueType &x() { return v_[0]; }
174 ValueType &y() { return v_[1]; }
175 ValueType &z() { return v_[2]; }
176 void setX(ValueType x) { v_[0] = x; }
177 void setY(ValueType y) { v_[1] = y; }
178 void setZ(ValueType z) { v_[2] = z; }
179
180 /** Access entry at index (0 <=> w(), 1 <=> v[0] etc.).
181 */
182 value_type & operator[](int index)
183 {
184 return (&w_)[index];
185 }
186
187 /** Read entry at index (0 <=> w(), 1 <=> v[0] etc.).
188 */
189 value_type operator[](int index) const
190 {
191 return (&w_)[index];
192 }
193
194 /** Magnitude.
195 */
196 NormType magnitude() const
197 {
198 return VIGRA_CSTD::sqrt((NormType)squaredMagnitude());
199 }
200
201 /** Squared magnitude.
202 */
203 SquaredNormType squaredMagnitude() const
204 {
205 return w_*w_ + v_.squaredMagnitude();
206 }
207
208 /** Add \a w to the real part.
209
210 If the quaternion represents a rotation, the rotation angle is
211 increased by \a w.
212 */
213 Quaternion &operator+=(value_type const &w)
214 {
215 w_ += w;
216 return *this;
217 }
218
219 /** Add assigment.
220 */
221 Quaternion &operator+=(Quaternion const &other)
222 {
223 w_ += other.w_;
224 v_ += other.v_;
225 return *this;
226 }
227
228 /** Subtract \a w from the real part.
229
230 If the quaternion represents a rotation, the rotation angle is
231 decreased by \a w.
232 */
233 Quaternion &operator-=(value_type const &w)
234 {
235 w_ -= w;
236 return *this;
237 }
238
239 /** Subtract assigment.
240 */
241 Quaternion &operator-=(Quaternion const &other)
242 {
243 w_ -= other.w_;
244 v_ -= other.v_;
245 return *this;
246 }
247
248 /** Addition.
249 */
250 Quaternion operator+() const
251 {
252 return *this;
253 }
254
255 /** Subtraction.
256 */
257 Quaternion operator-() const
258 {
259 return Quaternion(-w_, -v_);
260 }
261
262 /** Multiply assignment.
263
264 If the quaternions represent rotations, the rotations of <tt>this</tt> and
265 \a other are concatenated.
266 */
267 Quaternion &operator*=(Quaternion const &other)
268 {
269 value_type newW = w_*other.w_ - dot(v_, other.v_);
270 v_ = w_ * other.v_ + other.w_ * v_ + cross(v_, other.v_);
271 w_ = newW;
272 return *this;
273 }
274
275 /** Multiply all entries with the scalar \a scale.
276 */
277 Quaternion &operator*=(double scale)
278 {
279 w_ *= scale;
280 v_ *= scale;
281 return *this;
282 }
283
284 /** Divide assignment.
285 */
286 Quaternion &operator/=(Quaternion const &other)
287 {
288 (*this) *= conj(other) / squaredNorm(other);
289 return *this;
290 }
291
292 /** Devide all entries by the scalar \a scale.
293 */
294 Quaternion &operator/=(double scale)
295 {
296 w_ /= scale;
297 v_ /= scale;
298 return *this;
299 }
300
301 /** Equal.
302 */
303 bool operator==(Quaternion const &other) const
304 {
305 return (w_ == other.w_) && (v_ == other.v_);
306 }
307
308 /** Not equal.
309 */
310 bool operator!=(Quaternion const &other) const
311 {
312 return (w_ != other.w_) || (v_ != other.v_);
313 }
314
315 /**
316 * Fill the first 3x3 elements of the given matrix with a
317 * rotation matrix performing the same 3D rotation as this
318 * quaternion. If matrix is in column-major format, it should
319 * be pre-multiplied with the vectors to be rotated, i.e.
320 * matrix[0][0-3] will be the rotated X axis.
321 */
322 template<class MatrixType>
323 void fillRotationMatrix(MatrixType &matrix) const
324 {
325 // scale by 2 / norm
326 typename NumericTraits<ValueType>::RealPromote s =
327 2 / (typename NumericTraits<ValueType>::RealPromote)squaredMagnitude();
328
329 Vector
330 vs = v_ * s,
331 wv = w_ * vs,
332 vv = vs * v_;
333 value_type
334 xy = vs[0] * v_[1],
335 xz = vs[0] * v_[2],
336 yz = vs[1] * v_[2];
337
338 matrix[0][0] = 1 - (vv[1] + vv[2]);
339 matrix[0][1] = ( xy - wv[2]);
340 matrix[0][2] = ( xz + wv[1]);
341
342 matrix[1][0] = ( xy + wv[2]);
343 matrix[1][1] = 1 - (vv[0] + vv[2]);
344 matrix[1][2] = ( yz - wv[0]);
345
346 matrix[2][0] = ( xz - wv[1]);
347 matrix[2][1] = ( yz + wv[0]);
348 matrix[2][2] = 1 - (vv[0] + vv[1]);
349 }
350
351 void fillRotationMatrix(Matrix<value_type> &matrix) const
352 {
353 // scale by 2 / norm
354 typename NumericTraits<ValueType>::RealPromote s =
355 2 / (typename NumericTraits<ValueType>::RealPromote)squaredMagnitude();
356
357 Vector
358 vs = v_ * s,
359 wv = w_ * vs,
360 vv = vs * v_;
361 value_type
362 xy = vs[0] * v_[1],
363 xz = vs[0] * v_[2],
364 yz = vs[1] * v_[2];
365
366 matrix(0, 0) = 1 - (vv[1] + vv[2]);
367 matrix(0, 1) = ( xy - wv[2]);
368 matrix(0, 2) = ( xz + wv[1]);
369
370 matrix(1, 0) = ( xy + wv[2]);
371 matrix(1, 1) = 1 - (vv[0] + vv[2]);
372 matrix(1, 2) = ( yz - wv[0]);
373
374 matrix(2, 0) = ( xz - wv[1]);
375 matrix(2, 1) = ( yz + wv[0]);
376 matrix(2, 2) = 1 - (vv[0] + vv[1]);
377 }
378
379 protected:
380 ValueType w_;
381 Vector v_;
382};
383
384 template<class T>
385 struct NormTraits<Quaternion<T> >
386{
387 typedef Quaternion<T> Type;
388 typedef typename NumericTraits<T>::Promote SquaredNormType;
389 typedef typename SquareRootTraits<SquaredNormType>::SquareRootResult NormType;
390};
391
392
393/** \defgroup QuaternionOperations Quaternion Operations
394*/
395//@{
396
397 /// Create conjugate quaternion.
398 template<class ValueType>
399 Quaternion<ValueType> conj(Quaternion<ValueType> const & q)
400{
401 return Quaternion<ValueType>(q.w(), -q.v());
402}
403
404 /// Addition.
405 template<typename Type>
406 inline Quaternion<Type>
407 operator+(const Quaternion<Type>& t1,
408 const Quaternion<Type>& t2)
409{
410 return Quaternion<Type>(t1) += t2;
411}
412
413 /// Addition of a scalar on the right.
414 template<typename Type>
415 inline Quaternion<Type>
416 operator+(const Quaternion<Type>& t1,
417 const Type& t2)
418{
419 return Quaternion<Type>(t1) += t2;
420}
421
422 /// Addition of a scalar on the left.
423 template<typename Type>
424 inline Quaternion<Type>
425 operator+(const Type& t1,
426 const Quaternion<Type>& t2)
427{
428 return Quaternion<Type>(t1) += t2;
429}
430
431 /// Subtraction.
432 template<typename Type>
433 inline Quaternion<Type>
434 operator-(const Quaternion<Type>& t1,
435 const Quaternion<Type>& t2)
436{
437 return Quaternion<Type>(t1) -= t2;
438}
439
440 /// Subtraction of a scalar on the right.
441 template<typename Type>
442 inline Quaternion<Type>
443 operator-(const Quaternion<Type>& t1,
444 const Type& t2)
445{
446 return Quaternion<Type>(t1) -= t2;
447}
448
449 /// Subtraction of a scalar on the left.
450 template<typename Type>
451 inline Quaternion<Type>
452 operator-(const Type& t1,
453 const Quaternion<Type>& t2)
454{
455 return Quaternion<Type>(t1) -= t2;
456}
457
458 /// Multiplication.
459 template<typename Type>
460 inline Quaternion<Type>
461operator*(const Quaternion<Type>& t1,
462 const Quaternion<Type>& t2)
463{
464 return Quaternion<Type>(t1) *= t2;
465}
466
467 /// Multiplication with a scalar on the right.
468 template<typename Type>
469 inline Quaternion<Type>
470operator*(const Quaternion<Type>& t1,
471 double t2)
472{
473 return Quaternion<Type>(t1) *= t2;
474}
475
476 /// Multiplication with a scalar on the left.
477 template<typename Type>
478 inline Quaternion<Type>
479operator*(double t1,
480 const Quaternion<Type>& t2)
481{
482 return Quaternion<Type>(t1) *= t2;
483}
484
485 /// Division
486 template<typename Type>
487 inline Quaternion<Type>
488operator/(const Quaternion<Type>& t1,
489 const Quaternion<Type>& t2)
490{
491 return Quaternion<Type>(t1) /= t2;
492}
493
494 /// Division by a scalar.
495 template<typename Type>
496 inline Quaternion<Type>
497operator/(const Quaternion<Type>& t1,
498 double t2)
499{
500 return Quaternion<Type>(t1) /= t2;
501}
502
503 /// Division of a scalar by a Quaternion.
504 template<typename Type>
505 inline Quaternion<Type>
506operator/(double t1,
507 const Quaternion<Type>& t2)
508{
509 return Quaternion<Type>(t1) /= t2;
510}
511
512 /// squared norm
513 template<typename Type>
514 inline
515 typename Quaternion<Type>::SquaredNormType
516 squaredNorm(Quaternion<Type> const & q)
517{
518 return q.squaredMagnitude();
519}
520
521 /// norm
522 template<typename Type>
523 inline
524 typename Quaternion<Type>::NormType
525abs(Quaternion<Type> const & q)
526{
527 return norm(q);
528}
529
530//@}
531
532} // namespace vigra
533
534 namespace std {
535
536 template<class ValueType>
537 inline
538ostream & operator<<(ostream & os, vigra::Quaternion<ValueType> const & q)
539{
540 os << q.w() << " " << q.x() << " " << q.y() << " " << q.z();
541 return os;
542}
543
544 template<class ValueType>
545 inline
546istream & operator>>(istream & is, vigra::Quaternion<ValueType> & q)
547{
548 ValueType w, x, y, z;
549 is >> w >> x >> y >> z;
550 q.setW(w);
551 q.setX(x);
552 q.setY(y);
553 q.setZ(z);
554 return is;
555}
556
557} // namespace std
558
559#endif // VIGRA_QUATERNION_HXX
vigra::Quaternion
Definition quaternion.hxx:61
vigra::Quaternion::v
Vector & v()
Definition quaternion.hxx:157
vigra::Quaternion::operator!=
bool operator!=(Quaternion const &other) const
Definition quaternion.hxx:310
vigra::Quaternion::magnitude
NormType magnitude() const
Definition quaternion.hxx:196
vigra::Quaternion::operator[]
value_type operator[](int index) const
Definition quaternion.hxx:189
vigra::Quaternion::setW
void setW(ValueType w)
Definition quaternion.hxx:150
vigra::Quaternion::createRotation
static Quaternion createRotation(double angle, const Vector &rotationAxis)
Definition quaternion.hxx:131
vigra::Quaternion::v
const Vector & v() const
Definition quaternion.hxx:154
vigra::Quaternion::operator*=
Quaternion & operator*=(Quaternion const &other)
Definition quaternion.hxx:267
vigra::Quaternion::operator-
Quaternion operator-() const
Definition quaternion.hxx:257
vigra::Quaternion::operator[]
value_type & operator[](int index)
Definition quaternion.hxx:182
vigra::Quaternion::const_reference
ValueType const & const_reference
Definition quaternion.hxx:75
vigra::Quaternion::operator+=
Quaternion & operator+=(Quaternion const &other)
Definition quaternion.hxx:221
vigra::Quaternion::operator/=
Quaternion & operator/=(Quaternion const &other)
Definition quaternion.hxx:286
vigra::Quaternion::operator==
bool operator==(Quaternion const &other) const
Definition quaternion.hxx:303
vigra::Quaternion::w
ValueType & w()
Definition quaternion.hxx:147
vigra::Quaternion::setV
void setV(const Vector &v)
Definition quaternion.hxx:160
vigra::Quaternion::Quaternion
Quaternion(ValueType w, const Vector &v)
Definition quaternion.hxx:95
vigra::Quaternion::SquaredNormType
NormTraits< ValueType >::SquaredNormType SquaredNormType
Definition quaternion.hxx:79
vigra::Quaternion::value_type
ValueType value_type
Definition quaternion.hxx:67
vigra::Quaternion::w
ValueType w() const
Definition quaternion.hxx:144
vigra::Quaternion::Quaternion
Quaternion(ValueType w=0, ValueType x=0, ValueType y=0, ValueType z=0)
Definition quaternion.hxx:87
vigra::Quaternion::operator-=
Quaternion & operator-=(Quaternion const &other)
Definition quaternion.hxx:241
vigra::Quaternion::operator+=
Quaternion & operator+=(value_type const &w)
Definition quaternion.hxx:213
vigra::Quaternion::operator=
Quaternion & operator=(ValueType w)
Definition quaternion.hxx:116
vigra::Quaternion::operator*=
Quaternion & operator*=(double scale)
Definition quaternion.hxx:277
vigra::Quaternion::setV
void setV(ValueType x, ValueType y, ValueType z)
Definition quaternion.hxx:163
vigra::Quaternion::Quaternion
Quaternion(const Quaternion &q)
Definition quaternion.hxx:101
vigra::Quaternion::NormType
NormTraits< ValueType >::NormType NormType
Definition quaternion.hxx:83
vigra::Quaternion::fillRotationMatrix
void fillRotationMatrix(MatrixType &matrix) const
Definition quaternion.hxx:323
vigra::Quaternion::reference
ValueType & reference
Definition quaternion.hxx:71
vigra::Quaternion::operator/=
Quaternion & operator/=(double scale)
Definition quaternion.hxx:294
vigra::Quaternion::squaredMagnitude
SquaredNormType squaredMagnitude() const
Definition quaternion.hxx:203
vigra::Quaternion::operator-=
Quaternion & operator-=(value_type const &w)
Definition quaternion.hxx:233
vigra::Quaternion::operator=
Quaternion & operator=(Quaternion const &other)
Definition quaternion.hxx:107
vigra::Quaternion::operator+
Quaternion operator+() const
Definition quaternion.hxx:250
vigra::RGBValue
Class for a single RGB value.
Definition rgbvalue.hxx:128
vigra::RGBValue::magnitude
NormType magnitude() const
Definition rgbvalue.hxx:307
vigra::RGBValue::squaredMagnitude
SquaredNormType squaredMagnitude() const
Definition rgbvalue.hxx:313
vigra::TinyVectorBase::init
void init(Iterator i, Iterator end)
Definition tinyvector.hxx:708
vigra::TinyVectorBase::squaredMagnitude
SquaredNormType squaredMagnitude() const
Definition tinyvector.hxx:810
vigra::TinyVector< ValueType, 3 >
vigra
vigra::operator+
Diff2D operator+(Diff2D const &a, Diff2D const &b)
Definition diff2d.hxx:739
vigra::norm
FFTWComplex< R >::NormType norm(const FFTWComplex< R > &a)
norm (= magnitude)
Definition fftw3.hxx:1037
vigra::conj
FFTWComplex< R > conj(const FFTWComplex< R > &a)
complex conjugate
Definition fftw3.hxx:1030
vigra::squaredNorm
FFTWComplex< R >::SquaredNormType squaredNorm(const FFTWComplex< R > &a)
squared norm (= squared magnitude)
Definition fftw3.hxx:1044
vigra::operator-
Diff2D operator-(Diff2D const &a, Diff2D const &b)
Definition diff2d.hxx:711

© Ullrich Köthe (ullrich.koethe@iwr.uni-heidelberg.de) Heidelberg Collaboratory for Image Processing, University of Heidelberg, Germany | html generated using doxygen and Python vigra 1.11.1