Visualization Library v1.0.3A lightweight C++ OpenGL middleware for 2D/3D graphics |
[Download] [Tutorials] [All Classes] [Grouped Classes] |
00001 /**************************************************************************************/ 00002 /* */ 00003 /* Visualization Library */ 00004 /* http://visualizationlibrary.org */ 00005 /* */ 00006 /* Copyright (c) 2005-2010, Michele Bosi */ 00007 /* All rights reserved. */ 00008 /* */ 00009 /* Redistribution and use in source and binary forms, with or without modification, */ 00010 /* are permitted provided that the following conditions are met: */ 00011 /* */ 00012 /* - Redistributions of source code must retain the above copyright notice, this */ 00013 /* list of conditions and the following disclaimer. */ 00014 /* */ 00015 /* - Redistributions in binary form must reproduce the above copyright notice, this */ 00016 /* list of conditions and the following disclaimer in the documentation and/or */ 00017 /* other materials provided with the distribution. */ 00018 /* */ 00019 /* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND */ 00020 /* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED */ 00021 /* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE */ 00022 /* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR */ 00023 /* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES */ 00024 /* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; */ 00025 /* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON */ 00026 /* ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT */ 00027 /* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */ 00028 /* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ 00029 /* */ 00030 /**************************************************************************************/ 00031 00032 #ifndef Quaternion_INCLUDE_ONCE 00033 #define Quaternion_INCLUDE_ONCE 00034 00035 #include <vlCore/glsl_math.hpp> 00036 00037 namespace vl 00038 { 00039 //----------------------------------------------------------------------------- 00040 // Quaternion 00041 //----------------------------------------------------------------------------- 00043 template<typename T_Scalar> 00044 class Quaternion 00045 { 00046 public: 00047 typedef T_Scalar scalar_type; 00048 //----------------------------------------------------------------------------- 00050 Quaternion() 00051 { 00052 setNoRotation(); 00053 } 00054 //----------------------------------------------------------------------------- 00056 template<typename T> 00057 explicit Quaternion(const Quaternion<T>& quat) 00058 { 00059 mXYZW.x() = (T_Scalar)quat.xyzw().x(); 00060 mXYZW.y() = (T_Scalar)quat.xyzw().y(); 00061 mXYZW.z() = (T_Scalar)quat.xyzw().z(); 00062 mXYZW.w() = (T_Scalar)quat.xyzw().w(); 00063 } 00064 //----------------------------------------------------------------------------- 00066 explicit Quaternion(T_Scalar x, T_Scalar y, T_Scalar z, T_Scalar w) 00067 { 00068 mXYZW.x() = x; 00069 mXYZW.y() = y; 00070 mXYZW.z() = z; 00071 mXYZW.w() = w; 00072 } 00073 //----------------------------------------------------------------------------- 00075 explicit Quaternion(T_Scalar degrees, const Vector3<T_Scalar>& axis) 00076 { 00077 setFromAxisAngle(axis, degrees); 00078 } 00079 //----------------------------------------------------------------------------- 00081 explicit Quaternion(const Vector4<T_Scalar>& v) 00082 { 00083 mXYZW.x() = (T_Scalar)v.x(); 00084 mXYZW.y() = (T_Scalar)v.y(); 00085 mXYZW.z() = (T_Scalar)v.z(); 00086 mXYZW.w() = (T_Scalar)v.w(); 00087 } 00088 //----------------------------------------------------------------------------- 00090 Quaternion& operator=(const Quaternion& q) 00091 { 00092 mXYZW.x() = q.x(); 00093 mXYZW.y() = q.y(); 00094 mXYZW.z() = q.z(); 00095 mXYZW.w() = q.w(); 00096 return *this; 00097 } 00098 //----------------------------------------------------------------------------- 00100 Quaternion& operator=(const Vector4<T_Scalar>& v) 00101 { 00102 mXYZW.x() = (T_Scalar)v.x(); 00103 mXYZW.y() = (T_Scalar)v.y(); 00104 mXYZW.z() = (T_Scalar)v.z(); 00105 mXYZW.w() = (T_Scalar)v.w(); 00106 return *this; 00107 } 00108 //----------------------------------------------------------------------------- 00109 bool operator==(const Quaternion& q) const 00110 { 00111 return x() == q.x() && y() == q.y() && z() == q.z() && w() == q.w(); 00112 } 00113 //----------------------------------------------------------------------------- 00114 bool operator!=(const Quaternion& q) const 00115 { 00116 return !operator==(q); 00117 } 00118 //----------------------------------------------------------------------------- 00120 bool operator<(const Quaternion& other) const 00121 { 00122 if (x() != other.x()) 00123 return x() < other.x(); 00124 if (y() != other.y()) 00125 return y() < other.y(); 00126 if (z() != other.z()) 00127 return z() < other.z(); 00128 else 00129 return w() < other.w(); 00130 } 00131 //----------------------------------------------------------------------------- 00133 const Vector4<T_Scalar>& xyzw() const { return mXYZW; } 00134 //----------------------------------------------------------------------------- 00136 Vector4<T_Scalar>& xyzw() { return mXYZW; } 00137 //----------------------------------------------------------------------------- 00138 T_Scalar& x() { return mXYZW.x(); } 00139 //----------------------------------------------------------------------------- 00140 T_Scalar& y() { return mXYZW.y(); } 00141 //----------------------------------------------------------------------------- 00142 T_Scalar& z() { return mXYZW.z(); } 00143 //----------------------------------------------------------------------------- 00144 T_Scalar& w() { return mXYZW.w(); } 00145 //----------------------------------------------------------------------------- 00146 const T_Scalar& x() const { return mXYZW.x(); } 00147 //----------------------------------------------------------------------------- 00148 const T_Scalar& y() const { return mXYZW.y(); } 00149 //----------------------------------------------------------------------------- 00150 const T_Scalar& z() const { return mXYZW.z(); } 00151 //----------------------------------------------------------------------------- 00152 const T_Scalar& w() const { return mXYZW.w(); } 00153 //----------------------------------------------------------------------------- 00154 Quaternion operator*(T_Scalar val) const 00155 { 00156 Quaternion t = *this; 00157 t.x() *= val; 00158 t.y() *= val; 00159 t.z() *= val; 00160 t.w() *= val; 00161 return t; 00162 } 00163 //----------------------------------------------------------------------------- 00164 Quaternion& operator*=(T_Scalar val) 00165 { 00166 x() *= val; 00167 y() *= val; 00168 z() *= val; 00169 w() *= val; 00170 return *this; 00171 } 00172 //----------------------------------------------------------------------------- 00173 Quaternion operator/(T_Scalar val) const 00174 { 00175 Quaternion t = *this; 00176 val = (T_Scalar)1.0 / val; 00177 t.x() *= val; 00178 t.y() *= val; 00179 t.z() *= val; 00180 t.w() *= val; 00181 return t; 00182 } 00183 //----------------------------------------------------------------------------- 00184 Quaternion& operator/=(T_Scalar val) 00185 { 00186 val = (T_Scalar)1.0 / val; 00187 x() *= val; 00188 y() *= val; 00189 z() *= val; 00190 w() *= val; 00191 return *this; 00192 } 00193 //----------------------------------------------------------------------------- 00194 Quaternion operator+(const Quaternion& q) const 00195 { 00196 Quaternion t = *this; 00197 t.x() += q.x(); 00198 t.y() += q.y(); 00199 t.z() += q.z(); 00200 t.w() += q.w(); 00201 return t; 00202 } 00203 //----------------------------------------------------------------------------- 00204 Quaternion& operator+=(const Quaternion& q) 00205 { 00206 x() += q.x(); 00207 y() += q.y(); 00208 z() += q.z(); 00209 w() += q.w(); 00210 return *this; 00211 } 00212 //----------------------------------------------------------------------------- 00213 Quaternion operator-(const Quaternion& q) const 00214 { 00215 Quaternion t = *this; 00216 t.x() -= q.x(); 00217 t.y() -= q.y(); 00218 t.z() -= q.z(); 00219 t.w() -= q.w(); 00220 return t; 00221 } 00222 //----------------------------------------------------------------------------- 00223 Quaternion& operator-=(const Quaternion& q) 00224 { 00225 x() -= q.x(); 00226 y() -= q.y(); 00227 z() -= q.z(); 00228 w() -= q.w(); 00229 return *this; 00230 } 00231 //----------------------------------------------------------------------------- 00233 Quaternion operator-() const 00234 { 00235 return Quaternion(-x(), -y(), -z(), -w()); 00236 } 00237 //----------------------------------------------------------------------------- 00239 Quaternion& setZero() 00240 { 00241 mXYZW.x() = 0; 00242 mXYZW.y() = 0; 00243 mXYZW.z() = 0; 00244 mXYZW.w() = 0; 00245 return *this; 00246 } 00247 //----------------------------------------------------------------------------- 00249 static Quaternion getZero() 00250 { 00251 return Quaternion().setZero(); 00252 } 00253 //----------------------------------------------------------------------------- 00255 static Quaternion& getZero(Quaternion& q) 00256 { 00257 return q.setZero(); 00258 } 00259 //----------------------------------------------------------------------------- 00261 Quaternion& setNoRotation() 00262 { 00263 mXYZW.x() = 0; 00264 mXYZW.y() = 0; 00265 mXYZW.z() = 0; 00266 mXYZW.w() = 1; 00267 return *this; 00268 } 00269 //----------------------------------------------------------------------------- 00271 static Quaternion getNoRotation() 00272 { 00273 return Quaternion(); 00274 } 00275 //----------------------------------------------------------------------------- 00277 static Quaternion& getNoRotation(Quaternion& q) 00278 { 00279 return q.setNoRotation(); 00280 } 00281 //----------------------------------------------------------------------------- 00283 Quaternion& setFromVectors(const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to); 00284 //----------------------------------------------------------------------------- 00286 static Quaternion getFromVectors(const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to) 00287 { 00288 return Quaternion().setFromVectors(from, to); 00289 } 00290 //----------------------------------------------------------------------------- 00292 static Quaternion& getFromVectors(Quaternion& q, const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to) 00293 { 00294 return q.setFromVectors(from, to); 00295 } 00296 //----------------------------------------------------------------------------- 00299 Quaternion& setFromMatrix(const Matrix4<T_Scalar>& m); 00300 //----------------------------------------------------------------------------- 00302 static Quaternion getFromMatrix(const Matrix4<T_Scalar>& m) 00303 { 00304 return Quaternion().setFromMatrix(m); 00305 } 00306 //----------------------------------------------------------------------------- 00308 static Quaternion& getFromMatrix(Quaternion& q, const Matrix4<T_Scalar>& m) 00309 { 00310 return q.setFromMatrix(m); 00311 } 00312 //----------------------------------------------------------------------------- 00315 Quaternion& setFromMatrix(const Matrix3<T_Scalar>& m); 00316 //----------------------------------------------------------------------------- 00318 static Quaternion getFromMatrix(const Matrix3<T_Scalar>& m) 00319 { 00320 return Quaternion().setFromMatrix(m); 00321 } 00322 //----------------------------------------------------------------------------- 00324 static Quaternion& getFromMatrix(Quaternion& q, const Matrix3<T_Scalar>& m) 00325 { 00326 return q.setFromMatrix(m); 00327 } 00328 //----------------------------------------------------------------------------- 00329 Quaternion& setFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ); 00330 //----------------------------------------------------------------------------- 00331 static Quaternion getFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ) 00332 { 00333 return Quaternion().setFromEulerXYZ(degX, degY, degZ); 00334 } 00335 //----------------------------------------------------------------------------- 00336 static Quaternion& getFromEulerXYZ(Quaternion& q, T_Scalar degX, T_Scalar degY, T_Scalar degZ) 00337 { 00338 return q.setFromEulerXYZ(degX, degY, degZ); 00339 } 00340 //----------------------------------------------------------------------------- 00341 Quaternion& setFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX); 00342 //----------------------------------------------------------------------------- 00343 static Quaternion getFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX) 00344 { 00345 return Quaternion().setFromEulerZYX(degZ, degY, degX); 00346 } 00347 //----------------------------------------------------------------------------- 00348 static Quaternion& getFromEulerZYX(Quaternion& q, T_Scalar degZ, T_Scalar degY, T_Scalar degX) 00349 { 00350 return q.setFromEulerZYX(degZ, degY, degX); 00351 } 00352 //----------------------------------------------------------------------------- 00353 Quaternion& setFromAxisAngle(const Vector3<T_Scalar>& axis, T_Scalar degrees); 00354 //----------------------------------------------------------------------------- 00355 static Quaternion getFromAxisAngle(const Vector3<T_Scalar>& axis, T_Scalar degrees) 00356 { 00357 return Quaternion().setFromAxisAngle(axis, degrees); 00358 } 00359 //----------------------------------------------------------------------------- 00360 static Quaternion& getFromAxisAngle(Quaternion& q, const Vector3<T_Scalar>& axis, T_Scalar degrees) 00361 { 00362 return q.setFromAxisAngle(axis, degrees); 00363 } 00364 //----------------------------------------------------------------------------- 00366 void toAxisAngle( Vector3<T_Scalar>& axis, T_Scalar& degrees ) const; 00367 //----------------------------------------------------------------------------- 00369 Matrix4<T_Scalar> toMatrix4() const; 00370 //----------------------------------------------------------------------------- 00372 Matrix4<T_Scalar>& toMatrix4(Matrix4<T_Scalar>&) const; 00373 //----------------------------------------------------------------------------- 00375 Matrix3<T_Scalar> toMatrix3() const; 00376 //----------------------------------------------------------------------------- 00378 Matrix3<T_Scalar>& toMatrix3(Matrix3<T_Scalar>&) const; 00379 //----------------------------------------------------------------------------- 00381 T_Scalar dot(const Quaternion& q) const 00382 { 00383 return x()*q.x() + y()*q.y() + z()*q.z() + w()*q.w(); 00384 } 00385 //----------------------------------------------------------------------------- 00387 T_Scalar length() const { return mXYZW.length(); } 00388 //----------------------------------------------------------------------------- 00391 Quaternion& normalize(T_Scalar* len=NULL) { mXYZW.normalize(len); return *this; } 00392 //----------------------------------------------------------------------------- 00395 Quaternion getNormalized(T_Scalar* len=NULL) const { Quaternion t = *this; t.normalize(len); return t; } 00396 //----------------------------------------------------------------------------- 00399 Quaternion& getNormalized(Quaternion& q, T_Scalar* len=NULL) const { q = *this; q.normalize(len); return q; } 00400 //----------------------------------------------------------------------------- 00402 T_Scalar lengthSquared() const 00403 { 00404 return x()*x() + y()*y() + z()*z() + w()*w(); 00405 } 00406 //----------------------------------------------------------------------------- 00408 Quaternion getConjugate() const 00409 { 00410 return Quaternion(-x(), -y(), -z(), w()); 00411 } 00412 //----------------------------------------------------------------------------- 00414 Quaternion& getConjugate(Quaternion& q) const 00415 { 00416 q = Quaternion(-x(), -y(), -z(), w()); 00417 return q; 00418 } 00419 //----------------------------------------------------------------------------- 00421 Quaternion getInverse() const 00422 { 00423 return getConjugate() / lengthSquared(); 00424 } 00425 //----------------------------------------------------------------------------- 00427 Quaternion& getInverse(Quaternion& q) const 00428 { 00429 q = getConjugate() / lengthSquared(); 00430 return q; 00431 } 00432 //----------------------------------------------------------------------------- 00436 static Quaternion getSlerp(T_Scalar t, const Quaternion& a, const Quaternion& b); 00437 //----------------------------------------------------------------------------- 00441 static Quaternion& getSlerp(Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& b); 00442 //----------------------------------------------------------------------------- 00444 static Quaternion getSquad(T_Scalar t, const Quaternion& a, const Quaternion& p, const Quaternion& q, const Quaternion& b) 00445 { 00446 return getSlerp((T_Scalar)2.0*t*((T_Scalar)1.0-t), getSlerp(t,a,b), getSlerp(t,p,q)); 00447 } 00448 //----------------------------------------------------------------------------- 00450 static Quaternion& getSquad(Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& p, const Quaternion& q, const Quaternion& b) 00451 { 00452 return getSlerp(out, (T_Scalar)2.0*t*((T_Scalar)1.0-t), getSlerp(t,a,b), getSlerp(t,p,q)); 00453 } 00454 //----------------------------------------------------------------------------- 00458 static Quaternion getNlerp( T_Scalar t, const Quaternion& a, const Quaternion& b ) 00459 { 00460 Quaternion q = a + (b - a) * t; 00461 q.normalize(); 00462 return q; 00463 } 00464 //----------------------------------------------------------------------------- 00468 static Quaternion& getNlerp( Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& b ) 00469 { 00470 out = a + (b - a) * t; 00471 out.normalize(); 00472 return out; 00473 } 00474 //----------------------------------------------------------------------------- 00475 00476 protected: 00477 Vector4<T_Scalar> mXYZW; 00478 }; 00479 //----------------------------------------------------------------------------- 00480 template<typename T_Scalar> 00481 inline Quaternion<T_Scalar> operator*(T_Scalar r, const Quaternion<T_Scalar>& q) 00482 { 00483 return q * r; 00484 } 00485 //----------------------------------------------------------------------------- 00486 template<typename T_Scalar> 00487 inline Quaternion<T_Scalar> operator*(const Quaternion<T_Scalar>& q1, const Quaternion<T_Scalar>& q2) 00488 { 00489 Quaternion<T_Scalar> q; 00490 q.x() = q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(); 00491 q.y() = q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(); 00492 q.z() = q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(); 00493 q.w() = q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z(); 00494 return q; 00495 } 00496 //----------------------------------------------------------------------------- 00497 // post multiplication 00498 template<typename T_Scalar> 00499 inline Vector3<T_Scalar> operator*(const Quaternion<T_Scalar>&q, const Vector3<T_Scalar>& v) 00500 { 00501 // Matrix conversion formula based implementation 00502 T_Scalar x2 = q.x() * q.x(); 00503 T_Scalar y2 = q.y() * q.y(); 00504 T_Scalar z2 = q.z() * q.z(); 00505 T_Scalar xy = q.x() * q.y(); 00506 T_Scalar xz = q.x() * q.z(); 00507 T_Scalar yz = q.y() * q.z(); 00508 T_Scalar wx = q.w() * q.x(); 00509 T_Scalar wy = q.w() * q.y(); 00510 T_Scalar wz = q.w() * q.z(); 00511 00512 Vector3<T_Scalar> r; 00513 r.x() = ( v.x()*(1.0f - 2.0f * (y2 + z2)) + v.y()*(2.0f * (xy - wz)) + v.z()*(2.0f * (xz + wy)) ); 00514 r.y() = ( v.x()*(2.0f * (xy + wz)) + v.y()*(1.0f - 2.0f * (x2 + z2)) + v.z()*(2.0f * (yz - wx)) ); 00515 r.z() = ( v.x()*(2.0f * (xz - wy)) + v.y()*(2.0f * (yz + wx)) + v.z()*(1.0f - 2.0f * (x2 + y2)) ); 00516 return r; 00517 } 00518 //----------------------------------------------------------------------------- 00519 // post multiplication 00520 template<typename T_Scalar> 00521 inline Vector4<T_Scalar> operator*(const Quaternion<T_Scalar>&q, const Vector4<T_Scalar>& v) 00522 { 00523 return Vector4<T_Scalar>( q * v.xyz(), v.w() ); 00524 } 00525 //----------------------------------------------------------------------------- 00526 template<typename T_Scalar> 00527 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ ) 00528 { 00529 *this = Quaternion<T_Scalar>(degX, Vector3<T_Scalar>(1,0,0)) * Quaternion<T_Scalar>(degY, Vector3<T_Scalar>(0,1,0)) * Quaternion<T_Scalar>(degZ, Vector3<T_Scalar>(0,0,1)); 00530 return *this; 00531 } 00532 //----------------------------------------------------------------------------- 00533 template<typename T_Scalar> 00534 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX ) 00535 { 00536 *this = Quaternion<T_Scalar>(degZ, Vector3<T_Scalar>(0,0,1)) * Quaternion<T_Scalar>(degY, Vector3<T_Scalar>(0,1,0)) * Quaternion<T_Scalar>(degX, Vector3<T_Scalar>(1,0,0)); 00537 return *this; 00538 } 00539 //----------------------------------------------------------------------------- 00540 template<typename T_Scalar> 00541 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromMatrix(const Matrix4<T_Scalar>& m) 00542 { 00543 return setFromMatrix( m.get3x3() ); 00544 } 00545 //----------------------------------------------------------------------------- 00546 template<typename T_Scalar> 00547 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromMatrix(const Matrix3<T_Scalar>& m) 00548 { 00549 T_Scalar tr, s, q[4]; 00550 00551 int next[3] = {1, 2, 0}; 00552 00553 tr = m.e(0,0) + m.e(1,1) + m.e(2,2); 00554 00555 // check the diagonal 00556 if (tr + (T_Scalar)1.0 > 0.0) 00557 { 00558 s = vl::sqrt(tr + (T_Scalar)1.0); 00559 w() = s / (T_Scalar)2.0; 00560 s = (T_Scalar)0.5 / s; 00561 x() = (m.e(2,1) - m.e(1,2)) * s; 00562 y() = (m.e(0,2) - m.e(2,0)) * s; 00563 z() = (m.e(1,0) - m.e(0,1)) * s; 00564 } 00565 else 00566 { 00567 // diagonal is negative 00568 int i, j, k; 00569 i = 0; 00570 if (m.e(1,1) > m.e(0,0)) i = 1; 00571 if (m.e(2,2) > m.e(i,i)) i = 2; 00572 j = next[i]; 00573 k = next[j]; 00574 00575 s = vl::sqrt((m.e(i,i) - (m.e(j,j) + m.e(k,k))) + (T_Scalar)1.0); 00576 00577 q[i] = s * (T_Scalar)0.5; 00578 00579 if (s != 0.0) 00580 s = (T_Scalar)0.5 / s; 00581 00582 q[3] = (m.e(k,j) - m.e(j,k)) * s; 00583 q[j] = (m.e(j,i) + m.e(i,j)) * s; 00584 q[k] = (m.e(k,i) + m.e(i,k)) * s; 00585 00586 x() = q[0]; 00587 y() = q[1]; 00588 z() = q[2]; 00589 w() = q[3]; 00590 } 00591 00592 return *this; 00593 } 00594 //----------------------------------------------------------------------------- 00595 template<typename T_Scalar> 00596 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromAxisAngle( const Vector3<T_Scalar>& axis, T_Scalar degrees ) 00597 { 00598 degrees *= (T_Scalar)dDEG_TO_RAD; 00599 T_Scalar sa2 = sin(degrees * (T_Scalar)0.5); 00600 Vector3<T_Scalar> na = axis; 00601 na.normalize(); 00602 mXYZW.x() = na.x() * sa2; 00603 mXYZW.y() = na.y() * sa2; 00604 mXYZW.z() = na.z() * sa2; 00605 mXYZW.w() = cos(degrees * (T_Scalar)0.5); 00606 return *this; 00607 } 00608 //----------------------------------------------------------------------------- 00609 template<typename T_Scalar> 00610 void Quaternion<T_Scalar>::toAxisAngle( Vector3<T_Scalar>& axis, T_Scalar& degrees ) const 00611 { 00612 T_Scalar iscale = sqrt( x()*x() + y()*y() + z()*z() ); 00613 if (iscale == 0) 00614 { 00615 axis.x() = 0; 00616 axis.y() = 0; 00617 axis.z() = 0; 00618 degrees = 0; 00619 } 00620 else 00621 { 00622 iscale = T_Scalar(1.0) / iscale; 00623 axis.x() = x() * iscale; 00624 axis.y() = y() * iscale; 00625 axis.z() = z() * iscale; 00626 VL_CHECK(w()>=-1.0 && w()<=+1.0) 00627 T_Scalar tw = clamp(w(),(T_Scalar)-1.0,(T_Scalar)+1.0); 00628 degrees = acos( tw ) * (T_Scalar)2.0 * (T_Scalar)dRAD_TO_DEG; 00629 } 00630 } 00631 //----------------------------------------------------------------------------- 00632 template<typename T_Scalar> 00633 Matrix4<T_Scalar>& Quaternion<T_Scalar>::toMatrix4(Matrix4<T_Scalar>& out) const 00634 { 00635 T_Scalar x2 = x() * x(); 00636 T_Scalar y2 = y() * y(); 00637 T_Scalar z2 = z() * z(); 00638 T_Scalar xy = x() * y(); 00639 T_Scalar xz = x() * z(); 00640 T_Scalar yz = y() * z(); 00641 T_Scalar wx = w() * x(); 00642 T_Scalar wy = w() * y(); 00643 T_Scalar wz = w() * z(); 00644 00645 return out = Matrix4<T_Scalar>( 00646 (1.0f - 2.0f * (y2 + z2)), (2.0f * (xy - wz)), (2.0f * (xz + wy)), 0.0f, 00647 (2.0f * (xy + wz)), (1.0f - 2.0f * (x2 + z2)), (2.0f * (yz - wx)), 0.0f, 00648 (2.0f * (xz - wy)), (2.0f * (yz + wx)), (1.0f - 2.0f * (x2 + y2)), 0.0f, 00649 0.0f, 0.0f, 0.0f, 1.0f ); 00650 } 00651 //----------------------------------------------------------------------------- 00652 template<typename T_Scalar> 00653 Matrix4<T_Scalar> Quaternion<T_Scalar>::toMatrix4() const 00654 { 00655 Matrix4<T_Scalar> out; 00656 return toMatrix4(out); 00657 } 00658 //----------------------------------------------------------------------------- 00659 template<typename T_Scalar> 00660 Matrix3<T_Scalar>& Quaternion<T_Scalar>::toMatrix3(Matrix3<T_Scalar>& out) const 00661 { 00662 T_Scalar x2 = x() * x(); 00663 T_Scalar y2 = y() * y(); 00664 T_Scalar z2 = z() * z(); 00665 T_Scalar xy = x() * y(); 00666 T_Scalar xz = x() * z(); 00667 T_Scalar yz = y() * z(); 00668 T_Scalar wx = w() * x(); 00669 T_Scalar wy = w() * y(); 00670 T_Scalar wz = w() * z(); 00671 00672 return out = Matrix3<T_Scalar>( 00673 (1.0f - 2.0f * (y2 + z2)), (2.0f * (xy + wz)), (2.0f * (xz - wy)), 00674 (2.0f * (xy - wz)), (1.0f - 2.0f * (x2 + z2)), (2.0f * (yz + wx)), 00675 (2.0f * (xz + wy)), (2.0f * (yz - wx)), (1.0f - 2.0f * (x2 + y2)) ); 00676 } 00677 //----------------------------------------------------------------------------- 00678 template<typename T_Scalar> 00679 Matrix3<T_Scalar> Quaternion<T_Scalar>::toMatrix3() const 00680 { 00681 Matrix3<T_Scalar> out; 00682 return toMatrix3(out); 00683 } 00684 //----------------------------------------------------------------------------- 00685 template<typename T_Scalar> 00686 Quaternion<T_Scalar> Quaternion<T_Scalar>::getSlerp( T_Scalar t, const Quaternion<T_Scalar>& a, const Quaternion<T_Scalar>& b ) 00687 { 00688 Quaternion<T_Scalar> q; 00689 getSlerp(q, t, a, b); 00690 return q; 00691 } 00692 //----------------------------------------------------------------------------- 00693 template<typename T_Scalar> 00694 Quaternion<T_Scalar>& Quaternion<T_Scalar>::getSlerp( Quaternion<T_Scalar>& out, T_Scalar t, const Quaternion<T_Scalar>& a, const Quaternion<T_Scalar>& b ) 00695 { 00696 T_Scalar scale_a, scale_b, omega, sinom; 00697 T_Scalar cosom = a.dot(b); 00698 00699 Quaternion<T_Scalar> b2(b); 00700 00701 if ( cosom < 0 ) 00702 { 00703 cosom = -cosom; 00704 b2 = -b; 00705 } 00706 00707 // clamp rounding errors 00708 cosom = cosom > (T_Scalar)1 ? (T_Scalar)1 : cosom; 00709 00710 if( cosom < (T_Scalar)1.0 ) 00711 { 00712 omega = acos(cosom); 00713 sinom = sin(omega); 00714 VL_CHECK(sinom != 0) 00715 scale_a = sin(((T_Scalar)1.0-t) * omega) / sinom; 00716 scale_b = sin(t * omega) / sinom; 00717 } 00718 else 00719 { 00720 // linear interpolation for degenerate cases 00721 scale_a = (T_Scalar)1.0 - t; 00722 scale_b = t; 00723 } 00724 00725 return out = (a*scale_a) + (b2*scale_b); 00726 } 00727 //----------------------------------------------------------------------------- 00728 template<typename T_Scalar> 00729 Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromVectors(const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to) 00730 { 00731 Vector3<T_Scalar> a,b; 00732 a = from; 00733 b = to; 00734 a.normalize(); 00735 b.normalize(); 00736 Vector3<T_Scalar> axis = cross(a,b); 00737 T_Scalar len = 0; 00738 axis.normalize(&len); 00739 if(len) 00740 { 00741 T_Scalar cosa = vl::dot(a,b); 00742 cosa = clamp(cosa, (T_Scalar)-1.0, (T_Scalar)+1.0); 00743 T_Scalar alpha = acos( cosa ); 00744 setFromAxisAngle(axis, alpha*(T_Scalar)dRAD_TO_DEG); 00745 } 00746 else 00747 setNoRotation(); 00748 return *this; 00749 } 00750 //----------------------------------------------------------------------------- 00751 typedef Quaternion<float> fquat; 00752 typedef Quaternion<double> dquat; 00753 typedef Quaternion<real> quat; 00754 //----------------------------------------------------------------------------- 00755 } 00756 00757 #endif