iRoCS Toolbox  1.1.0
Quaternion.hh
Go to the documentation of this file.
1 /**************************************************************************
2  *
3  * Copyright (C) 2015 Thorsten Falk
4  *
5  * Image Analysis Lab, University of Freiburg, Germany
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 3 of the License, or
10  * (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, write to the Free Software Foundation,
19  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20  *
21  **************************************************************************/
22 
23 #ifndef QUATERNION_HH
24 #define QUATERNION_HH
25 
26 #ifdef HAVE_CONFIG_H
27 #include <config.hh>
28 #endif
29 
30 #include<stdexcept>
31 
32 namespace atb
33 {
34 
35 /*======================================================================*/
73 /*======================================================================*/
74  template<typename Type>
75  class Quaternion
76  {
77 
78  public:
79 
80 /*======================================================================*/
86 /*======================================================================*/
87  Quaternion(const Type& q0 = Type(),
88  const Type& q1 = Type(),
89  const Type& q2 = Type(),
90  const Type& q3 = Type());
91 
92 /*======================================================================*/
101 /*======================================================================*/
102  template<typename Type2>
103  Quaternion(Quaternion<Type2> const &q);
104 
105 /*======================================================================*/
116 /*======================================================================*/
117  Type& operator()(const int index);
118 
119 /*======================================================================*/
131 /*======================================================================*/
132  Type const &operator()(const int index) const;
133 
134 /*======================================================================*/
142 /*======================================================================*/
143  Quaternion<Type>& operator=(Type const &scalar);
144 
145 /*======================================================================*/
153 /*======================================================================*/
154  Quaternion<Type>& operator+=(Type const &scalar);
155 
156 /*======================================================================*/
164 /*======================================================================*/
165  Quaternion<Type>& operator-=(Type const &scalar);
166 
167 /*======================================================================*/
175 /*======================================================================*/
176  Quaternion<Type>& operator*=(Type const &scalar);
177 
178 /*======================================================================*/
186 /*======================================================================*/
187  Quaternion<Type>& operator/=(Type const &scalar);
188 
189 /*======================================================================*/
199 /*======================================================================*/
200  template<typename Type2>
202 
203 /*======================================================================*/
213 /*======================================================================*/
214  template<typename Type2>
216 
217 /*======================================================================*/
227 /*======================================================================*/
228  template<typename Type2>
230 
231 /*======================================================================*/
241 /*======================================================================*/
242  template<typename Type2>
244 
245 /*======================================================================*/
255 /*======================================================================*/
256  template<typename Type2>
258 
259 
260  private:
261 
262  Type _quat[4];
263 
264  };
265 
266 
267  template<typename Type>
268  inline
270  const Type& b,
271  const Type& c,
272  const Type& d)
273  {
274  _quat[0] = a;
275  _quat[1] = b;
276  _quat[2] = c;
277  _quat[3] = d;
278  }
279 
280  template<typename Type>
281  template<typename Type2>
282  inline
284  {
285  _quat[0] = quat(0);
286  _quat[1] = quat(1);
287  _quat[2] = quat(2);
288  _quat[3] = quat(3);
289  }
290 
291  template<typename Type>
292  inline Type &Quaternion<Type>::operator()(const int index)
293  {
294  if (index < 0 || index > 3)
295  {
296  std::stringstream errmsg;
297  errmsg << "Index exceeds quaternion size\n"
298  << " Index must be between 0 and 3 - got " << index;
299  std::out_of_range err(errmsg.str());
300  throw err;
301  }
302  return _quat[index];
303  }
304 
305  template<typename Type>
306  inline Type const &Quaternion<Type>::operator()(const int index) const
307  {
308  if (index < 0 || index > 3)
309  {
310  std::stringstream errmsg;
311  errmsg << "Index exceeds quaternion size\n"
312  << " Index must be between 0 and 3 - got " << index;
313  std::out_of_range err(errmsg.str());
314  throw err;
315  }
316  return _quat[index];
317  }
318 
319  template<typename Type>
320  inline Quaternion<Type>&
322  {
323  _quat[0] = t;
324  _quat[1] = Type();
325  _quat[2] = Type();
326  _quat[3] = Type();
327  return *this;
328  }
329 
330  template<typename Type>
331  inline Quaternion<Type>&
333  {
334  _quat[0] += t;
335  return *this;
336  }
337 
338  template<typename Type>
339  inline Quaternion<Type>&
341  {
342  _quat[0] -= t;
343  return *this;
344  }
345 
346  template<typename Type>
347  inline Quaternion<Type>&
349  {
350  _quat[0] *= t;
351  _quat[1] *= t;
352  _quat[2] *= t;
353  _quat[3] *= t;
354  return *this;
355  }
356 
357  template<typename Type>
358  inline Quaternion<Type>&
360  {
361  _quat[0] /= t;
362  _quat[1] /= t;
363  _quat[2] /= t;
364  _quat[3] /= t;
365  return *this;
366  }
367 
368  template<typename Type>
369  template<typename Type2>
370  inline Quaternion<Type>&
372  {
373  _quat[0] = t(0);
374  _quat[1] = t(1);
375  _quat[2] = t(2);
376  _quat[3] = t(3);
377  return *this;
378  }
379 
380  template<typename Type>
381  template<typename Type2>
382  inline Quaternion<Type>&
384  {
385  _quat[0] += t(0);
386  _quat[1] += t(1);
387  _quat[2] += t(2);
388  _quat[3] += t(3);
389  return *this;
390  }
391 
392  template<typename Type>
393  template<typename Type2>
394  inline Quaternion<Type>&
396  {
397  _quat[0] -= t(0);
398  _quat[1] -= t(1);
399  _quat[2] -= t(2);
400  _quat[3] -= t(3);
401  return *this;
402  }
403 
404  template<typename Type>
405  template<typename Type2>
406  inline Quaternion<Type>&
408  {
409  const Type a = _quat[0] * t(0) - _quat[1] * t(1) -
410  _quat[2] * t(2) - _quat[3] * t(3);
411  const Type b = _quat[0] * t(1) + _quat[1] * t(0) +
412  _quat[2] * t(3) - _quat[3] * t(2);
413  const Type c = _quat[0] * t(2) - _quat[1] * t(3) +
414  _quat[2] * t(0) + _quat[3] * t(1);
415  _quat[3] = _quat[0] * t(3) + _quat[1] * t(2) -
416  _quat[2] * t(1) + _quat[3] * t(0);
417  _quat[2] = c;
418  _quat[1] = b;
419  _quat[0] = a;
420  return *this;
421  }
422 
423  template<typename Type>
424  template<typename Type2>
425  inline Quaternion<Type>&
427  {
428  *this *= conj(t);
429  *this /= t(0) * t(0) + t(1) * t(1) + t(2) * t(2) + t(3) * t(3);
430  return *this;
431  }
432 
433 
434 // Class independent operators
435 
436 /*======================================================================*/
447 /*======================================================================*/
448  template<typename Type>
449  inline Quaternion<Type>
451  const Quaternion<Type>& t2)
452  {
453  return Quaternion<Type>(t1) += t2;
454  }
455 
456 /*======================================================================*/
467 /*======================================================================*/
468  template<typename Type>
469  inline Quaternion<Type>
471  const Type& t2)
472  {
473  return Quaternion<Type>(t1) += t2;
474  }
475 
476 /*======================================================================*/
487 /*======================================================================*/
488  template<typename Type>
489  inline Quaternion<Type>
490  operator+(const Type& t1,
491  const Quaternion<Type>& t2)
492  {
493  return Quaternion<Type>(t1) += t2;
494  }
495 
496 /*======================================================================*/
507 /*======================================================================*/
508  template<typename Type>
509  inline Quaternion<Type>
511  const Quaternion<Type>& t2)
512  {
513  return Quaternion<Type>(t1) -= t2;
514  }
515 
516 /*======================================================================*/
527 /*======================================================================*/
528  template<typename Type>
529  inline Quaternion<Type>
531  const Type& t2)
532  {
533  return Quaternion<Type>(t1) -= t2;
534  }
535 
536 /*======================================================================*/
547 /*======================================================================*/
548  template<typename Type>
549  inline Quaternion<Type>
550  operator-(const Type& t1,
551  const Quaternion<Type>& t2)
552  {
553  return Quaternion<Type>(t1) -= t2;
554  }
555 
556 /*======================================================================*/
567 /*======================================================================*/
568  template<typename Type>
569  inline Quaternion<Type>
571  const Quaternion<Type>& t2)
572  {
573  return Quaternion<Type>(t1) *= t2;
574  }
575 
576 /*======================================================================*/
587 /*======================================================================*/
588  template<typename Type>
589  inline Quaternion<Type>
591  const Type& t2)
592  {
593  return Quaternion<Type>(t1) *= t2;
594  }
595 
596 /*======================================================================*/
607 /*======================================================================*/
608  template<typename Type>
609  inline Quaternion<Type>
610  operator*(const Type& t1,
611  const Quaternion<Type>& t2)
612  {
613  return Quaternion<Type>(t1) *= t2;
614  }
615 
616 /*======================================================================*/
627 /*======================================================================*/
628  template<typename Type>
629  inline Quaternion<Type>
631  const Quaternion<Type>& t2)
632  {
633  return Quaternion<Type>(t1) /= t2;
634  }
635 
636 /*======================================================================*/
647 /*======================================================================*/
648  template<typename Type>
649  inline Quaternion<Type>
651  const Type& t2)
652  {
653  return Quaternion<Type>(t1) /= t2;
654  }
655 
656 /*======================================================================*/
667 /*======================================================================*/
668  template<typename Type>
669  inline Quaternion<Type>
670  operator/(const Type& t1,
671  const Quaternion<Type>& t2)
672  {
673  return Quaternion<Type>(t1) /= t2;
674  }
675 
676 /*======================================================================*/
686 /*======================================================================*/
687  template<typename Type>
688  inline Quaternion<Type>
690  {
691  return t;
692  }
693 
694 /*======================================================================*/
704 /*======================================================================*/
705  template<typename Type>
706  inline Quaternion<Type>
708  {
709  return Quaternion<Type>(-t(0), -t(1), -t(2), -t(3));
710  }
711 
712 /*======================================================================*/
723 /*======================================================================*/
724  template<typename Type>
725  inline bool
727  const Quaternion<Type>& t2)
728  {
729  return t1(0) == t2(0) && t1(1) == t2(1) &&
730  t1(2) == t2(2) && t1(3) == t2(3);
731  }
732 
733 /*======================================================================*/
744 /*======================================================================*/
745  template<typename Type>
746  inline bool
748  const Type& t2)
749  {
750  return t1(0) == t2 && t1(1) == Type() &&
751  t1(2) == Type() && t1(3) == Type();
752  }
753 
754 /*======================================================================*/
765 /*======================================================================*/
766  template<typename Type>
767  inline bool
768  operator==(const Type& t1,
769  const Quaternion<Type>& t2)
770  {
771  return t1 == t2(0) && t2(1) == Type() &&
772  t2(2) == Type() && t2(3) == Type();
773  }
774 
775 /*======================================================================*/
786 /*======================================================================*/
787  template<typename Type>
788  inline bool
790  const Quaternion<Type>& t2)
791  {
792  return t1(0) != t2(0) || t1(1) != t2(1) ||
793  t1(2) != t2(2) || t1(3) != t2(3);
794  }
795 
796 /*======================================================================*/
807 /*======================================================================*/
808  template<typename Type>
809  inline bool
811  const Type& t2)
812  {
813  return t1(0) != t2 || t1(1) != Type() ||
814  t1(2) != Type() || t1(3) != Type();
815  }
816 
817 /*======================================================================*/
828 /*======================================================================*/
829  template<typename Type>
830  inline bool
831  operator!=(const Type& t1,
832  const Quaternion<Type>& t2)
833  {
834  return t1 != t2(0) || t2(1) != Type() ||
835  t2(2) != Type() || t2(3) != Type();
836  }
837 
838 /*======================================================================*/
849 /*======================================================================*/
850  template<typename Type, typename CharT, class Traits>
851  std::basic_istream<CharT, Traits>&
852  operator>>(std::basic_istream<CharT, Traits>& is, Quaternion<Type>& t)
853  {
854  Type a, b, c, d;
855  CharT ch;
856  is >> ch;
857  if (ch == '(')
858  {
859  is >> a >> ch;
860  if (ch == ',')
861  {
862  is >> b >> ch;
863  if (ch == ',')
864  {
865  is >> c >> ch;
866  if (ch == ',')
867  {
868  is >> d >> ch;
869  if (ch == ')') t = Quaternion<Type>(a, b, c, d);
870  else is.setstate(std::ios_base::failbit);
871  }
872  else is.setstate(std::ios_base::failbit);
873  }
874  else is.setstate(std::ios_base::failbit);
875  }
876  else is.setstate(std::ios_base::failbit);
877  }
878  else
879  {
880  is.putback(ch);
881  is >> a;
882  t = Quaternion<Type>(a, Type(0), Type(0), Type(0));
883  }
884  return is;
885  }
886 
887 /*======================================================================*/
898 /*======================================================================*/
899  template<typename Type, typename CharT, class Traits>
900  std::basic_ostream<CharT, Traits>&
901  operator<<(std::basic_ostream<CharT, Traits>& os, const Quaternion<Type>& t)
902  {
903  std::basic_ostringstream<CharT, Traits> s;
904  s.flags(os.flags());
905  s.imbue(os.getloc());
906  s.precision(os.precision());
907  s << '(' << t(0) << ',' << t(1) << ',' << t(2) << ',' << t(3) << ')';
908  return os << s.str();
909  }
910 
911 /*======================================================================*/
921 /*======================================================================*/
922  template<typename Type>
923  inline Type
925  {
926  return std::sqrt(t(0) * t(0) + t(1) * t(1) +
927  t(2) * t(2) + t(3) * t(3));
928  }
929 
930 /*======================================================================*/
942 /*======================================================================*/
943  template<typename Type>
944  inline Quaternion<Type>
946  {
947  return Quaternion<Type>(t(0), -t(1), -t(2), -t(3));
948  }
949 
950 }
951 
952 #endif
bool operator!=(const Type &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:831
Type abs(const Quaternion< Type > &t)
Definition: Quaternion.hh:924
Quaternion< Type > & operator+=(Type const &scalar)
Add the given real number to this quaternion.
Definition: Quaternion.hh:332
Quaternion< Type > operator-(const Quaternion< Type > &t)
Definition: Quaternion.hh:707
Quaternion< Type > operator/(const Quaternion< Type > &t1, const Type &t2)
Definition: Quaternion.hh:650
Quaternion(const Type &q0=Type(), const Type &q1=Type(), const Type &q2=Type(), const Type &q3=Type())
Create a new quaternion .
Definition: Quaternion.hh:269
bool operator==(const Quaternion< Type > &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:726
The Quaternion class implements templated quaternions with few important operations.
Definition: Quaternion.hh:75
std::basic_istream< CharT, Traits > & operator>>(std::basic_istream< CharT, Traits > &is, Quaternion< Type > &t)
Definition: Quaternion.hh:852
Quaternion< Type > operator*(const Type &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:610
bool operator!=(const Quaternion< Type > &t1, const Type &t2)
Definition: Quaternion.hh:810
Quaternion< Type > & operator-=(Type const &scalar)
Subtract the given real number from this quaternion.
Definition: Quaternion.hh:340
bool operator==(const Type &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:768
Quaternion< Type > operator*(const Quaternion< Type > &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:570
bool operator!=(const Quaternion< Type > &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:789
Type & operator()(const int index)
Get a reference to the coefficient for the given quaternion dimension.
Definition: Quaternion.hh:292
Quaternion< Type > operator+(const Type &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:490
Quaternion< Type > & operator/=(Type const &scalar)
Divide this quaternion by the given scalar value.
Definition: Quaternion.hh:359
bool operator==(const Quaternion< Type > &t1, const Type &t2)
Definition: Quaternion.hh:747
Quaternion< Type > & operator*=(Type const &scalar)
Scale this quaternion with the given scalar value.
Definition: Quaternion.hh:348
Quaternion< Type > operator-(const Quaternion< Type > &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:510
Quaternion< Type > & operator=(Type const &scalar)
Set this quaternion to the given real number.
Definition: Quaternion.hh:321
Quaternion< Type > operator+(const Quaternion< Type > &t)
Definition: Quaternion.hh:689
Quaternion< Type > operator-(const Quaternion< Type > &t1, const Type &t2)
Definition: Quaternion.hh:530
Quaternion< Type > operator+(const Quaternion< Type > &t1, const Type &t2)
Definition: Quaternion.hh:470
Quaternion< Type > operator+(const Quaternion< Type > &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:450
Quaternion< Type > operator-(const Type &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:550
Quaternion< Type > conj(const Quaternion< Type > &t)
Definition: Quaternion.hh:945
Quaternion< Type > operator/(const Type &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:670
Quaternion< Type > operator/(const Quaternion< Type > &t1, const Quaternion< Type > &t2)
Definition: Quaternion.hh:630
Quaternion< Type > operator*(const Quaternion< Type > &t1, const Type &t2)
Definition: Quaternion.hh:590