26 #ifndef MLN_ALGEBRA_QUAT_HH
27 # define MLN_ALGEBRA_QUAT_HH
37 # include <mln/value/ops.hh>
39 # include <mln/value/concept/vectorial.hh>
40 # include <mln/value/internal/value_like.hh>
41 # include <mln/trait/value_.hh>
43 # include <mln/algebra/vec.hh>
44 # include <mln/math/abs.hh>
45 # include <mln/norm/l2.hh>
54 namespace algebra {
class quat; }
64 struct set_precise_binary_< op::plus, mln::algebra::quat, mln::algebra::quat >
66 typedef mln::algebra::quat ret;
70 struct set_precise_binary_< op::minus, mln::algebra::quat, mln::algebra::quat >
72 typedef mln::algebra::quat ret;
76 struct set_precise_binary_< op::
times, mln::algebra::quat, mln::algebra::quat >
78 typedef mln::algebra::quat ret;
83 template <
typename S >
84 struct set_precise_binary_< op::
times, mln::algebra::quat, mln::value::scalar_<S> >
86 typedef mln::algebra::quat ret;
89 template <
typename S >
90 struct set_precise_binary_< op::
div, mln::algebra::quat, mln::value::scalar_<S> >
92 typedef mln::algebra::quat ret;
100 struct value_< mln::algebra::quat >
102 typedef trait::value::nature::vectorial nature;
103 typedef trait::value::kind::data kind;
104 typedef trait::value::quant::high quant;
107 nbits = 4 *
sizeof(float),
111 typedef mln::algebra::quat
sum;
125 public value::Vectorial< quat >
127 public value::internal::value_like_< algebra::vec<4, float>,
128 algebra::vec<4, float>,
129 algebra::vec<4, float>,
138 quat(
float s,
float x,
float y,
float z);
141 quat(
float s,
const algebra::vec<3,float>& v);
145 quat(
const algebra::vec<4,float>& v);
148 quat& operator=(
const algebra::vec<4,float>& v);
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&);
160 const algebra::vec<4,float>& to_vec()
const;
163 operator const algebra::vec<4,float>&()
const;
171 const algebra::vec<3,float>& v()
const;
172 algebra::vec<3,float>& v();
174 void set_v(
float x,
float y,
float z);
177 float sprod(
const quat& rhs)
const;
180 bool is_unit()
const;
183 bool is_null()
const;
186 bool is_pure()
const;
198 template <
unsigned n,
typename T>
199 algebra::vec<n,float>
rotate(
const algebra::vec<n,T>& v)
const;
201 quat
rotate(
const quat& q)
const;
204 template <
typename T>
205 void set_unit(
float theta,
const algebra::vec<3,T>& uv);
210 quat(
unsigned one,
float theta,
const algebra::vec<3,float>& uv);
213 void set_theta(
float theta);
215 algebra::vec<3,float> uv()
const;
216 void set_uv(
const algebra::vec<3,float>& uv);
222 std::ostream& operator<<(std::ostream& ostr,
const quat& q);
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);
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);
242 bool interpol_ok(
const quat& p,
const quat& q,
float h);
247 quat lerp(
const quat& p,
const quat& q,
float h);
252 quat slerp(
const quat& p,
const quat& q,
float h);
254 quat slerp_2(
const quat& p,
const quat& q,
float h);
256 quat slerp_3(
const quat& p,
const quat& q,
float h);
258 quat slerp_4(
const quat& p,
const quat& q,
float h);
260 quat slerp_5(
const quat& p,
const quat& q,
float h);
263 # ifndef MLN_INCLUDE_ONLY
273 quat::quat(
float s,
float x,
float y,
float z)
280 quat::quat(
float s,
const algebra::vec<3,float>& v)
287 quat::quat(
const algebra::vec<4,float>& v)
294 quat::operator=(
const algebra::vec<4,float>& v)
304 quat::quat(
const literal::zero_t&)
311 quat::operator=(
const literal::zero_t&)
318 quat::quat(
const literal::one_t&)
326 quat::operator=(
const literal::one_t&)
335 const algebra::vec<4,float>&
342 quat::operator
const algebra::vec<4,float>&()
const
362 const algebra::vec<3, float>&
365 return *(
const algebra::vec<3, float>*)(
const void*)(& this->v_[1]);
370 algebra::vec<3, float>&
373 return *(algebra::vec<3, float>*)(
void*)(& this->v_[1]);
377 void quat::set_v(
float x,
float y,
float z)
386 quat::sprod(
const quat& rhs)
const
388 return v_ * rhs.to_vec();
392 bool quat::is_unit()
const
394 return about_equal(norm::l2(v_), 1.f);
398 bool quat::is_null()
const
400 return about_equal(norm::l2(v_), 0.f);
404 bool quat::is_pure()
const
406 return about_equal(v_[0], 0.f);
410 quat quat::conj()
const
412 return quat(s(), - v());
416 quat quat::inv()
const
418 mln_precondition(! is_null());
419 float f = norm::l2(v_);
420 return conj().to_vec() / (f * f);
424 quat& quat::set_unit()
426 if (about_equal(norm::l2(this->to_vec()), 0.f))
430 mln_postcondition(this->is_unit());
435 template <
typename T>
437 void quat::set_unit(
float theta,
const algebra::vec<3,T>& uv)
439 static const float pi = 3.14159265358979323846f;
441 mln_precondition(theta > - pi - mln_epsilon(
float)
442 && theta < pi + mln_epsilon(
float));
443 mln_precondition(about_equal(norm::l2(uv), 1.f));
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;
457 quat::quat(
unsigned one,
float theta,
const algebra::vec<3,float>& uv)
459 mln_precondition(one == 1);
465 float quat::theta()
const
467 mln_precondition(is_unit());
468 return std::acos(s());
472 void quat::set_theta(
float theta)
474 mln_precondition(is_unit());
475 set_unit(theta, uv());
479 algebra::vec<3, float> quat::uv()
const
481 mln_precondition(is_unit());
482 algebra::vec<3, float> w = v();
483 return w.normalize();
487 void quat::set_uv(
const algebra::vec<3,float>& uv)
489 mln_precondition(is_unit());
490 set_unit(theta(), uv);
493 template <
unsigned n,
typename T>
495 algebra::vec<n,float>
496 quat::rotate(
const algebra::vec<n,T>& v)
const
498 mln_precondition(is_unit());
499 return ((*
this) * quat(0. ,v) * (*this).inv()).v();
503 quat quat::rotate(
const quat& q)
const
505 mln_precondition(this->is_unit());
506 mln_precondition(q.is_pure());
507 return (*
this) * q * this->inv();
514 std::ostream& operator<<(std::ostream& ostr,
const quat& q)
516 return ostr << q.to_vec();
520 quat operator+(
const quat& lhs,
const quat& rhs)
522 quat tmp(lhs.to_vec() + rhs.to_vec());
527 quat operator-(
const quat& lhs,
const quat& rhs)
529 quat tmp(lhs.to_vec() - rhs.to_vec());
534 quat operator*(
const quat& lhs,
const quat& rhs)
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());
541 template <
typename S>
543 quat operator*(
const quat& lhs,
const value::scalar_<S>& rhs)
545 mlc_converts_to(S,
float)::check();
546 quat tmp(lhs.to_vec() * rhs.to_equiv());
550 template <
typename S>
552 quat operator/(
const quat& lhs,
const value::scalar_<S>& rhs_)
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);
565 quat log(
const quat& q)
567 mln_precondition(q.is_unit());
568 return quat(0.f, q.theta() * q.uv());
573 quat exp(
const quat& q)
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);
585 quat pow(
const quat& q,
double t)
587 mln_precondition(q.is_unit());
588 return exp(t * log(q));
591 template <
typename T>
593 bool about_equal(
const T& f,
const T& q)
595 return math::abs(q - f) <= mln_epsilon(T);
599 bool about_equal(
const quat& p,
const quat& q)
601 return about_equal<float>(norm::l2(p.to_vec() - q.to_vec()), 0);
607 bool interpol_ok(
const quat& p,
const quat& q,
float h)
620 quat lerp(
const quat& p,
const quat& q,
float h)
622 assert(interpol_ok(p, q, h));
623 return (1 - h) * p + h * q;
630 quat slerp(
const quat& p,
const quat& q,
float h)
632 assert(interpol_ok(p, q, h));
633 float omega = std::acos(p.sprod(q));
635 about_equal(omega, 0.f) ?
637 quat((std::sin((1-h)*omega) * p + std::sin(h*omega) * q) / std::sin(omega));
641 quat slerp_2(
const quat& p,
const quat& q,
float h)
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)));
650 quat slerp_3(
const quat& p,
const quat& q,
float h)
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)));
659 quat slerp_4(
const quat& p,
const quat& q,
float h)
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)));
668 quat slerp_5(
const quat& p,
const quat& q,
float h)
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)));
676 # endif // ! MLN_INCLUDE_ONLY
682 #endif // ! MLN_ALGEBRA_QUAT_HH