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