Milena (Olena): quat.hh Source File

00001 // Copyright (C) 2007, 2008, 2009 EPITA Research and Development Laboratory (LRDE)
00002 //
00003 // This file is part of Olena.
00004 //
00005 // Olena is free software: you can redistribute it and/or modify it under
00006 // the terms of the GNU General Public License as published by the Free
00007 // Software Foundation, version 2 of the License.
00008 //
00009 // Olena is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00012 // General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU General Public License
00015 // along with Olena.  If not, see <http://www.gnu.org/licenses/>.
00016 //
00017 // As a special exception, you may use this file as part of a free
00018 // software project without restriction.  Specifically, if other files
00019 // instantiate templates or use macros or inline functions from this
00020 // file, or you compile this file and link it with other files to produce
00021 // an executable, this file does not by itself cause the resulting
00022 // executable to be covered by the GNU General Public License.  This
00023 // exception does not however invalidate any other reasons why the
00024 // executable file might be covered by the GNU General Public License.
00025 
00026 #ifndef MLN_ALGEBRA_QUAT_HH
00027 # define MLN_ALGEBRA_QUAT_HH
00028 
00034 
00035 # include <cmath>
00036 
00037 # include <mln/value/ops.hh>
00038 
00039 # include <mln/value/concept/vectorial.hh>
00040 # include <mln/value/internal/value_like.hh>
00041 # include <mln/trait/value_.hh>
00042 
00043 # include <mln/algebra/vec.hh>
00044 # include <mln/math/abs.hh>
00045 # include <mln/norm/l2.hh>
00046 
00047 // FIXME: pow, exp etc... are def here and in value::...
00048 
00049 
00050 namespace mln
00051 {
00052 
00053   // Fwd decls.
00054   namespace algebra { class quat; }
00055   namespace literal { struct zero_t; struct one_t; }
00056 
00057 
00058   namespace trait
00059   {
00060 
00061     // quat OP quat
00062 
00063     template <>
00064     struct set_precise_binary_< op::plus, mln::algebra::quat, mln::algebra::quat >
00065     {
00066       typedef mln::algebra::quat ret;
00067     };
00068 
00069     template <>
00070     struct set_precise_binary_< op::minus, mln::algebra::quat, mln::algebra::quat >
00071     {
00072       typedef mln::algebra::quat ret;
00073     };
00074 
00075     template <>
00076     struct set_precise_binary_< op::times, mln::algebra::quat, mln::algebra::quat >
00077     {
00078       typedef mln::algebra::quat ret;
00079     };
00080 
00081     // quat OP scalar
00082 
00083     template < typename S >
00084     struct set_precise_binary_< op::times, mln::algebra::quat, mln::value::scalar_<S> >
00085     {
00086       typedef mln::algebra::quat ret;
00087     };
00088 
00089     template < typename S >
00090     struct set_precise_binary_< op::div, mln::algebra::quat, mln::value::scalar_<S> >
00091     {
00092       typedef mln::algebra::quat ret;
00093     };
00094 
00095 
00096     // 'quat' as a value.
00097 
00098 
00099     template <>
00100     struct value_< mln::algebra::quat >
00101     {
00102       typedef trait::value::nature::vectorial nature;
00103       typedef trait::value::kind::data        kind;
00104       typedef trait::value::quant::high       quant;
00105 
00106       enum {
00107         nbits = 4 * sizeof(float),
00108         card  = 0
00109       };
00110 
00111       typedef mln::algebra::quat sum;
00112     };
00113 
00114 
00115   } // end of namespace mln::trait
00116 
00117 
00118 
00119   namespace algebra
00120   {
00121 
00122     // FIXME value::Vectorial ??? value ???
00123     class quat
00124       :
00125       public value::Vectorial< quat >
00126       ,
00127       public value::internal::value_like_< algebra::vec<4, float>, // Equivalent.
00128                                     algebra::vec<4, float>, // Encoding.
00129                                     algebra::vec<4, float>, // Interoperation.
00130                                     quat >                // Exact.
00131     {
00132     public:
00133 
00135       quat();
00136 
00138       quat(float s, float x, float y, float z);
00139 
00141       quat(float s, const algebra::vec<3,float>& v);
00142 
00143 
00145       quat(const algebra::vec<4,float>& v);
00146 
00148       quat& operator=(const algebra::vec<4,float>& v);
00149 
00150 
00152       quat(const literal::zero_t&);
00153       quat& operator=(const literal::zero_t&);
00154       quat(const literal::one_t&);
00155       quat& operator=(const literal::one_t&);
00157 
00158 
00160       const algebra::vec<4,float>& to_vec() const;
00161 
00163       operator const algebra::vec<4,float>&() const;
00164 
00166       float  s() const;
00167 
00169       float& s();
00170 
00171       const algebra::vec<3,float>& v() const;
00172       algebra::vec<3,float>& v();
00173 
00174       void set_v(float x, float y, float z);
00175 
00177       float sprod(const quat& rhs) const;
00178 
00180       bool is_unit() const;
00181 
00183       bool is_null() const;
00184 
00186       bool is_pure() const;
00187 
00189       quat conj() const;
00190 
00192       quat inv() const; // FIXME: rename invert.
00193 
00195       quat& set_unit();
00196 
00198       template <unsigned n, typename T>
00199       algebra::vec<n,float> rotate(const algebra::vec<n,T>& v) const;
00200 
00201       quat rotate(const quat& q) const;
00202 
00204       template <typename T>
00205       void set_unit(float theta, const algebra::vec<3,T>& uv);
00206 
00207       // only for unit quaternions described by theta and uv such as:
00208       // q = ( cos(theta), sin(theta) * uv )
00209 
00210       quat(unsigned one, float theta, const algebra::vec<3,float>& uv);
00211 
00212       float theta() const;
00213       void set_theta(float theta);
00214 
00215       algebra::vec<3,float> uv() const;
00216       void set_uv(const algebra::vec<3,float>& uv);
00217     };
00218 
00219 
00220     // Operators.
00221 
00222     std::ostream& operator<<(std::ostream& ostr, const quat& q);
00223 
00224     quat operator+(const quat& lhs, const quat& rhs);
00225     quat operator-(const quat& lhs, const quat& rhs);
00226     quat operator*(const quat& lhs, const quat& rhs);
00227     template <typename S> quat operator*(const quat& lhs, const value::scalar_<S>& rhs);
00228     template <typename S> quat operator/(const quat& lhs, const value::scalar_<S>& rhs);
00229 
00230     // overloaded math procs
00231 
00232     quat log(const quat& q);
00233     quat exp(const quat& q);
00234     quat pow(const quat& q, double t);
00235     template <typename T>
00236     bool about_equal(const T& f, const T& q);
00237     bool about_equal(const quat& p, const quat& q);
00238 
00239 
00240     // Misc.
00241 
00242     bool interpol_ok(const quat& p, const quat& q, float h);
00243 
00244 
00245     // Linear Quaternion Interpolation.
00246 
00247     quat lerp(const quat& p, const quat& q, float h);
00248 
00249 
00250     // Spherical Linear Quaternion Interpolation.
00251 
00252     quat slerp(const quat& p, const quat& q, float h);
00253 
00254     quat slerp_2(const quat& p, const quat& q, float h);
00255 
00256     quat slerp_3(const quat& p, const quat& q, float h);
00257 
00258     quat slerp_4(const quat& p, const quat& q, float h);
00259 
00260     quat slerp_5(const quat& p, const quat& q, float h);
00261 
00262 
00263 # ifndef MLN_INCLUDE_ONLY
00264 
00265     // Constructors.
00266 
00267     inline
00268     quat::quat()
00269     {
00270     }
00271 
00272     inline
00273     quat::quat(float s, float x, float y, float z)
00274     {
00275       v_[0] = s;
00276       set_v(x, y, z);
00277     }
00278 
00279     inline
00280     quat::quat(float s, const algebra::vec<3,float>& v)
00281     {
00282       v_[0] = s;
00283       this->v() = v;
00284     }
00285 
00286     inline
00287     quat::quat(const algebra::vec<4,float>& v)
00288     {
00289       this->v_ = v;
00290     }
00291 
00292     inline
00293     quat&
00294     quat::operator=(const algebra::vec<4,float>& v)
00295     {
00296       this->v_ = v;
00297       return *this;
00298     }
00299 
00300 
00301     // With literals.
00302 
00303     inline
00304     quat::quat(const literal::zero_t&)
00305     {
00306       v_.set_all(0);
00307     }
00308 
00309     inline
00310     quat&
00311     quat::operator=(const literal::zero_t&)
00312     {
00313       v_.set_all(0);
00314       return *this;
00315     }
00316 
00317     inline
00318     quat::quat(const literal::one_t&)
00319     {
00320       s() = 1;
00321       v().set_all(0);
00322     }
00323 
00324     inline
00325     quat&
00326     quat::operator=(const literal::one_t&)
00327     {
00328       s() = 1;
00329       v().set_all(0);
00330       return *this;
00331     }
00332 
00333 
00334     inline
00335     const algebra::vec<4,float>&
00336     quat::to_vec() const
00337     {
00338       return this->v_;
00339     }
00340 
00341     inline
00342     quat::operator const algebra::vec<4,float>&() const
00343     {
00344       return this->v_;
00345     }
00346 
00347     inline
00348     float
00349     quat::s() const
00350     {
00351       return this->v_[0];
00352     }
00353 
00354     inline
00355     float&
00356     quat::s()
00357     {
00358       return this->v_[0];
00359     }
00360 
00361     inline
00362     const algebra::vec<3, float>&
00363     quat::v() const
00364     {
00365       return *(const algebra::vec<3, float>*)(const void*)(& this->v_[1]);
00366       // return make::vec(this->v_[1], this->v_[2], this->v_[3]);
00367     }
00368 
00369     inline
00370     algebra::vec<3, float>&
00371     quat::v()
00372     {
00373       return *(algebra::vec<3, float>*)(void*)(& this->v_[1]);
00374     }
00375 
00376     inline
00377     void quat::set_v(float x, float y, float z)
00378     {
00379       this->v_[1] = x;
00380       this->v_[2] = y;
00381       this->v_[3] = z;
00382     }
00383 
00384     inline
00385     float
00386     quat::sprod(const quat& rhs) const
00387     {
00388       return v_ * rhs.to_vec();
00389     }
00390 
00391     inline
00392     bool quat::is_unit() const
00393     {
00394       return about_equal(norm::l2(v_), 1.f);
00395     }
00396 
00397     inline
00398     bool quat::is_null() const
00399     {
00400       return about_equal(norm::l2(v_), 0.f);
00401     }
00402 
00403     inline
00404     bool quat::is_pure() const
00405     {
00406       return about_equal(v_[0], 0.f);
00407     }
00408 
00409     inline
00410     quat quat::conj() const
00411     {
00412       return quat(s(), - v());
00413     }
00414 
00415     inline
00416     quat quat::inv() const
00417     {
00418       mln_precondition(! is_null());
00419       float f = norm::l2(v_);
00420       return conj().to_vec() / (f * f);
00421     }
00422 
00423     inline
00424     quat& quat::set_unit()
00425     {
00426       if (about_equal(norm::l2(this->to_vec()), 0.f))
00427         return *this;
00428 
00429       v_.normalize();
00430       mln_postcondition(this->is_unit());
00431 
00432       return *this;
00433     }
00434 
00435     template <typename T>
00436     inline
00437     void quat::set_unit(float theta, const algebra::vec<3,T>& uv)
00438     {
00439       static const float pi = 3.14159265358979323846f;
00440 
00441       mln_precondition(theta > - pi - mln_epsilon(float)
00442                        && theta < pi + mln_epsilon(float));
00443       mln_precondition(about_equal(norm::l2(uv), 1.f));
00444       (void) pi;
00445 
00446       this->v_[0] = std::cos(theta);
00447       float sint = std::sin(theta);
00448       this->v_[1] = uv[0] * sint;
00449       this->v_[2] = uv[1] * sint;
00450       this->v_[3] = uv[2] * sint;
00451     }
00452 
00453     // only for unit quaternions described by theta and uv such as:
00454     // q = ( cos(theta), sin(theta) * uv )
00455 
00456     inline
00457     quat::quat(unsigned one, float theta, const algebra::vec<3,float>& uv)
00458     {
00459       mln_precondition(one == 1);
00460       (void) one;
00461       set_unit(theta, uv);
00462     }
00463 
00464     inline
00465     float quat::theta() const
00466     {
00467       mln_precondition(is_unit());
00468       return std::acos(s());
00469     }
00470 
00471     inline
00472     void quat::set_theta(float theta)
00473     {
00474       mln_precondition(is_unit());
00475       set_unit(theta, uv());
00476     }
00477 
00478     inline
00479     algebra::vec<3, float> quat::uv() const
00480     {
00481       mln_precondition(is_unit());
00482       algebra::vec<3, float> w = v();
00483       return w.normalize();
00484     }
00485 
00486     inline
00487     void quat::set_uv(const algebra::vec<3,float>& uv)
00488     {
00489       mln_precondition(is_unit());
00490       set_unit(theta(), uv);
00491     }
00492 
00493     template <unsigned n, typename T>
00494     inline
00495     algebra::vec<n,float>
00496     quat::rotate(const algebra::vec<n,T>& v) const
00497     {
00498       mln_precondition(is_unit());
00499       return ((*this) * quat(0. ,v) * (*this).inv()).v();
00500     }
00501 
00502     inline
00503     quat quat::rotate(const quat& q) const
00504     {
00505       mln_precondition(this->is_unit());
00506       mln_precondition(q.is_pure());
00507       return (*this) * q * this->inv();
00508     }
00509 
00510 
00511     // Operators.
00512 
00513     inline
00514     std::ostream& operator<<(std::ostream& ostr, const quat& q)
00515     {
00516       return ostr << q.to_vec();
00517     }
00518 
00519     inline
00520     quat operator+(const quat& lhs, const quat& rhs)
00521     {
00522       quat tmp(lhs.to_vec() + rhs.to_vec());
00523       return tmp;
00524     }
00525 
00526     inline
00527     quat operator-(const quat& lhs, const quat& rhs)
00528     {
00529       quat tmp(lhs.to_vec() - rhs.to_vec());
00530       return tmp;
00531     }
00532 
00533     inline
00534     quat operator*(const quat& lhs, const quat& rhs)
00535     {
00536       quat tmp(lhs.s() * rhs.s() - lhs.v() * rhs.v(),
00537                algebra::vprod(lhs.v(), rhs.v()) + lhs.s() * rhs.v() + rhs.s() * lhs.v());
00538       return tmp;
00539     }
00540 
00541     template <typename S>
00542     inline
00543     quat operator*(const quat& lhs, const value::scalar_<S>& rhs)
00544     {
00545       mlc_converts_to(S, float)::check();
00546       quat tmp(lhs.to_vec() * rhs.to_equiv());
00547       return tmp;
00548     }
00549 
00550     template <typename S>
00551     inline
00552     quat operator/(const quat& lhs, const value::scalar_<S>& rhs_)
00553     {
00554       mlc_converts_to(S, float)::check();
00555       float rhs = rhs_.to_equiv();
00556       mln_precondition(rhs != 0.f);
00557       quat tmp(lhs.to_vec() / rhs);
00558       return tmp;
00559     }
00560 
00561 
00562     // overloaded math procs
00563 
00564     inline
00565     quat log(const quat& q)
00566     {
00567       mln_precondition(q.is_unit());
00568       return quat(0.f, q.theta() * q.uv());
00569     }
00570 
00571 
00572     inline
00573     quat exp(const quat& q)
00574     {
00575       mln_precondition(about_equal(q.s(), 0.f));
00576       algebra::vec<3, float> v = q.v();
00577       float theta = norm::l2(v);
00578       mln_precondition(!about_equal(theta, 0.f));
00579       algebra::vec<3, float> uv = v / theta;
00580       return quat(std::cos(theta), std::sin(theta) * uv);
00581     }
00582 
00583 
00584     inline
00585     quat pow(const quat& q, double t)
00586     {
00587       mln_precondition(q.is_unit());
00588       return exp(t * log(q));
00589     }
00590 
00591     template <typename T>
00592     inline
00593     bool about_equal(const T& f, const T& q)
00594     {
00595       return math::abs(q - f) <= mln_epsilon(T);
00596     }
00597 
00598     inline
00599     bool about_equal(const quat& p, const quat& q)
00600     {
00601       return about_equal<float>(norm::l2(p.to_vec() - q.to_vec()), 0);
00602     }
00603 
00604     // Misc.
00605 
00606     inline
00607     bool interpol_ok(const quat& p, const quat& q, float h)
00608     {
00609       return
00610         p.is_unit() &&
00611         q.is_unit() &&
00612         h >= 0 &&
00613         h <= 1;
00614     }
00615 
00616 
00617     // Linear Quaternion Interpolation.
00618 
00619     inline
00620     quat lerp(const quat& p, const quat& q, float h)
00621     {
00622       assert(interpol_ok(p, q, h));
00623       return (1 - h) * p + h * q;
00624     }
00625 
00626 
00627     // Spherical Linear Quaternion Interpolation.
00628 
00629     inline
00630     quat slerp(const quat& p, const quat& q, float h)
00631     {
00632       assert(interpol_ok(p, q, h));
00633       float omega = std::acos(p.sprod(q));
00634       return
00635         about_equal(omega, 0.f) ?
00636         lerp(p, q, h) :
00637         quat((std::sin((1-h)*omega) * p + std::sin(h*omega) * q) / std::sin(omega));
00638     }
00639 
00640     inline
00641     quat slerp_2(const quat& p, const quat& q, float h)
00642     {
00643       assert(interpol_ok(p, q, h));
00644       quat tmp = p * pow(p.conj() * q, h);
00645       assert(about_equal(tmp, slerp(p, q, h)));
00646       return tmp;
00647     }
00648 
00649     inline
00650     quat slerp_3(const quat& p, const quat& q, float h)
00651     {
00652       assert(interpol_ok(p, q, h));
00653       quat tmp = pow(p * q.conj(), 1 - h) * q;
00654       assert(about_equal(tmp, slerp(p, q, h)));
00655       return tmp;
00656     }
00657 
00658     inline
00659     quat slerp_4(const quat& p, const quat& q, float h)
00660     {
00661       assert(interpol_ok(p, q, h));
00662       quat tmp = pow(q * p.conj(), h) * p;
00663       assert(about_equal(tmp, slerp(p, q, h)));
00664       return tmp;
00665     }
00666 
00667     inline
00668     quat slerp_5(const quat& p, const quat& q, float h)
00669     {
00670       assert(interpol_ok(p, q, h));
00671       quat tmp = q * pow(q.conj() * p, 1 - h);
00672       assert(about_equal(tmp, slerp(p, q, h)));
00673       return tmp;
00674     }
00675 
00676 # endif // ! MLN_INCLUDE_ONLY
00677 
00678   } // end of namespace mln::algebra
00679 
00680 } // end of namespace mln
00681 
00682 #endif // ! MLN_ALGEBRA_QUAT_HH