00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
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
00048
00049
00050 namespace mln
00051 {
00052
00053
00054 namespace algebra { class quat; }
00055 namespace literal { struct zero_t; struct one_t; }
00056
00057
00058 namespace trait
00059 {
00060
00061
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
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
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 }
00116
00117
00118
00119 namespace algebra
00120 {
00121
00122
00123 class quat
00124 :
00125 public value::Vectorial< quat >
00126 ,
00127 public value::internal::value_like_< algebra::vec<4, float>,
00128 algebra::vec<4, float>,
00129 algebra::vec<4, float>,
00130 quat >
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;
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
00208
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
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
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
00241
00242 bool interpol_ok(const quat& p, const quat& q, float h);
00243
00244
00245
00246
00247 quat lerp(const quat& p, const quat& q, float h);
00248
00249
00250
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
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
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
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
00454
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
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
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
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
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
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 }
00679
00680 }
00681
00682 #endif // ! MLN_ALGEBRA_QUAT_HH