vigra - vigra/quaternion.hxx Source File

[ 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>
461 operator*(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>
470 operator*(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>
479 operator*(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>
488 operator/(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>
497 operator/(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>
506 operator/(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
525 abs(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
538 ostream & 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
546 istream & 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

© 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.10.0 (Mon Nov 18 2013)