$extrastylesheet
Olena  User documentation 2.1
An Image Processing Platform
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
quat.hh
1 // Copyright (C) 2007, 2008, 2009, 2012, 2013 EPITA Research and Development
2 // Laboratory (LRDE)
3 //
4 // This file is part of Olena.
5 //
6 // Olena is free software: you can redistribute it and/or modify it under
7 // the terms of the GNU General Public License as published by the Free
8 // Software Foundation, version 2 of the License.
9 //
10 // Olena is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Public License for more details.
14 //
15 // You should have received a copy of the GNU General Public License
16 // along with Olena. If not, see <http://www.gnu.org/licenses/>.
17 //
18 // As a special exception, you may use this file as part of a free
19 // software project without restriction. Specifically, if other files
20 // instantiate templates or use macros or inline functions from this
21 // file, or you compile this file and link it with other files to produce
22 // an executable, this file does not by itself cause the resulting
23 // executable to be covered by the GNU General Public License. This
24 // exception does not however invalidate any other reasons why the
25 // executable file might be covered by the GNU General Public License.
26 
27 #ifndef MLN_ALGEBRA_QUAT_HH
28 # define MLN_ALGEBRA_QUAT_HH
29 
35 
36 # include <cmath>
37 
38 # include <mln/value/ops.hh>
39 
40 # include <mln/value/concept/vectorial.hh>
41 # include <mln/value/internal/value_like.hh>
42 # include <mln/trait/value_.hh>
43 
44 # include <mln/algebra/vec.hh>
45 # include <mln/math/abs.hh>
46 # include <mln/norm/l2.hh>
47 
48 # include <mln/util/couple.hh>
49 # include <mln/math/pi.hh>
50 // FIXME: pow, exp etc... are def here and in value::...
51 
52 
53 namespace mln
54 {
55 
56  // Fwd decls.
57  namespace algebra { class quat; }
58  namespace literal { struct zero_t; struct one_t; }
59 
60 
61  namespace trait
62  {
63 
64  // quat OP quat
65 
66  template <>
67  struct set_precise_binary_< op::plus, mln::algebra::quat, mln::algebra::quat >
68  {
69  typedef mln::algebra::quat ret;
70  };
71 
72  template <>
73  struct set_precise_binary_< op::minus, mln::algebra::quat, mln::algebra::quat >
74  {
75  typedef mln::algebra::quat ret;
76  };
77 
78  template <>
79  struct set_precise_binary_< op::times, mln::algebra::quat, mln::algebra::quat >
80  {
81  typedef mln::algebra::quat ret;
82  };
83 
84  // quat OP scalar
85 
86  template < typename S >
87  struct set_precise_binary_< op::times, mln::algebra::quat, mln::value::scalar_<S> >
88  {
89  typedef mln::algebra::quat ret;
90  };
91 
92  template < typename S >
93  struct set_precise_binary_< op::div, mln::algebra::quat, mln::value::scalar_<S> >
94  {
95  typedef mln::algebra::quat ret;
96  };
97 
98 
99  // 'quat' as a value.
100 
101 
102  template <>
103  struct value_< mln::algebra::quat >
104  {
105  typedef trait::value::nature::vectorial nature;
106  typedef trait::value::kind::data kind;
107  typedef trait::value::quant::high quant;
108 
109  enum {
110  nbits = 4 * sizeof(float),
111  card = 0
112  };
113 
114  typedef mln::algebra::quat sum;
115  };
116 
117 
118  } // end of namespace mln::trait
119 
120 
121 
122  namespace algebra
123  {
124 
125  // FIXME value::Vectorial ??? value ???
129  class quat
130  :
131  public value::Vectorial< quat >
132  ,
133  public value::internal::value_like_< algebra::vec<4, float>, // Equivalent.
134  algebra::vec<4, float>, // Encoding.
135  algebra::vec<4, float>, // Interoperation.
136  quat > // Exact.
137  {
138  public:
139 
141  quat();
142 
144  quat(float s, float x, float y, float z);
145 
147  quat(float s, const algebra::vec<3,float>& v);
148 
149 
151  quat(const algebra::vec<4,float>& v);
152 
154  quat& operator=(const algebra::vec<4,float>& v);
155 
156 
158  quat(const literal::zero_t&);
159  quat& operator=(const literal::zero_t&);
160  quat(const literal::one_t&);
161  quat& operator=(const literal::one_t&);
163 
164 
166  const algebra::vec<4,float>& to_vec() const;
167 
169  operator const algebra::vec<4,float>&() const;
170 
172  float s() const;
173 
175  float& s();
176 
177  const algebra::vec<3,float>& v() const;
179 
180  void set_v(float x, float y, float z);
181 
183  float sprod(const quat& rhs) const;
184 
186  bool is_unit() const;
187 
189  bool is_null() const;
190 
192  bool is_pure() const;
193 
195  quat conj() const;
196 
198  quat inv() const; // FIXME: rename invert.
199 
201  quat& set_unit();
202 
204  template <unsigned n, typename T>
206 
207  quat rotate(const quat& q) const;
208 
210  template <typename T>
211  void set_unit(float theta, const algebra::vec<3,T>& uv);
212 
213  // only for unit quaternions described by theta and uv such as:
214  // q = ( cos(theta), sin(theta) * uv )
215 
216  quat(unsigned one, float theta, const algebra::vec<3,float>& uv);
217 
218  float theta() const;
219  void set_theta(float theta);
220 
221  algebra::vec<3,float> uv() const;
222  void set_uv(const algebra::vec<3,float>& uv);
223  };
224 
225 
226  // Operators.
227 
228  std::ostream& operator<<(std::ostream& ostr, const quat& q);
229 
230  quat operator+(const quat& lhs, const quat& rhs);
231  quat operator-(const quat& lhs, const quat& rhs);
232  quat operator*(const quat& lhs, const quat& rhs);
233  template <typename S> quat operator*(const quat& lhs, const value::scalar_<S>& rhs);
234  template <typename S> quat operator/(const quat& lhs, const value::scalar_<S>& rhs);
235 
236  // overloaded math procs
237 
238  quat log(const quat& q);
239  quat exp(const quat& q);
240  quat pow(const quat& q, double t);
241  template <typename T>
242  bool about_equal(const T& f, const T& q);
243  bool about_equal(const quat& p, const quat& q);
244 
245 
246  // Misc.
247 
248  bool interpol_ok(const quat& p, const quat& q, float h);
249 
250 
251  // Linear Quaternion Interpolation.
252 
253  quat lerp(const quat& p, const quat& q, float h);
254 
255 
256  // Spherical Linear Quaternion Interpolation.
257 
258  quat slerp(const quat& p, const quat& q, float h);
259 
260  quat slerp_2(const quat& p, const quat& q, float h);
261 
262  quat slerp_3(const quat& p, const quat& q, float h);
263 
264  quat slerp_4(const quat& p, const quat& q, float h);
265 
266  quat slerp_5(const quat& p, const quat& q, float h);
267 
268 
270  template <typename C>
271  void from_to_(const quat& from, mln::util::couple<C, algebra::vec<3,C> >& to);
272 
273  } // end of namespace mln::algebra
274 
275 
276  namespace make
277  {
278 
279  template <typename C>
281  quat(double angle, const mln::algebra::vec<3,C>& axis);
282 
283  }
284 
285 # ifndef MLN_INCLUDE_ONLY
286 
287 
288  namespace algebra
289  {
290 
291  // Constructors.
292 
293  inline
294  quat::quat()
295  {
296  }
297 
298  inline
299  quat::quat(float s, float x, float y, float z)
300  {
301  v_[0] = s;
302  set_v(x, y, z);
303  }
304 
305  inline
306  quat::quat(float s, const algebra::vec<3,float>& v)
307  {
308  v_[0] = s;
309  this->v() = v;
310  }
311 
312  inline
313  quat::quat(const algebra::vec<4,float>& v)
314  {
315  this->v_ = v;
316  }
317 
318  inline
319  quat&
320  quat::operator=(const algebra::vec<4,float>& v)
321  {
322  this->v_ = v;
323  return *this;
324  }
325 
326 
327  // With literals.
328 
329  inline
330  quat::quat(const literal::zero_t&)
331  {
332  v_.set_all(0);
333  }
334 
335  inline
336  quat&
337  quat::operator=(const literal::zero_t&)
338  {
339  v_.set_all(0);
340  return *this;
341  }
342 
343  inline
344  quat::quat(const literal::one_t&)
345  {
346  s() = 1;
347  v().set_all(0);
348  }
349 
350  inline
351  quat&
352  quat::operator=(const literal::one_t&)
353  {
354  s() = 1;
355  v().set_all(0);
356  return *this;
357  }
358 
359 
360  inline
361  const algebra::vec<4,float>&
362  quat::to_vec() const
363  {
364  return this->v_;
365  }
366 
367  inline
368  quat::operator const algebra::vec<4,float>&() const
369  {
370  return this->v_;
371  }
372 
373  inline
374  float
375  quat::s() const
376  {
377  return this->v_[0];
378  }
379 
380  inline
381  float&
382  quat::s()
383  {
384  return this->v_[0];
385  }
386 
387  inline
388  const algebra::vec<3, float>&
389  quat::v() const
390  {
391  return *(const algebra::vec<3, float>*)(const void*)(& this->v_[1]);
392  // return make::vec(this->v_[1], this->v_[2], this->v_[3]);
393  }
394 
395  inline
396  algebra::vec<3, float>&
397  quat::v()
398  {
399  return *(algebra::vec<3, float>*)(void*)(& this->v_[1]);
400  }
401 
402  inline
403  void quat::set_v(float x, float y, float z)
404  {
405  this->v_[1] = x;
406  this->v_[2] = y;
407  this->v_[3] = z;
408  }
409 
410  inline
411  float
412  quat::sprod(const quat& rhs) const
413  {
414  return v_ * rhs.to_vec();
415  }
416 
417  inline
418  bool quat::is_unit() const
419  {
420  return about_equal(norm::l2(v_), 1.f);
421  }
422 
423  inline
424  bool quat::is_null() const
425  {
426  return about_equal(norm::l2(v_), 0.f);
427  }
428 
429  inline
430  bool quat::is_pure() const
431  {
432  return about_equal(v_[0], 0.f);
433  }
434 
435  inline
436  quat quat::conj() const
437  {
438  return quat(s(), - v());
439  }
440 
441  inline
442  quat quat::inv() const
443  {
444  mln_precondition(! is_null());
445  float f = norm::l2(v_);
446  return conj().to_vec() / (f * f);
447  }
448 
449  inline
450  quat& quat::set_unit()
451  {
452  if (about_equal(norm::l2(this->to_vec()), 0.f))
453  return *this;
454 
455  v_.normalize();
456  mln_postcondition(this->is_unit());
457 
458  return *this;
459  }
460 
461  template <typename T>
462  inline
463  void quat::set_unit(float theta, const algebra::vec<3,T>& uv)
464  {
465  mln_precondition(theta > - float(math::pi) - mln_epsilon(float)
466  && theta < float(math::pi) + mln_epsilon(float));
467  mln_precondition(about_equal(norm::l2(uv), 1.f));
468 
469  this->v_[0] = std::cos(theta);
470  float sint = std::sin(theta);
471  this->v_[1] = uv[0] * sint;
472  this->v_[2] = uv[1] * sint;
473  this->v_[3] = uv[2] * sint;
474  }
475 
476  // only for unit quaternions described by theta and uv such as:
477  // q = ( cos(theta), sin(theta) * uv )
478 
479  inline
480  quat::quat(unsigned one, float theta, const algebra::vec<3,float>& uv)
481  {
482  mln_precondition(one == 1);
483  (void) one;
484  set_unit(theta, uv);
485  }
486 
487  inline
488  float quat::theta() const
489  {
490  mln_precondition(is_unit());
491  return std::acos(s());
492  }
493 
494  inline
495  void quat::set_theta(float theta)
496  {
497  mln_precondition(is_unit());
498  set_unit(theta, uv());
499  }
500 
501  inline
502  algebra::vec<3, float> quat::uv() const
503  {
504  mln_precondition(is_unit());
505  algebra::vec<3, float> w = v();
506  return w.normalize();
507  }
508 
509  inline
510  void quat::set_uv(const algebra::vec<3,float>& uv)
511  {
512  mln_precondition(is_unit());
513  set_unit(theta(), uv);
514  }
515 
516  template <unsigned n, typename T>
517  inline
518  algebra::vec<n,float>
519  quat::rotate(const algebra::vec<n,T>& v) const
520  {
521  mln_precondition(is_unit());
522  return ((*this) * quat(0. ,v) * (*this).inv()).v();
523  }
524 
525  inline
526  quat quat::rotate(const quat& q) const
527  {
528  mln_precondition(this->is_unit());
529  mln_precondition(q.is_pure());
530  return (*this) * q * this->inv();
531  }
532 
533 
534  // Operators.
535 
536  inline
537  std::ostream& operator<<(std::ostream& ostr, const quat& q)
538  {
539  return ostr << q.to_vec();
540  }
541 
542  inline
543  quat operator+(const quat& lhs, const quat& rhs)
544  {
545  quat tmp(lhs.to_vec() + rhs.to_vec());
546  return tmp;
547  }
548 
549  inline
550  quat operator-(const quat& lhs, const quat& rhs)
551  {
552  quat tmp(lhs.to_vec() - rhs.to_vec());
553  return tmp;
554  }
555 
556  inline
557  quat operator*(const quat& lhs, const quat& rhs)
558  {
559  quat tmp(lhs.s() * rhs.s() - lhs.v() * rhs.v(),
560  algebra::vprod(lhs.v(), rhs.v()) + lhs.s() * rhs.v() + rhs.s() * lhs.v());
561  return tmp;
562  }
563 
564  template <typename S>
565  inline
566  quat operator*(const quat& lhs, const value::scalar_<S>& rhs)
567  {
568  mlc_converts_to(S, float)::check();
569  quat tmp(lhs.to_vec() * rhs.to_equiv());
570  return tmp;
571  }
572 
573  template <typename S>
574  inline
575  quat operator/(const quat& lhs, const value::scalar_<S>& rhs_)
576  {
577  mlc_converts_to(S, float)::check();
578  float rhs = rhs_.to_equiv();
579  mln_precondition(rhs != 0.f);
580  quat tmp(lhs.to_vec() / rhs);
581  return tmp;
582  }
583 
584 
585  // overloaded math procs
586 
587  inline
588  quat log(const quat& q)
589  {
590  mln_precondition(q.is_unit());
591  return quat(0.f, q.theta() * q.uv());
592  }
593 
594 
595  inline
596  quat exp(const quat& q)
597  {
598  mln_precondition(about_equal(q.s(), 0.f));
599  algebra::vec<3, float> v = q.v();
600  float theta = norm::l2(v);
601  mln_precondition(!about_equal(theta, 0.f));
602  algebra::vec<3, float> uv = v / theta;
603  return quat(std::cos(theta), std::sin(theta) * uv);
604  }
605 
606 
607  inline
608  quat pow(const quat& q, double t)
609  {
610  mln_precondition(q.is_unit());
611  return exp(t * log(q));
612  }
613 
614  template <typename T>
615  inline
616  bool about_equal(const T& f, const T& q)
617  {
618  return math::abs(q - f) <= mln_epsilon(T);
619  }
620 
621  inline
622  bool about_equal(const quat& p, const quat& q)
623  {
624  return about_equal<float>(norm::l2(p.to_vec() - q.to_vec()), 0);
625  }
626 
627  // Misc.
628 
629  inline
630  bool interpol_ok(const quat& p, const quat& q, float h)
631  {
632  return
633  p.is_unit() &&
634  q.is_unit() &&
635  h >= 0 &&
636  h <= 1;
637  }
638 
639 
640  // Linear Quaternion Interpolation.
641 
642  inline
643  quat lerp(const quat& p, const quat& q, float h)
644  {
645  assert(interpol_ok(p, q, h));
646  return (1 - h) * p + h * q;
647  }
648 
649 
650  // Spherical Linear Quaternion Interpolation.
651 
652  inline
653  quat slerp(const quat& p, const quat& q, float h)
654  {
655  assert(interpol_ok(p, q, h));
656  float omega = std::acos(p.sprod(q));
657  return
658  about_equal(omega, 0.f) ?
659  lerp(p, q, h) :
660  quat((std::sin((1-h)*omega) * p + std::sin(h*omega) * q) / std::sin(omega));
661  }
662 
663  inline
664  quat slerp_2(const quat& p, const quat& q, float h)
665  {
666  assert(interpol_ok(p, q, h));
667  quat tmp = p * pow(p.conj() * q, h);
668  assert(about_equal(tmp, slerp(p, q, h)));
669  return tmp;
670  }
671 
672  inline
673  quat slerp_3(const quat& p, const quat& q, float h)
674  {
675  assert(interpol_ok(p, q, h));
676  quat tmp = pow(p * q.conj(), 1 - h) * q;
677  assert(about_equal(tmp, slerp(p, q, h)));
678  return tmp;
679  }
680 
681  inline
682  quat slerp_4(const quat& p, const quat& q, float h)
683  {
684  assert(interpol_ok(p, q, h));
685  quat tmp = pow(q * p.conj(), h) * p;
686  assert(about_equal(tmp, slerp(p, q, h)));
687  return tmp;
688  }
689 
690  inline
691  quat slerp_5(const quat& p, const quat& q, float h)
692  {
693  assert(interpol_ok(p, q, h));
694  quat tmp = q * pow(q.conj() * p, 1 - h);
695  assert(about_equal(tmp, slerp(p, q, h)));
696  return tmp;
697  }
698 
699 
700  // Conversions.
701 
702  template <typename C>
703  void from_to_(const quat& from, mln::util::couple<C, algebra::vec<3,C> >& to)
704  {
705  quat tmp = from;
706  tmp.set_unit();
707 
708  C w = tmp.to_vec()[0],
709  angle = std::acos(w) * 2 * 180/math::pi;
710 
711  C sa = std::sin(std::acos(w));
712  if (std::fabs( sa ) < 0.0005 )
713  sa = 1;
714 
715  to.first() = angle;
716  to.second()[0] = tmp.to_vec()[1] / sa;
717  to.second()[1] = tmp.to_vec()[2] / sa;
718  to.second()[2] = tmp.to_vec()[3] / sa;
719  }
720 
721  } // end of namespace mln::algebra
722 
723 
724  namespace make
725  {
726 
727  template <typename C>
729  quat(double angle, const mln::algebra::vec<3,C>& axis)
730  {
731  angle *= mln::math::pi/180;
732  C s = std::sin(angle / 2);
733 
734  C x = axis[0] * s,
735  y = axis[1] * s,
736  z = axis[2] * s,
737  w = std::cos(angle / 2);
738 
739  return mln::algebra::quat(w, x, y, z);
740  }
741 
742  } // end of namespace mln::make
743 
744 
745 # endif // ! MLN_INCLUDE_ONLY
746 
747 } // end of namespace mln
748 
749 #endif // ! MLN_ALGEBRA_QUAT_HH