Milena (Olena)
User documentation 2.0a Id
|
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