27 #ifndef MLN_ALGEBRA_QUAT_HH
28 # define MLN_ALGEBRA_QUAT_HH
38 # include <mln/value/ops.hh>
40 # include <mln/value/concept/vectorial.hh>
41 # include <mln/value/internal/value_like.hh>
42 # include <mln/trait/value_.hh>
44 # include <mln/algebra/vec.hh>
45 # include <mln/math/abs.hh>
46 # include <mln/norm/l2.hh>
48 # include <mln/util/couple.hh>
49 # include <mln/math/pi.hh>
57 namespace algebra {
class quat; }
86 template <
typename S >
92 template <
typename S >
103 struct value_< mln::algebra::quat >
105 typedef trait::value::nature::vectorial
nature;
106 typedef trait::value::kind::data
kind;
107 typedef trait::value::quant::high
quant;
134 algebra::vec<4, float>,
135 algebra::vec<4, float>,
144 quat(
float s,
float x,
float y,
float z);
180 void set_v(
float x,
float y,
float z);
183 float sprod(
const quat&
rhs)
const;
186 bool is_unit()
const;
189 bool is_null()
const;
192 bool is_pure()
const;
204 template <
unsigned n,
typename T>
210 template <
typename T>
219 void set_theta(
float theta);
241 template <
typename T>
270 template <
typename C>
279 template <
typename C>
285 # ifndef MLN_INCLUDE_ONLY
299 quat::quat(
float s,
float x,
float y,
float z)
306 quat::quat(
float s,
const algebra::vec<3,float>& v)
313 quat::quat(
const algebra::vec<4,float>& v)
320 quat::operator=(
const algebra::vec<4,float>& v)
330 quat::quat(
const literal::zero_t&)
337 quat::operator=(
const literal::zero_t&)
344 quat::quat(
const literal::one_t&)
352 quat::operator=(
const literal::one_t&)
361 const algebra::vec<4,float>&
368 quat::operator
const algebra::vec<4,float>&()
const
388 const algebra::vec<3, float>&
391 return *(
const algebra::vec<3, float>*)(
const void*)(& this->v_[1]);
396 algebra::vec<3, float>&
399 return *(algebra::vec<3, float>*)(
void*)(& this->v_[1]);
403 void quat::set_v(
float x,
float y,
float z)
412 quat::sprod(
const quat&
rhs)
const
414 return v_ * rhs.to_vec();
418 bool quat::is_unit()
const
424 bool quat::is_null()
const
430 bool quat::is_pure()
const
436 quat quat::conj()
const
438 return quat(s(), - v());
442 quat quat::inv()
const
444 mln_precondition(! is_null());
446 return conj().to_vec() / (f * f);
450 quat& quat::set_unit()
456 mln_postcondition(this->is_unit());
461 template <
typename T>
463 void quat::set_unit(
float theta,
const algebra::vec<3,T>& uv)
465 mln_precondition(theta > -
float(
math::pi) - mln_epsilon(
float)
466 && theta <
float(
math::pi) + mln_epsilon(
float));
471 this->v_[1] = uv[0] * sint;
472 this->v_[2] = uv[1] * sint;
473 this->v_[3] = uv[2] * sint;
480 quat::quat(
unsigned one,
float theta,
const algebra::vec<3,float>& uv)
482 mln_precondition(one == 1);
488 float quat::theta()
const
490 mln_precondition(is_unit());
495 void quat::set_theta(
float theta)
497 mln_precondition(is_unit());
498 set_unit(theta, uv());
502 algebra::vec<3, float> quat::uv()
const
504 mln_precondition(is_unit());
505 algebra::vec<3, float> w = v();
506 return w.normalize();
510 void quat::set_uv(
const algebra::vec<3,float>& uv)
512 mln_precondition(is_unit());
513 set_unit(theta(), uv);
516 template <
unsigned n,
typename T>
518 algebra::vec<n,float>
521 mln_precondition(is_unit());
522 return ((*
this) *
quat(0. ,v) * (*this).
inv()).v();
528 mln_precondition(this->is_unit());
529 mln_precondition(q.is_pure());
530 return (*
this) * q * this->inv();
537 std::ostream&
operator<<(std::ostream& ostr,
const quat& q)
539 return ostr << q.to_vec();
545 quat tmp(lhs.to_vec() + rhs.to_vec());
552 quat tmp(lhs.to_vec() - rhs.to_vec());
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());
564 template <
typename S>
566 quat operator*(
const quat& lhs,
const value::scalar_<S>& rhs)
568 mlc_converts_to(S,
float)::check();
569 quat tmp(lhs.to_vec() * rhs.to_equiv());
573 template <
typename S>
575 quat operator/(
const quat& lhs,
const value::scalar_<S>& rhs_)
577 mlc_converts_to(S,
float)::check();
578 float rhs = rhs_.to_equiv();
579 mln_precondition(rhs != 0.f);
590 mln_precondition(q.is_unit());
591 return quat(0.f, q.theta() * q.uv());
599 algebra::vec<3, float> v = q.v();
602 algebra::vec<3, float> uv = v / theta;
608 quat pow(
const quat& q,
double t)
610 mln_precondition(q.is_unit());
614 template <
typename T>
624 return about_equal<float>(
norm::l2(p.to_vec() - q.to_vec()), 0);
630 bool interpol_ok(
const quat& p,
const quat& q,
float h)
643 quat lerp(
const quat& p,
const quat& q,
float h)
646 return (1 - h) * p + h * q;
653 quat slerp(
const quat& p,
const quat& q,
float h)
660 quat((std::
sin((1-h)*omega) * p + std::
sin(h*omega) * q) / std::
sin(omega));
664 quat slerp_2(
const quat& p,
const quat& q,
float h)
667 quat tmp = p *
pow(p.conj() * q, h);
673 quat slerp_3(
const quat& p,
const quat& q,
float h)
676 quat tmp =
pow(p * q.conj(), 1 - h) * q;
682 quat slerp_4(
const quat& p,
const quat& q,
float h)
685 quat tmp =
pow(q * p.conj(), h) * p;
691 quat slerp_5(
const quat& p,
const quat& q,
float h)
694 quat tmp = q *
pow(q.conj() *
p, 1 - h);
702 template <
typename C>
712 if (std::fabs( sa ) < 0.0005 )
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;
727 template <
typename C>
745 # endif // ! MLN_INCLUDE_ONLY
749 #endif // ! MLN_ALGEBRA_QUAT_HH