OSGQuaternion.inl

Go to the documentation of this file.
00001 /*---------------------------------------------------------------------------*\
00002  *                                OpenSG                                     *
00003  *                                                                           *
00004  *                                                                           *
00005  *             Copyright (C) 2000-2003 by the OpenSG Forum                   *
00006  *                                                                           *
00007  *                            www.opensg.org                                 *
00008  *                                                                           *
00009  *   contact: dirk@opensg.org, gerrit.voss@vossg.org, jbehr@zgdv.de          *
00010  *                                                                           *
00011 \*---------------------------------------------------------------------------*/
00012 /*---------------------------------------------------------------------------*\
00013  *                                License                                    *
00014  *                                                                           *
00015  * This library is free software; you can redistribute it and/or modify it   *
00016  * under the terms of the GNU Library General Public License as published    *
00017  * by the Free Software Foundation, version 2.                               *
00018  *                                                                           *
00019  * This library is distributed in the hope that it will be useful, but       *
00020  * WITHOUT ANY WARRANTY; without even the implied warranty of                *
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU         *
00022  * Library General Public License for more details.                          *
00023  *                                                                           *
00024  * You should have received a copy of the GNU Library General Public         *
00025  * License along with this library; if not, write to the Free Software       *
00026  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 *
00027  *                                                                           *
00028 \*---------------------------------------------------------------------------*/
00029 /*---------------------------------------------------------------------------*\
00030  *                                Changes                                    *
00031  *                                                                           *
00032  *                                                                           *
00033  *                                                                           *
00034  *                                                                           *
00035  *                                                                           *
00036  *                                                                           *
00037 \*---------------------------------------------------------------------------*/
00038
00039 OSG_BEGIN_NAMESPACE
00040
00053 /*-------------------------------------------------------------------------*/
00054 /*                            Class Get                                    */
00055
00057
00058 template <class ValueTypeT>
00059 QuaternionBase<ValueTypeT> QuaternionBase<ValueTypeT>::_identity;
00060
00062
00063 template <class ValueTypeT> inline
00064 const QuaternionBase<ValueTypeT> &QuaternionBase<ValueTypeT>::identity(void)
00065 {
00066     return _identity;
00067 }
00068
00070
00071 template <class ValueTypeT> inline
00072 QuaternionBase<ValueTypeT>
00073     QuaternionBase<ValueTypeT>::slerp(const QuaternionBase &rot0,
00074                                       const QuaternionBase &rot1,
00075                                       const ValueTypeT      t   )
00076 {
00077     QuaternionBase returnValue;
00078
00079     slerp(rot0, rot1, returnValue, t);
00080
00081     return returnValue;
00082 }
00083
00084 /*-------------------------------------------------------------------------*/
00085 /*                            Constructors                                 */
00086
00087 template <class ValueTypeT> inline
00088 QuaternionBase<ValueTypeT>::QuaternionBase(void)
00089 {
00090     _quat[0] =
00091         _quat[1] =
00092         _quat[2] = TypeTraits<ValueTypeT>::getZeroElement();
00093
00094     _quat[3] = TypeTraits<ValueTypeT>::getOneElement();
00095 }
00096
00097
00098 template <class ValueTypeT> inline
00099 QuaternionBase<ValueTypeT>::QuaternionBase(const QuaternionBase &source)
00100 {
00101     for(UInt32 i = 0; i < 4; i++)
00102     {
00103         _quat[i] = source._quat[i];
00104     }
00105 }
00106
00107
00108 template <class ValueTypeT> inline
00109 QuaternionBase<ValueTypeT>::QuaternionBase(const MatrixType &matrix)
00110 {
00111     setValue(matrix);
00112 }
00113
00114
00115 template <class ValueTypeT> inline
00116 QuaternionBase<ValueTypeT>::QuaternionBase(const VectorType &axis,
00117                                            const ValueTypeT  angle)
00118 {
00119     setValueAsAxisRad(axis, angle);
00120 }
00121
00123
00124 template <class ValueTypeT> inline
00125 QuaternionBase<ValueTypeT>::QuaternionBase(const VectorType &rotateFrom,
00126                                            const VectorType &rotateTo)
00127 {
00128     setValue(rotateFrom, rotateTo);
00129 }
00130
00131 /*-------------------------------------------------------------------------*/
00132 /*                             Destructor                                  */
00133
00134 template <class ValueTypeT> inline
00135 QuaternionBase<ValueTypeT>::~QuaternionBase(void)
00136 {
00137 }
00138
00139 /*-------------------------------------------------------------------------*/
00140 /*                                Set                                      */
00141
00143
00144 template <class ValueTypeT> inline
00145 void QuaternionBase<ValueTypeT>::setIdentity(void)
00146 {
00147     _quat[0] =
00148         _quat[1] =
00149         _quat[2] = TypeTraits<ValueTypeT>::getZeroElement();
00150
00151     _quat[3] = TypeTraits<ValueTypeT>::getOneElement();
00152 }
00153
00158 template <class ValueTypeT> inline
00159 void QuaternionBase<ValueTypeT>::setValueAsAxisRad(
00160     const ValueTypeT *valsP)
00161 {
00162     setValueAsAxisRad(valsP[0], valsP[1], valsP[2], valsP[3]);
00163 }
00164
00169 template <class ValueTypeT> inline
00170 void QuaternionBase<ValueTypeT>::setValueAsAxisDeg(
00171     const ValueTypeT *valsP)
00172 {
00173     setValueAsAxisDeg(valsP[0], valsP[1], valsP[2], valsP[3]);
00174 }
00175
00177
00178 template <class ValueTypeT> inline
00179 void QuaternionBase<ValueTypeT>::setValueAsQuat(
00180     const ValueTypeT *valsP)
00181 {
00182     UInt32 i;
00183
00184     for(i = 0; i < 4; i++)
00185     {
00186         _quat[i] = valsP[i];
00187     }
00188 }
00189
00194 template <class ValueTypeT> inline
00195 void QuaternionBase<ValueTypeT>::setValueAsAxisRad(const ValueTypeT x,
00196                                                    const ValueTypeT y,
00197                                                    const ValueTypeT z,
00198                                                    const ValueTypeT w)
00199 {
00200     ValueTypeT rTmp = osgSqrt(x * x + y * y + z * z);
00201
00202     if(rTmp > Eps)
00203     {
00204         rTmp = osgSin(w / 2.0f) / rTmp;
00205
00206         _quat[0] = x * rTmp;
00207         _quat[1] = y * rTmp;
00208         _quat[2] = z * rTmp;
00209         _quat[3] = osgCos(w / 2.0f);
00210     }
00211     else
00212     {
00213         setIdentity();
00214     }
00215 }
00216
00221 template <class ValueTypeT> inline
00222 void QuaternionBase<ValueTypeT>::setValueAsAxisDeg(const ValueTypeT x,
00223                                                    const ValueTypeT y,
00224                                                    const ValueTypeT z,
00225                                                    const ValueTypeT w)
00226 {
00227     setValueAsAxisRad(x,y,z,osgDegree2Rad(w));
00228 }
00229
00234 template <class ValueTypeT> inline
00235 void QuaternionBase<ValueTypeT>::setValueAsQuat(const ValueTypeT x,
00236                                                 const ValueTypeT y,
00237                                                 const ValueTypeT z,
00238                                                 const ValueTypeT w)
00239 {
00240     _quat[0] = x;
00241     _quat[1] = y;
00242     _quat[2] = z;
00243     _quat[3] = w;
00244 }
00245
00246 template <class ValueTypeT> inline
00247 void QuaternionBase<ValueTypeT>::setValue(const Self &quat)
00248 {
00249     _quat[0] = quat[0];
00250     _quat[1] = quat[1];
00251     _quat[2] = quat[2];
00252     _quat[3] = quat[3];
00253 }
00254
00256
00257 template <class ValueTypeT> inline
00258 void QuaternionBase<ValueTypeT>::setValue(const MatrixType &matrix)
00259 {
00260     Real64 tr;
00261     Real64 s;
00262     Real64 qt[3];
00263
00264     UInt32 i;
00265     UInt32 j;
00266     UInt32 k;
00267
00268     UInt32 nxt[3] = { 1, 2, 0};
00269
00270     tr = matrix[0][0] + matrix[1][1] + matrix[2][2];
00271
00272     if(tr > 0.0)
00273     {
00274         s = osgSqrt(tr + 1.0);
00275
00276         _quat[3] = ValueTypeT(s * 0.5);
00277
00278         s = 0.5 / s;
00279         _quat[0] = ValueTypeT((matrix[1][2] - matrix[2][1]) * s);
00280         _quat[1] = ValueTypeT((matrix[2][0] - matrix[0][2]) * s);
00281         _quat[2] = ValueTypeT((matrix[0][1] - matrix[1][0]) * s);
00282     }
00283     else
00284     {
00285         if(matrix[1][1] > matrix[0][0])
00286             i = 1;
00287         else
00288             i = 0;
00289
00290         if(matrix[2][2] > matrix[i][i])
00291             i = 2;
00292
00293         j = nxt[i];
00294         k = nxt[j];
00295
00296         s = osgSqrt(matrix[i][i] - (matrix[j][j] + matrix[k][k]) + 1.0 );
00297
00298         qt[i] = s * 0.5;
00299         s     = 0.5 / s;
00300
00301         _quat[3] = ValueTypeT((matrix[j][k] - matrix[k][j]) * s);
00302
00303         qt[j] = (matrix[i][j] + matrix[j][i]) * s;
00304         qt[k] = (matrix[i][k] + matrix[k][i]) * s;
00305
00306         _quat[0] = ValueTypeT(qt[0]);
00307         _quat[1] = ValueTypeT(qt[1]);
00308         _quat[2] = ValueTypeT(qt[2]);
00309     }
00310
00311     if(_quat[3] > 1.0 || _quat[3] < -1.0)
00312     {
00313         const ValueTypeT errThreshold = 1 + (Eps * 100);
00314
00315         if(_quat[3] > errThreshold || _quat[3] < -errThreshold)
00316         {
00317             fprintf(stderr,
00318                     "\nMatToQuat: BUG: |quat[4]| (%f) >> 1.0 !\n\n",
00319                     _quat[3]);
00320         }
00321
00322         if(_quat[3] > 1.0)
00323         {
00324             _quat[3] = 1.0;
00325         }
00326         else
00327         {
00328             _quat[3] = -1.0;
00329         }
00330     }
00331 }
00332
00337 template <class ValueTypeT> inline
00338 void QuaternionBase<ValueTypeT>::setValueAsAxisRad(const VectorType &axis,
00339                                                          ValueTypeT  angle)
00340 {
00341   setValueAsAxisRad(axis[0], axis[1], axis[2], angle);
00342 }
00343
00348 template <class ValueTypeT> inline
00349 void QuaternionBase<ValueTypeT>::setValueAsAxisDeg(const VectorType &axis,
00350                                                          ValueTypeT  angle)
00351 {
00352   setValueAsAxisDeg(axis[0], axis[1], axis[2], angle);
00353 }
00354
00356
00357 template <class ValueTypeT> inline
00358 void QuaternionBase<ValueTypeT>::setValue(const VectorType &rotateFrom,
00359                                           const VectorType &rotateTo)
00360 {
00361     VectorType from = rotateFrom;
00362     VectorType to   = rotateTo;
00363     VectorType axis;
00364     ValueTypeT cost;
00365
00366     from.normalize();
00367     to  .normalize();
00368
00369     cost = from.dot(to);
00370
00371     // check for degeneracies
00372     if(cost > 0.99999)
00373     {   // vectors are parallel
00374         setIdentity();
00375         return;
00376     }
00377     else if(cost < -0.99999)
00378     {
00379         // vectors are opposite
00380         // find an axis to rotate around, which should be
00381         // perpendicular to the original axis
00382         // Try cross product with (1,0,0) first, if that's one of our
00383         // original vectors then try  (0,1,0).
00384
00385         VectorType cAxis(1.0, 0.0, 0.0);
00386
00387         VectorType tmp = from.cross(cAxis);
00388
00389         if(tmp.length() < 0.00001)
00390         {
00391             cAxis.setValues(0.0, 1.0, 0.0);
00392
00393             tmp = from.cross(cAxis);
00394         }
00395
00396         tmp.normalize();
00397
00398         setValueAsAxisRad(tmp[0], tmp[1], tmp[2], Pi);
00399
00400         return;
00401     }
00402
00403     axis = rotateFrom.cross(rotateTo);
00404     axis.normalize();
00405
00406     // use half-angle formulae
00407     // sin^2 t = ( 1 - cos (2t) ) / 2
00408
00409     axis *= ValueTypeT(osgSqrt(0.5 * (1.0 - cost)));
00410
00411     // scale the axis by the sine of half the rotation angle to get
00412     // the normalized quaternion
00413
00414     _quat[0] = axis[0];
00415     _quat[1] = axis[1];
00416     _quat[2] = axis[2];
00417
00418     // cos^2 t = ( 1 + cos (2t) ) / 2
00419     // w part is cosine of half the rotation angle
00420
00421     _quat[3] = ValueTypeT(osgSqrt(0.5 * (1.0 + cost)));
00422 }
00423
00424
00429 template <class ValueTypeT> inline
00430 void QuaternionBase<ValueTypeT>::setValueAsAxisRad(const Char8 *str)
00431 {
00432     setValueAsQuat(str);
00433
00434     if(osgFinite(_quat[0]) == 0 ||
00435        osgFinite(_quat[1]) == 0 ||
00436        osgFinite(_quat[2]) == 0 ||
00437        osgFinite(_quat[3]) == 0   )
00438     {
00439         setIdentity();
00440     }
00441     else
00442     {
00443         setValueAsAxisRad(_quat[0], _quat[1], _quat[2], _quat[3]);
00444     }
00445 }
00446
00451 template <class ValueTypeT> inline
00452 void QuaternionBase<ValueTypeT>::setValueAsAxisDeg(const Char8 *str)
00453 {
00454     setValueAsQuat(str);
00455
00456     if(osgFinite(_quat[0]) == 0 ||
00457        osgFinite(_quat[1]) == 0 ||
00458        osgFinite(_quat[2]) == 0 ||
00459        osgFinite(_quat[3]) == 0   )
00460     {
00461         setIdentity();
00462     }
00463     else
00464     {
00465         setValueAsAxisDeg(_quat[0], _quat[1], _quat[2], _quat[3]);
00466     }
00467 }
00468
00473 template <class ValueTypeT> inline
00474 void QuaternionBase<ValueTypeT>::setValueAsQuat(const Char8 *str)
00475 {
00476     UInt32 i;
00477     UInt32 numOfToken = 4;
00478
00479     Char8 *c = const_cast<Char8 *>(str);
00480
00481     Char8 *tokenC = 0;
00482     Char8  token[256];
00483
00484     ValueTypeT vec[4];
00485
00486     if( (str  == NULL) ||
00487         (*str == '\0') )
00488     {
00489         setIdentity();
00490         return;
00491     }
00492
00493     for(i = 0; i < numOfToken; c++)
00494     {
00495         switch (*c)
00496         {
00497             case '\0':
00498                 if (tokenC)
00499                 {
00500                     *tokenC   = 0;
00501                      vec[i++] = TypeTraits<ValueTypeT>::getFromCString(token);
00502
00503                 }
00504
00505                 while (i < numOfToken)
00506                 {
00507                     vec[i++] = TypeTraits<ValueTypeT>::getZeroElement();
00508                 }
00509
00510                 break;
00511             case ' ' :
00512             case '\t':
00513             case '\n':
00514                 if (tokenC)
00515                 {
00516                     *tokenC   = 0;
00517                      vec[i++] = TypeTraits<ValueTypeT>::getFromCString(token);
00518                      tokenC   = 0;
00519                 }
00520                 break;
00521             default:
00522                 if (!tokenC)
00523                 {
00524                     tokenC = token;
00525                 }
00526                 *tokenC++ = *c;
00527                 break;
00528         }
00529     }
00530
00531   _quat[0] = vec[0];
00532   _quat[1] = vec[1];
00533   _quat[2] = vec[2];
00534   _quat[3] = vec[3];
00535 }
00536
00538
00539 template <class ValueTypeT> inline
00540 void QuaternionBase<ValueTypeT>::setValue(const ValueTypeT alpha,
00541                                           const ValueTypeT beta,
00542                                           const ValueTypeT gamma)
00543 {
00544     ValueTypeT sx = osgSin(alpha * 0.5f);
00545     ValueTypeT cx = osgCos(alpha * 0.5f);
00546
00547     ValueTypeT sy = osgSin(beta  * 0.5f);
00548     ValueTypeT cy = osgCos(beta  * 0.5f);
00549
00550     ValueTypeT sz = osgSin(gamma * 0.5f);
00551     ValueTypeT cz = osgCos(gamma * 0.5f);
00552
00553     _quat[0] = (sx * cy * cz) - (cx * sy * sz);
00554     _quat[1] = (cx * sy * cz) + (sx * cy * sz);
00555     _quat[2] = (cx * cy * sz) - (sx * sy * cz);
00556     _quat[3] = (cx * cy * cz) + (sx * sy * sz);
00557 }
00558
00559 template <class ValueTypeT> inline
00560 void QuaternionBase<ValueTypeT>::setValueFromCString(const Char8 *szString)
00561 {
00562     setValueAsAxisRad(szString);
00563 }
00564
00565 template <class ValueTypeT> inline
00566 void QuaternionBase<ValueTypeT>::setValueFromCString(      Char8 *szString)
00567 {
00568     setValueAsAxisRad(szString);
00569 }
00570
00571 #ifndef OSG_DISABLE_DEPRECATED
00572 template <class ValueTypeT> inline
00573 void QuaternionBase<ValueTypeT>::setValue(const Char8 *szString)
00574 {
00575     setValueAsAxisRad(szString);
00576 }
00577
00578 template <class ValueTypeT> inline
00579 void QuaternionBase<ValueTypeT>::setValue(Char8 *szString)
00580 {
00581     setValueAsAxisRad(szString);
00582 }
00583 #endif
00584 
00585 /*-------------------------------------------------------------------------*/
00586 /*                                Get                                      */
00587
00589
00590 template <class ValueTypeT> inline
00591 const ValueTypeT *QuaternionBase<ValueTypeT>::getValues(void) const
00592 {
00593     return _quat;
00594 }
00595
00600 template <class ValueTypeT> inline
00601 void QuaternionBase<ValueTypeT>::getValueAsAxisRad(ValueTypeT &x,
00602                                                    ValueTypeT &y,
00603                                                    ValueTypeT &z,
00604                                                    ValueTypeT &w) const
00605 {
00606     getValueAsAxisDeg(x, y, z, w);
00607
00608     w = osgDegree2Rad(w);
00609 }
00610
00615 template <class ValueTypeT> inline
00616 void QuaternionBase<ValueTypeT>::getValueAsAxisDeg(ValueTypeT &x,
00617                                                    ValueTypeT &y,
00618                                                    ValueTypeT &z,
00619                                                    ValueTypeT &w) const
00620 {
00621     ValueTypeT len;
00622
00623     VectorType q(_quat[0], _quat[1], _quat[2]);
00624
00625     len = q.length();
00626
00627     if(len > Eps)
00628     {
00629         q *= (TypeTraits<ValueTypeT>::getOneElement() / len);
00630
00631         x  = q[0];
00632         y  = q[1];
00633         z  = q[2];
00634
00635         w = osgRad2Degree(2.0f * osgACos(_quat[3]));
00636     }
00637     else
00638     {
00639         x = TypeTraits<ValueTypeT>::getZeroElement();
00640         y = TypeTraits<ValueTypeT>::getZeroElement();
00641         z = TypeTraits<ValueTypeT>::getOneElement();
00642
00643         w = TypeTraits<ValueTypeT>::getZeroElement();
00644     }
00645 }
00646
00648
00649 template <class ValueTypeT> inline
00650 void QuaternionBase<ValueTypeT>::getValueAsQuat(ValueTypeT &x,
00651                                                 ValueTypeT &y,
00652                                                 ValueTypeT &z,
00653                                                 ValueTypeT &w) const
00654 {
00655     x = _quat[0];
00656     y = _quat[1];
00657     z = _quat[2];
00658     w = _quat[3];
00659 }
00660
00662
00663 template <class ValueTypeT> inline
00664 void QuaternionBase<ValueTypeT>::getValueAsAxisRad(VectorType &axis,
00665                                                    ValueTypeT &radians) const
00666 {
00667   ValueTypeT x;
00668   ValueTypeT y;
00669   ValueTypeT z;
00670   ValueTypeT w;
00671
00672   getValueAsAxisRad(x, y, z, w);
00673
00674   axis.setValues(x, y, z);
00675
00676   radians = w;
00677 }
00678
00680
00681 template <class ValueTypeT> inline
00682 void QuaternionBase<ValueTypeT>::getValueAsAxisDeg(VectorType &axis,
00683                                                    ValueTypeT &degrees) const
00684 {
00685   ValueTypeT x;
00686   ValueTypeT y;
00687   ValueTypeT z;
00688   ValueTypeT w;
00689
00690   getValueAsAxisDeg(x, y, z, w);
00691
00692   axis.setValues(x, y, z);
00693
00694   degrees = w;
00695 }
00696
00698
00699 template <class ValueTypeT> inline
00700 void QuaternionBase<ValueTypeT>::getValue(MatrixType &matrix) const
00701 {
00702     getValuesOnly(matrix);
00703
00704     matrix[0][3] = 0.0f;
00705     matrix[1][3] = 0.0f;
00706     matrix[2][3] = 0.0f;
00707
00708     matrix[3][0] = 0.0f;
00709     matrix[3][1] = 0.0f;
00710     matrix[3][2] = 0.0f;
00711     matrix[3][3] = 1.0f;
00712 }
00713
00715
00716 template <class ValueTypeT> inline
00717 void QuaternionBase<ValueTypeT>::getValuesOnly(MatrixType &matrix) const
00718 {
00719     matrix[0][0] = 1.0f - 2.0f * (_quat[Q_Y] * _quat[Q_Y] +
00720                                   _quat[Q_Z] * _quat[Q_Z]);
00721     matrix[0][1] =        2.0f * (_quat[Q_X] * _quat[Q_Y] +
00722                                   _quat[Q_Z] * _quat[Q_W]);
00723     matrix[0][2] =        2.0f * (_quat[Q_Z] * _quat[Q_X] -
00724                                   _quat[Q_Y] * _quat[Q_W]);
00725
00726     matrix[1][0] =        2.0f * (_quat[Q_X] * _quat[Q_Y] -
00727                                   _quat[Q_Z] * _quat[Q_W]);
00728     matrix[1][1] = 1.0f - 2.0f * (_quat[Q_Z] * _quat[Q_Z] +
00729                                   _quat[Q_X] * _quat[Q_X]);
00730     matrix[1][2] =        2.0f * (_quat[Q_Y] * _quat[Q_Z] +
00731                                   _quat[Q_X] * _quat[Q_W]);
00732
00733     matrix[2][0] =        2.0f * (_quat[Q_Z] * _quat[Q_X] +
00734                                   _quat[Q_Y] * _quat[Q_W]);
00735     matrix[2][1] =        2.0f * (_quat[Q_Y] * _quat[Q_Z] -
00736                                   _quat[Q_X] * _quat[Q_W]);
00737     matrix[2][2] = 1.0f - 2.0f * (_quat[Q_Y] * _quat[Q_Y] +
00738                                   _quat[Q_X] * _quat[Q_X]);
00739 }
00740
00741 template <class ValueTypeT> inline
00742 ValueTypeT QuaternionBase<ValueTypeT>::x(void) const
00743 {
00744     return _quat[0];
00745 }
00746
00747 template <class ValueTypeT> inline
00748 ValueTypeT QuaternionBase<ValueTypeT>::y(void) const
00749 {
00750     return _quat[1];
00751 }
00752
00753 template <class ValueTypeT> inline
00754 ValueTypeT QuaternionBase<ValueTypeT>::z(void) const
00755 {
00756     return _quat[2];
00757 }
00758
00759 template <class ValueTypeT> inline
00760 ValueTypeT QuaternionBase<ValueTypeT>::w(void) const
00761 {
00762     return _quat[3];
00763 }
00764
00765 /*-------------------------------------------------------------------------*/
00766 /*                                Init                                     */
00767
00769
00770 template <class ValueTypeT> inline
00771 ValueTypeT QuaternionBase<ValueTypeT>::length(void) const
00772 {
00773     return osgSqrt(_quat[0] * _quat[0] +
00774                    _quat[1] * _quat[1] +
00775                    _quat[2] * _quat[2] +
00776                    _quat[3] * _quat[3]);
00777 }
00778
00780
00781 template <class ValueTypeT> inline
00782 void QuaternionBase<ValueTypeT>::normalize(void)
00783 {
00784     ValueTypeT rLength = length();
00785
00786     if(osgAbs(rLength) < Eps)
00787     {
00788         rLength =  TypeTraits<ValueTypeT>::getOneElement();
00789     }
00790     else
00791     {
00792         rLength =  TypeTraits<ValueTypeT>::getOneElement() / rLength;
00793     }
00794
00795     for(UInt32 i = 0; i < 4; i++)
00796     {
00797         _quat[i] *= rLength;
00798     }
00799 }
00800
00802
00803 template <class ValueTypeT> inline
00804 void QuaternionBase<ValueTypeT>::invert(void)
00805 {
00806     _quat[0] = -_quat[0];
00807     _quat[1] = -_quat[1];
00808     _quat[2] = -_quat[2];
00809 //    _quat[3] =  _quat[3];
00810 }
00811
00813
00814 template <class ValueTypeT> inline
00815 const QuaternionBase<ValueTypeT>QuaternionBase<ValueTypeT>::inverse(void) const
00816 {
00817     QuaternionBase returnValue(*this);
00818
00819     returnValue.invert();
00820
00821     return returnValue;
00822 }
00823
00825
00826 // this one should be optimized a little bit too (GV)
00827 // Luckily somebody did it for us. ;) Thanks to Daniel Grest
00828 // (grest@mip.informatik.uni-kiel.de) for the code. (DR, 20030626)
00829
00830 template <class ValueTypeT> inline
00831 void QuaternionBase<ValueTypeT>::multVec(const VectorType &src,
00832                                                VectorType &dst) const
00833 {
00834     ValueTypeT rx,ry,rz;
00835     ValueTypeT QwQx, QwQy, QwQz, QxQy, QxQz, QyQz;
00836
00837     QwQx = _quat[3] * _quat[0];
00838     QwQy = _quat[3] * _quat[1];
00839     QwQz = _quat[3] * _quat[2];
00840     QxQy = _quat[0] * _quat[1];
00841     QxQz = _quat[0] * _quat[2];
00842     QyQz = _quat[1] * _quat[2];
00843
00844     ValueTypeT Vx = src[0], Vy = src[1], Vz = src[2];
00845
00846     rx = 2* (Vy * (-QwQz + QxQy) + Vz *( QwQy + QxQz));
00847     ry = 2* (Vx * ( QwQz + QxQy) + Vz *(-QwQx + QyQz));
00848     rz = 2* (Vx * (-QwQy + QxQz) + Vy *( QwQx + QyQz));
00849
00850     ValueTypeT QwQw, QxQx, QyQy, QzQz;
00851
00852     QwQw = _quat[3] * _quat[3];
00853     QxQx = _quat[0] * _quat[0];
00854     QyQy = _quat[1] * _quat[1];
00855     QzQz = _quat[2] * _quat[2];
00856
00857     rx+= Vx * (QwQw + QxQx - QyQy - QzQz);
00858     ry+= Vy * (QwQw - QxQx + QyQy - QzQz);
00859     rz+= Vz * (QwQw - QxQx - QyQy + QzQz);
00860
00861     dst.setValues(rx,ry,rz);
00862 }
00863
00868 template <class ValueTypeT> inline
00869 void QuaternionBase<ValueTypeT>::scaleAngle(ValueTypeT scaleFactor)
00870 {
00871     VectorType axis;
00872     ValueTypeT radians;
00873
00874     getValueAsAxisRad(axis, radians);
00875     setValueAsAxisRad(axis, radians * scaleFactor);
00876 }
00877
00878 template <class ValueTypeT> inline
00879 void QuaternionBase<ValueTypeT>::slerpThis(const QuaternionBase &rot0,
00880                                            const QuaternionBase &rot1,
00881                                            const ValueTypeT      t)
00882 {
00883     slerp(rot0, rot1, *this, t);
00884 }
00885
00886 template <class ValueTypeT> inline
00887 void QuaternionBase<ValueTypeT>::mult(const QuaternionBase &other)
00888 {
00889     mult(_quat, other._quat);
00890 }
00891
00892 template <class ValueTypeT> inline
00893 void QuaternionBase<ValueTypeT>::multLeft(const QuaternionBase &other)
00894 {
00895     mult(other._quat, _quat);
00896 }
00897
00898 template <class ValueTypeT> inline
00899 bool QuaternionBase<ValueTypeT>::equals(const QuaternionBase &rot,
00900                                         const ValueTypeT tolerance) const
00901 {
00902     bool returnValue = true;
00903
00904     for(UInt32 i = 0; i < 4; i++)
00905     {
00906         returnValue &= ( (    _quat[i] - rot._quat[i] <= tolerance) &&
00907                          (rot._quat[i] -     _quat[i] <= tolerance));
00908     }
00909
00910     return returnValue;
00911 }
00912
00913 /*-------------------------------------------------------------------------*/
00914 /*                           Element Access                                */
00915
00916 template <class ValueTypeT> inline
00917 ValueTypeT &QuaternionBase<ValueTypeT>::operator [](const UInt32 index)
00918 {
00919     return _quat[index];
00920 }
00921
00922 template <class ValueTypeT> inline
00923 const ValueTypeT &QuaternionBase<ValueTypeT>::operator [](
00924     const UInt32 index) const
00925 {
00926     return _quat[index];
00927 }
00928
00929 /*-------------------------------------------------------------------------*/
00930 /*                             Math Operators                              */
00931
00932 template <class ValueTypeT> inline
00933 void QuaternionBase<ValueTypeT>::operator *=(const QuaternionBase &other)
00934 {
00935     mult(_quat, other._quat);
00936 }
00937
00938 /*-------------------------------------------------------------------------*/
00939 /*                             Assignment                                  */
00940
00941 template <class ValueTypeT> inline
00942 QuaternionBase<ValueTypeT> &
00943     QuaternionBase<ValueTypeT>::operator =(const QuaternionBase &source)
00944 {
00945     if(this == &source)
00946         return *this;
00947
00948     for(UInt32 i = 0; i < 4; i++)
00949     {
00950         _quat[i] = source._quat[i];
00951     }
00952
00953     return *this;
00954 }
00955
00956 /*-------------------------------------------------------------------------*/
00957 /*                             Comparison                                  */
00958
00959 template <class ValueTypeT> inline
00960 bool QuaternionBase<ValueTypeT>::operator ==(const QuaternionBase &other) const
00961 {
00962     return equals(other, Eps);
00963 }
00964
00965 template <class ValueTypeT> inline
00966 bool QuaternionBase<ValueTypeT>::operator !=(const QuaternionBase &other) const
00967 {
00968     return ! (*this == other);
00969 }
00970
00971 /*-------------------------------------------------------------------------*/
00972 /*                               Helper                                    */
00973
00975
00976 template <class ValueTypeT> inline
00977 void QuaternionBase<ValueTypeT>::slerp(const QuaternionBase &rot0,
00978                                        const QuaternionBase &rot1,
00979                                              QuaternionBase &result,
00980                                        const ValueTypeT      t     )
00981 {
00982     ValueTypeT rot1q[4];
00983
00984     Real64     omega;
00985     Real64     cosom;
00986     Real64     sinom;
00987     Real64     scalerot0;
00988     Real64     scalerot1;
00989
00990     UInt32     i;
00991     UInt32     j;
00992
00993     // Calculate the cosine
00994     cosom =
00995         rot0._quat[0] * rot1._quat[0] +
00996         rot0._quat[1] * rot1._quat[1] +
00997         rot0._quat[2] * rot1._quat[2] +
00998         rot0._quat[3] * rot1._quat[3];
00999
01000     // adjust signs if necessary
01001     if(cosom < 0.0)
01002     {
01003         cosom = -cosom;
01004
01005         for(j = 0; j < 4; j++)
01006         {
01007             rot1q[j] = -rot1[j];
01008         }
01009     }
01010     else
01011     {
01012         for(j = 0; j < 4; j++)
01013         {
01014             rot1q[j] = rot1[j];
01015         }
01016     }
01017
01018     // calculate interpolating coeffs
01019     if ((1.0 - cosom) > 0.00001)
01020     {
01021         // standard case
01022         omega = osgACos(cosom);
01023         sinom = osgSin(omega);
01024         scalerot0 = osgSin((1.0 - t) * omega) / sinom;
01025         scalerot1 = osgSin(t * omega) / sinom;
01026     }
01027     else
01028     {
01029         // rot0 and rot1 very close - just do linear interp.
01030         scalerot0 = 1.0 - t;
01031         scalerot1 = t;
01032     }
01033
01034     // build the new quarternion
01035     for (i = 0; i < 4; i++)
01036         result[i] = ValueTypeT(scalerot0 * rot0._quat[i] +
01037                                scalerot1 * rot1q[i]);
01038 }
01043 template <class ValueTypeT> inline
01044 void QuaternionBase<ValueTypeT>::mult(const ValueTypeT rVal1[4],
01045                                       const ValueTypeT rVal2[4])
01046 {
01047     ValueTypeT s1, s2, s3, s4, s5, s6, s7, s8, s9, t;
01048
01049     s1 = (rVal1[2] - rVal1[1]) * (rVal2[1] - rVal2[2]);
01050     s2 = (rVal1[3] + rVal1[0]) * (rVal2[3] + rVal2[0]);
01051     s3 = (rVal1[3] - rVal1[0]) * (rVal2[1] + rVal2[2]);
01052     s4 = (rVal1[2] + rVal1[1]) * (rVal2[3] - rVal2[0]);
01053     s5 = (rVal1[2] - rVal1[0]) * (rVal2[0] - rVal2[1]);
01054     s6 = (rVal1[2] + rVal1[0]) * (rVal2[0] + rVal2[1]);
01055     s7 = (rVal1[3] + rVal1[1]) * (rVal2[3] - rVal2[2]);
01056     s8 = (rVal1[3] - rVal1[1]) * (rVal2[3] + rVal2[2]);
01057
01058     s9 = s6 + s7 + s8;
01059
01060     t  = (s5 + s9) / 2.0f;
01061
01062     _quat[3] = s1 + t - s6;
01063     _quat[0] = s2 + t - s9;
01064     _quat[1] = s3 + t - s8;
01065     _quat[2] = s4 + t - s7;
01066
01067     normalize();
01068 }
01069
01070 OSG_END_NAMESPACE