OSG::QuaternionBase< ValueTypeT > Class Template Reference
[Base, Objects, libOSGBase]

00270     {
00271         Q_X = 0,
00272         Q_Y = 1,
00273         Q_Z = 2,
00274         Q_W = 3
00275     };

00089 {
00090     _quat[0] =
00091         _quat[1] =
00092         _quat[2] = TypeTraits<ValueTypeT>::getZeroElement();
00093
00094     _quat[3] = TypeTraits<ValueTypeT>::getOneElement();
00095 }

00100 {
00101     for(UInt32 i = 0; i < 4; i++)
00102     {
00103         _quat[i] = source._quat[i];
00104     }
00105 }

00110 {
00111     setValue(matrix);
00112 }

00118 {
00119     setValueAsAxisRad(axis, angle);
00120 }

00127 {
00128     setValue(rotateFrom, rotateTo);
00129 }

00136 {
00137 }

00065 {
00066     return _identity;
00067 }

00076 {
00077     QuaternionBase returnValue;
00078
00079     slerp(rot0, rot1, returnValue, t);
00080
00081     return returnValue;
00082 }

00146 {
00147     _quat[0] =
00148         _quat[1] =
00149         _quat[2] = TypeTraits<ValueTypeT>::getZeroElement();
00150
00151     _quat[3] = TypeTraits<ValueTypeT>::getOneElement();
00152 }

00161 {
00162     setValueAsAxisRad(valsP[0], valsP[1], valsP[2], valsP[3]);
00163 }

00172 {
00173     setValueAsAxisDeg(valsP[0], valsP[1], valsP[2], valsP[3]);
00174 }

00181 {
00182     UInt32 i;
00183
00184     for(i = 0; i < 4; i++)
00185     {
00186         _quat[i] = valsP[i];
00187     }
00188 }

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 }

00226 {
00227     setValueAsAxisRad(x,y,z,osgDegree2Rad(w));
00228 }

00239 {
00240     _quat[0] = x;
00241     _quat[1] = y;
00242     _quat[2] = z;
00243     _quat[3] = w;
00244 }

00248 {
00249     _quat[0] = quat[0];
00250     _quat[1] = quat[1];
00251     _quat[2] = quat[2];
00252     _quat[3] = quat[3];
00253 }

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 }

00340 {
00341   setValueAsAxisRad(axis[0], axis[1], axis[2], angle);
00342 }

00351 {
00352   setValueAsAxisDeg(axis[0], axis[1], axis[2], angle);
00353 }

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 }

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 }

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 }

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 }

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 }

00561 {
00562     setValueAsAxisRad(szString);
00563 }

00567 {
00568     setValueAsAxisRad(szString);
00569 }

00574 {
00575     setValueAsAxisRad(szString);
00576 }

00580 {
00581     setValueAsAxisRad(szString);
00582 }

00592 {
00593     return _quat;
00594 }

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 }

00605 {
00606     getValueAsAxisDeg(x, y, z, w);
00607
00608     w = osgDegree2Rad(w);
00609 }

00654 {
00655     x = _quat[0];
00656     y = _quat[1];
00657     z = _quat[2];
00658     w = _quat[3];
00659 }

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 }

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 }

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 }

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 }

00743 {
00744     return _quat[0];
00745 }

00749 {
00750     return _quat[1];
00751 }

00755 {
00756     return _quat[2];
00757 }

00761 {
00762     return _quat[3];
00763 }

00772 {
00773     return osgSqrt(_quat[0] * _quat[0] +
00774                    _quat[1] * _quat[1] +
00775                    _quat[2] * _quat[2] +
00776                    _quat[3] * _quat[3]);
00777 }

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 }

00805 {
00806     _quat[0] = -_quat[0];
00807     _quat[1] = -_quat[1];
00808     _quat[2] = -_quat[2];
00809 //    _quat[3] =  _quat[3];
00810 }

00816 {
00817     QuaternionBase returnValue(*this);
00818
00819     returnValue.invert();
00820
00821     return returnValue;
00822 }

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 }

00870 {
00871     VectorType axis;
00872     ValueTypeT radians;
00873
00874     getValueAsAxisRad(axis, radians);
00875     setValueAsAxisRad(axis, radians * scaleFactor);
00876 }

00882 {
00883     slerp(rot0, rot1, *this, t);
00884 }

00888 {
00889     mult(_quat, other._quat);
00890 }

00894 {
00895     mult(other._quat, _quat);
00896 }

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 }

00918 {
00919     return _quat[index];
00920 }

00925 {
00926     return _quat[index];
00927 }

00934 {
00935     mult(_quat, other._quat);
00936 }

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 }

00961 {
00962     return equals(other, Eps);
00963 }

00967 {
00968     return ! (*this == other);
00969 }

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 }

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 }