BALL
1.4.1
|
00001 // -*- Mode: C++; tab-width: 2; -*- 00002 // vi: set ts=2: 00003 00004 #ifndef BALL_MATHS_QUATERNION_H 00005 #define BALL_MATHS_QUATERNION_H 00006 00007 #ifndef BALL_MATHS_MATRIX44_H 00008 # include <BALL/MATHS/matrix44.h> 00009 #endif 00010 00011 #ifndef BALL_MATHS_VECTOR3_H 00012 # include <BALL/MATHS/vector3.h> 00013 #endif 00014 00015 #include <boost/math/quaternion.hpp> 00016 #include <iostream> 00017 00018 namespace BALL 00019 { 00024 00029 template <typename T> 00030 class TQuaternion 00031 : public boost::math::quaternion<T> //{/*...*/}; 00032 { 00033 public: 00034 00035 BALL_CREATE(TQuaternion<T>) 00036 00037 00040 00045 TQuaternion(); 00046 00051 TQuaternion(const TQuaternion& q); 00052 00057 TQuaternion(const boost::math::quaternion<T>& q); 00058 00066 TQuaternion(const T& w, const T& i, const T& j, const T& k); 00067 00073 TQuaternion(const TVector3<T>& axis, const T& angle); 00074 00079 ~TQuaternion(); 00080 00084 void clear(); 00085 00087 00090 00092 void set(const TQuaternion& q); 00093 00095 void set(const boost::math::quaternion<T>& q); 00096 00102 BALL_DEPRECATED 00103 void set(const TVector3<T>& axis, const T& angle); 00104 00111 void set(const T& w, const T& i, const T& j, const T& k); 00112 00116 TQuaternion& operator = (const TQuaternion& q); 00117 00121 TQuaternion& operator = (const boost::math::quaternion<T>& q); 00122 00127 void setIdentity(); 00128 00133 TQuaternion<T>& normalize(); 00134 00138 void swap(TQuaternion& q); 00139 00144 void fromAxisAngle(const TVector3<T>& axis, const T& angle); 00145 00153 void fromEulerAngles(const T& yaw, const T& pitch, const T& roll); 00154 00160 void toAxisAngle(TVector3<T>& axis, T& angle); 00161 00169 void toEulerAngles(T& yaw, T& pitch, T& roll); 00170 //void toEA(T& yaw, T& pitch, T& roll); 00171 00176 void get(TQuaternion& q) const; 00177 00181 T getAngle() const; 00182 00186 TVector3<T> getAxis(); 00187 00192 TMatrix4x4<T>& getRotationMatrix(TMatrix4x4<T>& m) const; 00193 00197 TQuaternion getInverse() const; 00198 00203 TQuaternion getConjugate() const; 00205 00208 00212 T& w(); 00213 00217 const T& w() const; 00218 00222 T& i(); 00223 00227 const T& i() const; 00228 00232 T& j(); 00233 00237 const T& j() const; 00238 00242 T& k(); 00243 00247 const T& k() const; 00248 00249 00251 00254 00261 void dump(std::ostream& s = std::cout, Size depth = 0) const; 00263 00264 }; 00266 00267 template <typename T> 00268 TQuaternion<T>::TQuaternion() 00269 : boost::math::quaternion<T>() 00270 { 00271 this->setIdentity(); 00272 } 00273 00274 template <typename T> 00275 TQuaternion<T>::TQuaternion(const TQuaternion& q) 00276 : boost::math::quaternion<T>(q) 00277 { 00278 } 00279 00280 template <typename T> 00281 TQuaternion<T>::TQuaternion(const boost::math::quaternion<T>& q) 00282 : boost::math::quaternion<T>(q) 00283 { 00284 } 00285 00286 template <typename T> 00287 TQuaternion<T>::TQuaternion(const T& w, const T& i, const T& j, const T& k) 00288 : boost::math::quaternion<T>(w, i, j, k) 00289 { 00290 } 00291 00292 template <typename T> 00293 TQuaternion<T>::TQuaternion(const TVector3<T>& axis, const T& angle) 00294 : boost::math::quaternion<T>() 00295 { 00296 fromAxisAngle(axis, angle); 00297 } 00298 00299 template <typename T> 00300 TQuaternion<T>::~TQuaternion() 00301 { 00302 } 00303 00304 template <typename T> 00305 void TQuaternion<T>::clear() 00306 { 00307 this->setIdentity(); 00308 } 00309 00310 template <typename T> 00311 void TQuaternion<T>::set(const TQuaternion<T>& q) 00312 { 00313 boost::math::quaternion<T>::operator= (q); 00314 } 00315 00316 template <typename T> 00317 void TQuaternion<T>::set(const boost::math::quaternion<T>& q) 00318 { 00319 boost::math::quaternion<T>::operator= (q); 00320 } 00321 00322 template <typename T> 00323 BALL_INLINE 00324 void TQuaternion<T>::set(const TVector3<T>& axis, const T& angle) 00325 { 00326 fromAxisAngle(axis, angle); 00327 } 00328 00329 template <typename T> 00330 BALL_INLINE 00331 void TQuaternion<T>::set(const T& w, const T& i, const T& j, const T& k) 00332 { 00333 this->a = w; 00334 this->b = i; 00335 this->c = j; 00336 this->d = k; 00337 00338 } 00339 00340 template <typename T> 00341 BALL_INLINE 00342 TQuaternion<T>& TQuaternion<T>::operator = (const boost::math::quaternion<T>& q) 00343 { 00344 set(q); 00345 return *this; 00346 } 00347 00348 template <typename T> 00349 BALL_INLINE 00350 TQuaternion<T>& TQuaternion<T>::operator = (const TQuaternion<T>& q) 00351 { 00352 set(q); 00353 return *this; 00354 } 00355 00356 template <typename T> 00357 BALL_INLINE 00358 void TQuaternion<T>::setIdentity() 00359 { 00360 this->a = (T)1; 00361 this->b = this->c = this->d = (T)0; 00362 } 00363 00364 template <typename T> 00365 BALL_INLINE 00366 TQuaternion<T>& TQuaternion<T>::normalize() 00367 { 00368 T length = boost::math::norm(*this); 00369 00370 if (!(Maths::isEqual(length, (T)0))) 00371 { 00372 this->a /= length; 00373 this->b /= length; 00374 this->c /= length; 00375 this->d /= length; 00376 } 00377 return *this; 00378 } 00379 00380 template <typename T> 00381 void TQuaternion<T>::swap(TQuaternion<T>& q) 00382 { 00383 T tmp = q.a; 00384 q.a = this->a; 00385 this->a = tmp; 00386 00387 tmp = q.b; 00388 q.b = this->b; 00389 this->b = tmp; 00390 00391 tmp = q.c; 00392 q.c = this->c; 00393 this->c = tmp; 00394 00395 tmp = q.d; 00396 q.d = this->d; 00397 this->d = tmp; 00398 } 00399 00400 template <typename T> 00401 void TQuaternion<T>::fromAxisAngle(const TVector3<T>& axis, const T& angle) 00402 { 00403 T length = axis.getLength(); 00404 00405 if (Maths::isEqual(length, (T)0)) 00406 { 00407 this->b = this->c = this->d = (T)0; 00408 this->a = (T)1; 00409 } 00410 else 00411 { 00412 T omega = (T) (angle * 0.5); 00413 T sin_omega = (T)::sin(omega); 00414 00415 this->a = (T)::cos(omega); 00416 this->b = axis.x * sin_omega / length; 00417 this->c = axis.y * sin_omega / length; 00418 this->d = axis.z * sin_omega / length; 00419 } 00420 } 00421 00422 template <typename T> 00423 void TQuaternion<T>::fromEulerAngles(const T& yaw, const T& pitch, const T& roll) 00424 { 00425 T half_yaw = yaw / 2.0; 00426 T half_pitch = pitch / 2.0; 00427 T half_roll = roll / 2.0; 00428 00429 T cosYaw = cos(half_yaw); 00430 T sinYaw = sin(half_yaw); 00431 00432 T cosPitch = cos(half_pitch); 00433 T sinPitch = sin(half_pitch); 00434 00435 T cosRoll = cos(half_roll); 00436 T sinRoll = sin(half_roll); 00437 00438 00439 this->a = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; 00440 this->b = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; 00441 this->c = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; 00442 this->d = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; 00443 00444 } 00445 00446 template <typename T> 00447 void TQuaternion<T>::toAxisAngle(TVector3<T>& axis, T& angle) 00448 { 00449 T length = sqrt(this->b*this->b + this->c*this->c + this->d*this->d); 00450 00451 if (Maths::isEqual(length, (T)0)) 00452 { 00453 axis.x = axis.y = angle= (T)0; 00454 axis.y = (T)1; 00455 } 00456 else 00457 { 00458 angle = 2 * (T)::acos(this->a); 00459 axis.x = this->b / length; 00460 axis.y = this->c / length; 00461 axis.z = this->d / length; 00462 } 00463 } 00464 00465 00466 template <typename T> 00467 void TQuaternion<T>::toEulerAngles(T& yaw, T& pitch, T& roll) 00468 { 00469 TMatrix4x4<T> matrix; 00470 getRotationMatrix(matrix); 00471 T sinYaw, cosYaw, sinPitch, cosPitch, sinRoll, cosRoll; 00472 00473 sinPitch = -matrix(2,0); 00474 cosPitch = sqrt(1 - sinPitch*sinPitch); 00475 00476 if ( fabs(cosPitch) > Constants::EPSILON) 00477 { 00478 sinRoll = matrix(2,1) / cosPitch; 00479 cosRoll = matrix(2,2) / cosPitch; 00480 sinYaw = matrix(1,0) / cosPitch; 00481 cosYaw = matrix(0,0) / cosPitch; 00482 } 00483 else 00484 { 00485 sinRoll = -matrix(1,2); 00486 cosRoll = matrix(1,1); 00487 sinYaw = 0; 00488 cosYaw = 1; 00489 } 00490 00491 /* yaw */ 00492 yaw = atan2(sinYaw, cosYaw); 00493 00494 /* pitch */ 00495 pitch = atan2(sinPitch, cosPitch); 00496 00497 /* roll */ 00498 roll = atan2(sinRoll, cosRoll); 00499 00500 } 00501 00502 template <typename T> 00503 BALL_INLINE 00504 void TQuaternion<T>::get(TQuaternion<T>& q) const 00505 { 00506 q.set(*this); 00507 } 00508 00509 template <typename T> 00510 T TQuaternion<T>::getAngle() const 00511 { 00512 T length = sqrt(this->b*this->b + this->c*this->c + this->d*this->d); 00513 if (Maths::isEqual(length, (T)0)) 00514 { 00515 return (T)(0); 00516 } 00517 else 00518 { 00519 return (T)(2 * (T)::acos(this->a)); 00520 } 00521 } 00522 00523 template <typename T> 00524 TVector3<T> TQuaternion<T>::getAxis() 00525 { 00526 T length = sqrt(this->b*this->b + this->c*this->c + this->d*this->d); 00527 if (Maths::isEqual(length, (T)0)) 00528 { 00529 return TVector3<T>((T)0,(T)0,(T)1); 00530 } 00531 else 00532 { 00533 return TVector3<T>(this->b/length, this->c/length, this->d/length); 00534 } 00535 } 00536 00537 template <typename T> 00538 TMatrix4x4<T>& TQuaternion<T>::getRotationMatrix(TMatrix4x4<T>& m) const 00539 { 00540 T s = 2.0 / boost::math::norm(*this); 00541 m.set 00542 ( 00543 (T)(1.0 - s * (this->c * this->c + this->d * this->d)), 00544 (T)(s * (this->b * this->c - this->d * this->a)), 00545 (T)(s * (this->d * this->b + this->c * this->a)), 00546 (T)0, 00547 00548 (T)(s * (this->b * this->c + this->d * this->a)), 00549 (T)(1.0 - s * (this->d * this->d + this->b * this->b)), 00550 (T)(s * (this->c * this->d - this->b * this->a)), 00551 (T)0, 00552 00553 (T)(s * (this->d * this->b - this->c * this->a)), 00554 (T)(s * (this->c * this->d + this->b * this->a)), 00555 (T)(1.0 - s * (this->c * this->c + this->b * this->b)), 00556 (T)0, 00557 00558 (T)0, 00559 (T)0, 00560 (T)0, 00561 (T)1 00562 ); 00563 00564 return m; 00565 } 00566 00567 template <typename T> 00568 BALL_INLINE 00569 TQuaternion<T> TQuaternion<T>::getInverse() const 00570 { 00571 00572 return conj(*this) / boost::math::norm(*this); 00573 } 00574 00575 template <typename T> 00576 BALL_INLINE 00577 TQuaternion<T> TQuaternion<T>::getConjugate() const 00578 { 00579 return conj(*this); 00580 } 00581 00582 template <typename T> 00583 const T& TQuaternion<T>::w() const 00584 { 00585 return this->a; 00586 } 00587 00588 template <typename T> 00589 T& TQuaternion<T>::w() 00590 { 00591 return this->a; 00592 } 00593 00594 template <typename T> 00595 const T& TQuaternion<T>::i() const 00596 { 00597 return this->b; 00598 } 00599 00600 template <typename T> 00601 T& TQuaternion<T>::i() 00602 { 00603 return this->b; 00604 } 00605 00606 template <typename T> 00607 const T& TQuaternion<T>::j() const 00608 { 00609 return this->c; 00610 } 00611 00612 template <typename T> 00613 T& TQuaternion<T>::j() 00614 { 00615 return this->c; 00616 } 00617 00618 template <typename T> 00619 const T& TQuaternion<T>::k() const 00620 { 00621 return this->d; 00622 } 00623 00624 template <typename T> 00625 T& TQuaternion<T>::k() 00626 { 00627 return this->d; 00628 } 00629 00630 template <typename T> 00631 std::istream& operator >>(std::istream& s, TQuaternion<T>& q) 00632 00633 { 00634 char c; 00635 s >> c >> q.w() >> c >> q.i() >> c >> q.j() >> c >>q.k() >> c; 00636 return s; 00637 } 00638 00639 template <typename T> 00640 std::ostream& operator << (std::ostream& s, const TQuaternion<T>& q) 00641 00642 { 00643 s << '(' << q.w() << ',' << q.i() << ',' 00644 << q.j() << ',' << q.k() << ')'; 00645 00646 return s; 00647 } 00648 00649 template <typename T> 00650 void TQuaternion<T>::dump(std::ostream& s, Size depth) const 00651 { 00652 BALL_DUMP_STREAM_PREFIX(s); 00653 00654 BALL_DUMP_HEADER(s, this, this); 00655 00656 BALL_DUMP_DEPTH(s, depth); 00657 s << " w: " << this->w() << std::endl; 00658 00659 BALL_DUMP_DEPTH(s, depth); 00660 s << " i: " << this->i() << std::endl; 00661 00662 BALL_DUMP_DEPTH(s, depth); 00663 s << " j: " << this->j() << std::endl; 00664 00665 BALL_DUMP_DEPTH(s, depth); 00666 s << " k: " << this->k() << std::endl; 00667 00668 BALL_DUMP_STREAM_SUFFIX(s); 00669 } 00670 00671 00672 00673 typedef TQuaternion<float> Quaternion; 00674 00675 } // namespace BALL 00676 00677 #endif // BALL_MATHS_QUATERNION_H