C:/Active/libraries/cxutils/2.0/src/cxutils/math/quaternion.cpp
Go to the documentation of this file.
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 00027 00028 00029 00030 00031 00032 00033 00034 00035 00036 00037 00038 00039 00040 #include "cxutils/math/quaternion.h" 00041 #include "cxutils/math/cxmath.h" 00042 #include <math.h> 00043 #include <iostream> 00044 #include <iomanip> 00045 #include <assert.h> 00046 00052 CxUtils::Quaternion CxUtils::operator~(const CxUtils::Quaternion& q) 00053 { 00054 return CxUtils::Quaternion(q.mW, -q.mX, -q.mY, -q.mZ); 00055 } 00056 00057 using namespace CxUtils; 00058 00064 Quaternion::Quaternion() 00065 { 00066 mW = 1; 00067 mX = mY = mZ = 0; 00068 } 00069 00070 00076 Quaternion::Quaternion(const Quaternion& q) 00077 { 00078 mW = q.mW; 00079 mX = q.mX; 00080 mY = q.mY; 00081 mZ = q.mZ; 00082 } 00083 00084 00095 Quaternion::Quaternion(const Point3D& axis, 00096 const double angle, 00097 const bool degrees) 00098 { 00099 Point3D unit; 00100 mW = (degrees ? cos(CX_DEG2RAD(angle)/2.0) : cos(angle/2.0)); 00101 unit = axis; 00102 unit.Normalize(); 00103 unit *= (degrees ? sin(CX_DEG2RAD(angle)/2.0) : sin(angle/2.0)); 00104 mX = unit.mX; 00105 mY = unit.mY; 00106 mZ = unit.mZ; 00107 } 00108 00109 00115 Quaternion::Quaternion(const double w, 00116 const double x, 00117 const double y, 00118 const double z) 00119 { 00120 mW = w; 00121 mX = x; 00122 mY = y; 00123 mZ = z; 00124 } 00125 00126 00137 Quaternion::Quaternion(const double x, 00138 const double y, 00139 const double z) 00140 { 00141 CreateFromEuler(x, y, z, false); 00142 } 00143 00144 00153 Quaternion::Quaternion(const Point3D& p, const bool degrees) 00154 { 00155 CreateFromEuler(p.mX, p.mY, p.mZ, degrees); 00156 } 00157 00158 00169 void Quaternion::CreateRotation(const Point3D& axis, 00170 const double angle, 00171 const bool degrees) 00172 { 00173 Point3D unit; 00174 mW = (degrees ? cos(CX_DEG2RAD(angle)/2.0) : cos(angle/2.0)); 00175 unit = axis; 00176 unit.Normalize(); 00177 unit *= (degrees ? sin(CX_DEG2RAD(angle)/2.0) : sin(angle/2.0)); 00178 mX = unit.mX; 00179 mY = unit.mY; 00180 mZ = unit.mZ; 00181 } 00182 00183 00194 void Quaternion::CreateFromEuler(const double x, 00195 const double y, 00196 const double z, 00197 bool degrees) 00198 { 00199 00200 double cosHalfTheta, sinHalfTheta; 00201 double cosHalfPsi, sinHalfPsi; 00202 double cosHalfPhi, sinHalfPhi; 00203 if(degrees) 00204 { 00205 cosHalfTheta = cos(CX_DEG2RAD(y)/2.0); sinHalfTheta = sin(CX_DEG2RAD(y)/2.0); 00206 cosHalfPsi = cos(CX_DEG2RAD(z)/2.0); sinHalfPsi = sin(CX_DEG2RAD(z)/2.0); 00207 cosHalfPhi = cos(CX_DEG2RAD(x)/2.0); sinHalfPhi = sin(CX_DEG2RAD(x)/2.0); 00208 } 00209 else 00210 { 00211 cosHalfTheta = cos(y/2.0); sinHalfTheta = sin(y/2.0); 00212 cosHalfPsi = cos(z/2.0); sinHalfPsi = sin(z/2.0); 00213 cosHalfPhi = cos(x/2.0); sinHalfPhi = sin(x/2.0); 00214 } 00215 00216 // Convet from Euler to quaternion. 00217 mW = cosHalfPhi*cosHalfTheta*cosHalfPsi + sinHalfPhi*sinHalfTheta*sinHalfPsi; 00218 mX = sinHalfPhi*cosHalfTheta*cosHalfPsi - cosHalfPhi*sinHalfTheta*sinHalfPsi; 00219 mY = cosHalfPhi*sinHalfTheta*cosHalfPsi + sinHalfPhi*cosHalfTheta*sinHalfPsi; 00220 mZ = cosHalfPhi*cosHalfTheta*sinHalfPsi - sinHalfPhi*sinHalfTheta*cosHalfPsi; 00221 } 00222 00223 00232 void Quaternion::CreateFromEuler(const Point3D& p, 00233 bool degrees) 00234 { 00235 CreateFromEuler(p.mX, p.mY, p.mZ, degrees); 00236 } 00237 00238 00247 void Quaternion::SetRotationZ(const double z, const bool degrees) 00248 { 00249 mX = 0; 00250 mY = 0; 00251 mZ = (degrees ? sin(CX_DEG2RAD(z/2.0)) : sin(z/2.0)); 00252 mW = (degrees ? cos(CX_DEG2RAD(z/2.0)) : cos(z/2.0)); 00253 } 00254 00255 00264 void Quaternion::SetRotationY(const double y, const bool degrees) 00265 { 00266 mZ= 0; 00267 mX = 0; 00268 mY = (degrees ? sin(CX_DEG2RAD(y/2.0)) : sin(y/2.0)); 00269 mW = (degrees ? cos(CX_DEG2RAD(y/2.0)) : cos(y/2.0)); 00270 } 00271 00272 00281 void Quaternion::SetRotationX(const double x, const bool degrees ) 00282 { 00283 mY = 0; 00284 mZ = 0; 00285 mX = (degrees ? sin(CX_DEG2RAD(x/2.0)) : sin(x/2.0)); 00286 mW = (degrees ? cos(CX_DEG2RAD(x/2.0)) : cos(x/2.0)); 00287 } 00288 00289 00300 void Quaternion::ConvertToEuler(double& x, 00301 double& y, 00302 double& z, 00303 bool degrees) const 00304 { 00305 x = atan2( (2.0*(mW*mX + mY*mZ)) , (1.0 - 2.0*(mX*mX + mY*mY)) ); 00306 double t = 2.0*(mW*mY - mZ*mX); 00307 if(t < -1.0) 00308 t = -1.0; 00309 if(t > 1.0) 00310 t = 1.0; 00311 y = asin(t); 00312 z = atan2( (2.0*(mW*mZ + mX*mY)) , (1.0 - 2.0*(mY*mY + mZ*mZ)) ); 00313 00314 if(degrees) 00315 { 00316 x = CX_RAD2DEG(x); 00317 y = CX_RAD2DEG(y); 00318 z = CX_RAD2DEG(z); 00319 } 00320 } 00321 00322 00331 void Quaternion::ConvertToEuler(Point3D& p, 00332 bool degrees) const 00333 { 00334 ConvertToEuler(p.mX, p.mY, p.mZ, degrees); 00335 } 00336 00346 Point3D Quaternion::ConvertToEuler(bool degrees) const 00347 { 00348 double 00349 x, y, z; 00350 ConvertToEuler(x, y, z, degrees); 00351 return Point3D(x, y, z); 00352 } 00353 00359 void Quaternion::Clear() 00360 { 00361 mW = mX = mY = mZ = 0; 00362 } 00363 00364 00375 void Quaternion::Print(const bool euler, const bool degrees) const 00376 { 00377 if(euler) 00378 { 00379 Point3D e; 00380 ConvertToEuler(e, degrees); 00381 e.Print(); 00382 } 00383 else 00384 std::cout << "<" << mW << ", " << mX << ", " << mY << ", " << mZ << ">\n"; 00385 } 00386 00387 00393 double Quaternion::Norm() const 00394 { 00395 return mW*mW + mX*mX + mY*mY + mZ*mZ; 00396 } 00397 00398 00406 double Quaternion::Norm(const Quaternion& q) 00407 { 00408 return q.mW*q.mW + q.mX*q.mX + q.mY*q.mY + q.mZ*q.mZ; 00409 } 00410 00411 00421 double Quaternion::Dot(const Quaternion& q) const 00422 { 00423 return mW*q.mW + mX*q.mX + mY*q.mY + mZ*q.mZ; 00424 } 00425 00426 00437 double Quaternion::Dot(const Quaternion& a, const Quaternion& b) 00438 { 00439 return a.mW*b.mW + a.mX*b.mX + a.mY*b.mY + a.mZ*b.mZ; 00440 } 00441 00442 00448 Point3D Quaternion::GetAxis() const 00449 { 00450 return Point3D(mX, mY, mZ); 00451 } 00452 00453 00459 Quaternion Quaternion::Normalize() const 00460 { 00461 double n = sqrt(mW*mW + mX*mX + mY*mY + mZ*mZ); 00462 // Prevent division by zero. 00463 if(n < CX_EPSILON) 00464 n = CX_EPSILON; 00465 return Quaternion(mW/n, mX/n, mY/n, mZ/n); 00466 } 00467 00468 00478 Quaternion Quaternion::Normalize(const Quaternion& q) 00479 { 00480 double n = sqrt(q.mW*q.mW + q.mX*q.mX + q.mY*q.mY + q.mZ*q.mZ); 00481 Quaternion r; 00482 // Prevent division by zero. 00483 if(n < CX_EPSILON) 00484 n = CX_EPSILON; 00485 r.mW = q.mW/n; 00486 r.mX = q.mX/n; 00487 r.mY = q.mY/n; 00488 r.mZ = q.mZ/n; 00489 return r; 00490 } 00491 00492 00498 Quaternion Quaternion::Conjugate() const 00499 { 00500 return Quaternion(mW, -mX, -mY, -mZ); 00501 } 00502 00503 00511 Quaternion Quaternion::Conjugate(const Quaternion& q) 00512 { 00513 Quaternion r; 00514 r.mW = q.mW; 00515 r.mX = -q.mX; 00516 r.mY = -q.mY; 00517 r.mZ = -q.mZ; 00518 return r; 00519 } 00520 00521 00527 Quaternion Quaternion::Invert() const 00528 { 00529 double base = mW*mW + mX*mX + mY*mY + mZ*mZ; 00530 // Prevent divide by zero. 00531 if(base < CX_EPSILON) 00532 base = CX_EPSILON; 00533 return Quaternion(mW/base, -mX/base, -mY/base, -mZ/base); 00534 } 00535 00536 00544 Quaternion Quaternion::Invert(const Quaternion& q) 00545 { 00546 Quaternion r; 00547 double base = q.mW*q.mW + q.mX*q.mX + q.mY*q.mY + q.mZ*q.mZ; 00548 // Prevent divide by zero. 00549 if(base < CX_EPSILON) 00550 base = CX_EPSILON; 00551 00552 r.mW = q.mW/base; 00553 r.mX = -q.mX/base; 00554 r.mY = -q.mY/base; 00555 r.mZ = -q.mZ/base; 00556 00557 return r; 00558 } 00559 00560 00570 Point3D Quaternion::Rotate(const Point3D& p) const 00571 { 00572 return (((*this)*p)*(~(*this))).GetAxis(); 00573 } 00574 00575 00585 Point3D Quaternion::RotateInverse(const Point3D& p) const 00586 { 00587 Quaternion q(mW, -mX, -mY, -mZ); 00588 return ((q*p)*(*this)).GetAxis(); 00589 } 00590 00591 00603 Quaternion Quaternion::Rotate(const Quaternion& q1, const Quaternion& q2) 00604 { 00605 return q1*q2*(~q1); 00606 } 00607 00608 00614 Quaternion& Quaternion::operator+=(const Quaternion& q) 00615 { 00616 mW += q.mW; 00617 mX += q.mX; 00618 mY += q.mY; 00619 mZ += q.mZ; 00620 return *this; 00621 } 00622 00623 00629 Quaternion Quaternion::operator+(const Quaternion& q) const 00630 { 00631 Quaternion result; 00632 result.mW = mW + q.mW; 00633 result.mX = mX + q.mX; 00634 result.mY = mY + q.mY; 00635 result.mZ = mZ + q.mZ; 00636 return result; 00637 } 00638 00639 00645 Quaternion& Quaternion::operator-=(const Quaternion& q) 00646 { 00647 mW -= q.mW; 00648 mX -= q.mX; 00649 mY -= q.mY; 00650 mZ -= q.mZ; 00651 return *this; 00652 } 00653 00654 00660 Quaternion Quaternion::operator-(const Quaternion& q) const 00661 { 00662 Quaternion result; 00663 result.mW = mW - q.mW; 00664 result.mX = mX - q.mX; 00665 result.mY = mY - q.mY; 00666 result.mZ = mZ - q.mZ; 00667 return result; 00668 } 00669 00670 00676 Quaternion &Quaternion::operator*=(const Quaternion& q) 00677 { 00678 Quaternion t; 00679 t.mW = (mW*q.mW - mX*q.mX - mY*q.mY - mZ*q.mZ); 00680 t.mX = (mW*q.mX + mX*q.mW + mY*q.mZ - mZ*q.mY); 00681 t.mY = (mW*q.mY - mX*q.mZ + mY*q.mW + mZ*q.mX); 00682 t.mZ = (mW*q.mZ + mX*q.mY - mY*q.mX + mZ*q.mW); 00683 mW = t.mW; 00684 mX = t.mX; 00685 mY = t.mY; 00686 mZ = t.mZ; 00687 return *this; 00688 } 00689 00690 00696 Quaternion Quaternion::operator*(const Quaternion& q) const 00697 { 00698 Quaternion t; 00699 t.mW = (mW*q.mW - mX*q.mX - mY*q.mY - mZ*q.mZ); 00700 t.mX = (mW*q.mX + mX*q.mW + mY*q.mZ - mZ*q.mY); 00701 t.mY = (mW*q.mY - mX*q.mZ + mY*q.mW + mZ*q.mX); 00702 t.mZ = (mW*q.mZ + mX*q.mY - mY*q.mX + mZ*q.mW); 00703 return t; 00704 } 00705 00706 00712 Quaternion& Quaternion::operator*=(const Point3D& p) 00713 { 00714 Quaternion q(0, p.mX, p.mY, p.mZ); 00715 *this *= q; 00716 return *this; 00717 } 00718 00719 00725 Quaternion Quaternion::operator*(const Point3D& p) const 00726 { 00727 Quaternion q(0, p.mX, p.mY, p.mZ); 00728 return (*this)*q; 00729 } 00730 00731 00737 Quaternion& Quaternion::operator/=(const Quaternion& q) 00738 { 00739 Quaternion t; 00740 double base = q.mW*q.mW + q.mX*q.mX + q.mY*q.mY + q.mZ*q.mZ; 00741 // Prevent divide by zero. 00742 if(base < CX_EPSILON) 00743 base = CX_EPSILON; 00744 t.mW = (q.mW*mW + q.mX*mX + q.mY*mY + q.mZ*mZ)/base; 00745 t.mX = (q.mW*mX - q.mX*mW - q.mY*mZ + q.mZ*mY)/base; 00746 t.mY = (q.mW*mY + q.mX*mZ - q.mY*mW - q.mZ*mX)/base; 00747 t.mZ = (q.mW*mZ - q.mX*mY + q.mY*mX - q.mZ*mW)/base; 00748 mW = t.mW; 00749 mX = t.mX; 00750 mY = t.mY; 00751 mZ = t.mZ; 00752 00753 return *this; 00754 } 00755 00756 00762 Quaternion Quaternion::operator/(const Quaternion& q) const 00763 { 00764 Quaternion t; 00765 double base = q.mW*q.mW + q.mX*q.mX + q.mY*q.mY + q.mZ*q.mZ; 00766 // Prevent divide by zero. 00767 if(base < CX_EPSILON) 00768 base = CX_EPSILON; 00769 t.mW = (q.mW*mW + q.mX*mX + q.mY*mY + q.mZ*mZ)/base; 00770 t.mX = (q.mW*mX - q.mX*mW - q.mY*mZ + q.mZ*mY)/base; 00771 t.mY = (q.mW*mY + q.mX*mZ - q.mY*mW - q.mZ*mX)/base; 00772 t.mZ = (q.mW*mZ - q.mX*mY + q.mY*mX - q.mZ*mW)/base; 00773 00774 return t; 00775 } 00776 00782 Quaternion& Quaternion::operator=(const Quaternion& q) 00783 { 00784 mW = q.mW; 00785 mX = q.mX; 00786 mY = q.mY; 00787 mZ = q.mZ; 00788 return *this; 00789 } 00790 00791 00797 Quaternion& Quaternion::operator=(const Point3D& p) 00798 { 00799 mW = 0; 00800 mX = p.mX; 00801 mY = p.mY; 00802 mZ = p.mZ; 00803 00804 return *this; 00805 } 00806 00807 00808 /* End of File */
