GTestAndEngine: quatern.c Source File

00001 /****************************************************************************************/
00002 /*  QUATERN.C                                                                           */
00003 /*                                                                                      */
00004 /*  Author: Mike Sandige                                                                */
00005 /*  Description: Quaternion mathematical system implementation                          */
00006 /*                                                                                      */
00007 /*  The contents of this file are subject to the Genesis3D Public License               */
00008 /*  Version 1.01 (the "License"); you may not use this file except in                   */
00009 /*  compliance with the License. You may obtain a copy of the License at                */
00010 /*  http://www.genesis3d.com                                                            */
00011 /*                                                                                      */
00012 /*  Software distributed under the License is distributed on an "AS IS"                 */
00013 /*  basis, WITHOUT WARRANTY OF ANY KIND, either express or implied.  See                */
00014 /*  the License for the specific language governing rights and limitations              */
00015 /*  under the License.                                                                  */
00016 /*                                                                                      */
00017 /*  The Original Code is Genesis3D, released March 25, 1999.                            */
00018 /*Genesis3D Version 1.1 released November 15, 1999                            */
00019 /*  Copyright (C) 1999 WildTangent, Inc. All Rights Reserved           */
00020 /*                                                                                      */
00021 /****************************************************************************************/
00022 #include <assert.h>
00023 #include <math.h>
00024 #include "basetype.h"
00025 #include "quatern.h"
00026 
00027 
00028 #ifndef NDEBUG
00029         static geQuaternion_MaximalAssertionMode = GE_TRUE;
00030         #define geQuaternion_Assert if (geQuaternion_MaximalAssertionMode) assert
00031 
00032         void GENESISCC geQuaternion_SetMaximalAssertionMode( geBoolean Enable )
00033         {
00034                 assert( (Enable == GE_TRUE) || (Enable == GE_FALSE) );
00035                 geQuaternion_MaximalAssertionMode = Enable;
00036         }
00037 #else
00038         #define geQuaternion_Assert assert
00039 #endif
00040 
00041 #define UNIT_TOLERANCE 0.001  
00042         // Quaternion magnitude must be closer than this tolerance to 1.0 to be 
00043         // considered a unit quaternion
00044 
00045 #define QZERO_TOLERANCE 0.00001 
00046         // quaternion magnitude must be farther from this tolerance to 0.0 to be 
00047         // normalized
00048 
00049 #define TRACE_QZERO_TOLERANCE 0.1
00050         // trace of matrix must be greater than this to be used for converting a matrix
00051         // to a quaternion.
00052 
00053 #define AA_QZERO_TOLERANCE 0.0001
00054         
00055 
00056 geBoolean GENESISCC geQuaternion_IsValid(const geQuaternion *Q)
00057 {
00058         if (Q == NULL)
00059                 return GE_FALSE;
00060         if ((Q->W * Q->W) < 0.0f)
00061                 return GE_FALSE;
00062         if ((Q->X * Q->X) < 0.0f)
00063                 return GE_FALSE;
00064         if ((Q->Y * Q->Y) < 0.0f)
00065                 return GE_FALSE;
00066         if ((Q->Z * Q->Z) < 0.0f)
00067                 return GE_FALSE;
00068         return GE_TRUE;
00069 }
00070 
00071 void GENESISCC geQuaternion_Set( 
00072         geQuaternion *Q, geFloat W, geFloat X, geFloat Y, geFloat Z)
00073 {
00074         assert( Q != NULL );
00075 
00076         Q->W = W;
00077         Q->X = X;
00078         Q->Y = Y;
00079         Q->Z = Z;
00080         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00081 }
00082 
00083 void GENESISCC geQuaternion_SetVec3d(
00084         geQuaternion *Q, geFloat W, const geVec3d *V)
00085 {
00086         assert( Q != NULL );
00087         assert( geVec3d_IsValid(V) != GE_FALSE );
00088 
00089         Q->W = W;
00090         Q->X = V->X;
00091         Q->Y = V->Y;
00092         Q->Z = V->Z;
00093 }       
00094 
00095 void GENESISCC geQuaternion_Get( 
00096         const geQuaternion *Q, 
00097         geFloat *W, 
00098         geFloat *X, 
00099         geFloat *Y, 
00100         geFloat *Z)
00101         // get quaternion components into W,X,Y,Z
00102 {
00103         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00104         assert( W != NULL );
00105         assert( X != NULL );
00106         assert( Y != NULL );
00107         assert( Z != NULL );
00108 
00109         *W = Q->W;
00110         *X = Q->X;
00111         *Y = Q->Y;
00112         *Z = Q->Z;
00113 }
00114 
00115 GENESISAPI void GENESISCC geQuaternion_SetFromAxisAngle(geQuaternion *Q, const geVec3d *Axis, geFloat Theta)
00116         // set a quaternion from an axis and a rotation around the axis
00117 {
00118         geFloat sinTheta;
00119         assert( Q != NULL);
00120         assert( geVec3d_IsValid(Axis) != GE_FALSE);
00121         assert( (Theta * Theta) >= 0.0f );
00122         assert( ( fabs(geVec3d_Length(Axis)-1.0f) < AA_QZERO_TOLERANCE) );
00123         
00124         Theta = Theta * (geFloat)0.5f;
00125         Q->W     = (geFloat) cos(Theta);
00126         sinTheta = (geFloat) sin(Theta);
00127         Q->X = sinTheta * Axis->X;
00128         Q->Y = sinTheta * Axis->Y;
00129         Q->Z = sinTheta * Axis->Z;
00130 
00131         geQuaternion_Assert( geQuaternion_IsUnit(Q) == GE_TRUE );
00132 }
00133 
00134 
00135 geBoolean GENESISCC geQuaternion_GetAxisAngle(const geQuaternion *Q, geVec3d *Axis, geFloat *Theta)
00136 {       
00137         geFloat OneOverSinTheta;
00138         geFloat HalfTheta;
00139         assert( Q != NULL );
00140         assert( Axis != NULL );
00141         assert( Theta != NULL );
00142         geQuaternion_Assert( geQuaternion_IsUnit(Q) != GE_FALSE );
00143         
00144         HalfTheta  = (geFloat)acos( Q->W );
00145         if (HalfTheta>QZERO_TOLERANCE)
00146                 {
00147                         OneOverSinTheta = 1.0f / (geFloat)sin( HalfTheta );
00148                         Axis->X = OneOverSinTheta * Q->X;
00149                         Axis->Y = OneOverSinTheta * Q->Y;
00150                         Axis->Z = OneOverSinTheta * Q->Z;
00151                         *Theta = 2.0f * HalfTheta;
00152                         geQuaternion_Assert( geVec3d_IsValid(Axis) != GE_FALSE );
00153                         geQuaternion_Assert( (*Theta * *Theta) >= 0.0f);
00154                         return GE_TRUE;
00155                 }
00156         else
00157                 {
00158                         Axis->X = Axis->Y = Axis->Z = 0.0f;
00159                         *Theta = 0.0f;
00160                         return GE_FALSE;
00161                 }
00162 }
00163 
00164 
00165 void GENESISCC geQuaternion_GetVec3d( 
00166         const geQuaternion *Q, 
00167         geFloat *W, 
00168         geVec3d *V)
00169         // get quaternion components into W and V
00170 {
00171         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00172         assert( W != NULL );
00173         assert( V != NULL );
00174         
00175         *W   = Q->W;
00176         V->X = Q->X;
00177         V->Y = Q->Y;
00178         V->Z = Q->Z;
00179 }
00180 
00181 
00182 void GENESISCC geQuaternion_FromMatrix(
00183         const geXForm3d         *M,
00184               geQuaternion      *Q)
00185         // takes upper 3 by 3 portion of matrix (rotation sub matrix) 
00186         // and generates a quaternion
00187 {
00188         geFloat trace,s;
00189 
00190         assert( M != NULL );
00191         assert( Q != NULL );
00192         geQuaternion_Assert( geXForm3d_IsOrthonormal(M)==GE_TRUE );
00193 
00194         trace = M->AX + M->BY + M->CZ;
00195         if (trace > 0.0f)
00196                 {
00197                         s = (geFloat)sqrt(trace + 1.0f);
00198                         Q->W = s * 0.5f;
00199                         s = 0.5f / s;
00200 
00201                         Q->X = (M->CY - M->BZ) * s;
00202                         Q->Y = (M->AZ - M->CX) * s;
00203                         Q->Z = (M->BX - M->AY) * s;
00204                 }
00205         else
00206                 {
00207                         int biggest;
00208                         enum {A,E,I};
00209                         if (M->AX > M->BY)
00210                                 {
00211                                         if (M->CZ > M->AX)
00212                                                 biggest = I;    
00213                                         else
00214                                                 biggest = A;
00215                                 }
00216                         else
00217                                 {
00218                                         if (M->CZ > M->AX)
00219                                                 biggest = I;
00220                                         else
00221                                                 biggest = E;
00222                                 }
00223 
00224                         // in the unusual case the original trace fails to produce a good sqrt, try others...
00225                         switch (biggest)
00226                                 {
00227                                 case A:
00228                                         s = (geFloat)sqrt( M->AX - (M->BY + M->CZ) + 1.0);
00229                                         if (s > TRACE_QZERO_TOLERANCE)
00230                                                 {
00231                                                         Q->X = s * 0.5f;
00232                                                         s = 0.5f / s;
00233                                                         Q->W = (M->CY - M->BZ) * s;
00234                                                         Q->Y = (M->AY + M->BX) * s;
00235                                                         Q->Z = (M->AZ + M->CX) * s;
00236                                                         break;
00237                                                 }
00238                                                         // I
00239                                                         s = (geFloat)sqrt( M->CZ - (M->AX + M->BY) + 1.0);
00240                                                         if (s > TRACE_QZERO_TOLERANCE)
00241                                                                 {
00242                                                                         Q->Z = s * 0.5f;
00243                                                                         s = 0.5f / s;
00244                                                                         Q->W = (M->BX - M->AY) * s;
00245                                                                         Q->X = (M->CX + M->AZ) * s;
00246                                                                         Q->Y = (M->CY + M->BZ) * s;
00247                                                                         break;
00248                                                                 }
00249                                                         // E
00250                                                         s = (geFloat)sqrt( M->BY - (M->CZ + M->AX) + 1.0);
00251                                                         if (s > TRACE_QZERO_TOLERANCE)
00252                                                                 {
00253                                                                         Q->Y = s * 0.5f;
00254                                                                         s = 0.5f / s;
00255                                                                         Q->W = (M->AZ - M->CX) * s;
00256                                                                         Q->Z = (M->BZ + M->CY) * s;
00257                                                                         Q->X = (M->BX + M->AY) * s;
00258                                                                         break;
00259                                                                 }
00260                                                         break;
00261                                 case E:
00262                                         s = (geFloat)sqrt( M->BY - (M->CZ + M->AX) + 1.0);
00263                                         if (s > TRACE_QZERO_TOLERANCE)
00264                                                 {
00265                                                         Q->Y = s * 0.5f;
00266                                                         s = 0.5f / s;
00267                                                         Q->W = (M->AZ - M->CX) * s;
00268                                                         Q->Z = (M->BZ + M->CY) * s;
00269                                                         Q->X = (M->BX + M->AY) * s;
00270                                                         break;
00271                                                 }
00272                                                         // I
00273                                                         s = (geFloat)sqrt( M->CZ - (M->AX + M->BY) + 1.0);
00274                                                         if (s > TRACE_QZERO_TOLERANCE)
00275                                                                 {
00276                                                                         Q->Z = s * 0.5f;
00277                                                                         s = 0.5f / s;
00278                                                                         Q->W = (M->BX - M->AY) * s;
00279                                                                         Q->X = (M->CX + M->AZ) * s;
00280                                                                         Q->Y = (M->CY + M->BZ) * s;
00281                                                                         break;
00282                                                                 }
00283                                                         // A
00284                                                         s = (geFloat)sqrt( M->AX - (M->BY + M->CZ) + 1.0);
00285                                                         if (s > TRACE_QZERO_TOLERANCE)
00286                                                                 {
00287                                                                         Q->X = s * 0.5f;
00288                                                                         s = 0.5f / s;
00289                                                                         Q->W = (M->CY - M->BZ) * s;
00290                                                                         Q->Y = (M->AY + M->BX) * s;
00291                                                                         Q->Z = (M->AZ + M->CX) * s;
00292                                                                         break;
00293                                                                 }
00294                                         break;
00295                                 case I:
00296                                         s = (geFloat)sqrt( M->CZ - (M->AX + M->BY) + 1.0);
00297                                         if (s > TRACE_QZERO_TOLERANCE)
00298                                                 {
00299                                                         Q->Z = s * 0.5f;
00300                                                         s = 0.5f / s;
00301                                                         Q->W = (M->BX - M->AY) * s;
00302                                                         Q->X = (M->CX + M->AZ) * s;
00303                                                         Q->Y = (M->CY + M->BZ) * s;
00304                                                         break;
00305                                                 }
00306                                                         // A
00307                                                         s = (geFloat)sqrt( M->AX - (M->BY + M->CZ) + 1.0);
00308                                                         if (s > TRACE_QZERO_TOLERANCE)
00309                                                                 {
00310                                                                         Q->X = s * 0.5f;
00311                                                                         s = 0.5f / s;
00312                                                                         Q->W = (M->CY - M->BZ) * s;
00313                                                                         Q->Y = (M->AY + M->BX) * s;
00314                                                                         Q->Z = (M->AZ + M->CX) * s;
00315                                                                         break;
00316                                                                 }
00317                                                         // E
00318                                                         s = (geFloat)sqrt( M->BY - (M->CZ + M->AX) + 1.0);
00319                                                         if (s > TRACE_QZERO_TOLERANCE)
00320                                                                 {
00321                                                                         Q->Y = s * 0.5f;
00322                                                                         s = 0.5f / s;
00323                                                                         Q->W = (M->AZ - M->CX) * s;
00324                                                                         Q->Z = (M->BZ + M->CY) * s;
00325                                                                         Q->X = (M->BX + M->AY) * s;
00326                                                                         break;
00327                                                                 }
00328                                         break;
00329                                 default:
00330                                         assert(0);
00331                                 }
00332                 }
00333         geQuaternion_Assert( geQuaternion_IsUnit(Q) == GE_TRUE );
00334 }
00335 
00336 GENESISAPI void GENESISCC geQuaternion_ToMatrix(
00337         const geQuaternion      *Q, 
00338                   geXForm3d             *M)
00339         // takes a unit quaternion and fills out an equivelant rotation
00340         // portion of a xform
00341 {
00342         geFloat X2,Y2,Z2;               //2*QX, 2*QY, 2*QZ
00343         geFloat XX2,YY2,ZZ2;    //2*QX*QX, 2*QY*QY, 2*QZ*QZ
00344         geFloat XY2,XZ2,XW2;    //2*QX*QY, 2*QX*QZ, 2*QX*QW
00345         geFloat YZ2,YW2,ZW2;    // ...
00346 
00347         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00348         assert( M != NULL );
00349         geQuaternion_Assert( geQuaternion_IsUnit(Q) == GE_TRUE );
00350         
00351         
00352         X2  = 2.0f * Q->X;
00353         XX2 = X2   * Q->X;
00354         XY2 = X2   * Q->Y;
00355         XZ2 = X2   * Q->Z;
00356         XW2 = X2   * Q->W;
00357 
00358         Y2  = 2.0f * Q->Y;
00359         YY2 = Y2   * Q->Y;
00360         YZ2 = Y2   * Q->Z;
00361         YW2 = Y2   * Q->W;
00362         
00363         Z2  = 2.0f * Q->Z;
00364         ZZ2 = Z2   * Q->Z;
00365         ZW2 = Z2   * Q->W;
00366         
00367         M->AX = 1.0f - YY2 - ZZ2;
00368         M->AY = XY2  - ZW2;
00369         M->AZ = XZ2  + YW2;
00370 
00371         M->BX = XY2  + ZW2;
00372         M->BY = 1.0f - XX2 - ZZ2;
00373         M->BZ = YZ2  - XW2;
00374 
00375         M->CX = XZ2  - YW2;
00376         M->CY = YZ2  + XW2;
00377         M->CZ = 1.0f - XX2 - YY2;
00378 
00379         M->Translation.X = M->Translation.Y = M->Translation.Z = 0.0f;
00380 
00381         geQuaternion_Assert( geXForm3d_IsOrthonormal(M)==GE_TRUE );
00382 
00383 }
00384 
00385 
00386 #define EPSILON (0.00001)
00387 
00388 
00389 void GENESISCC geQuaternion_Slerp(
00390         const geQuaternion              *Q0, 
00391         const geQuaternion              *Q1, 
00392         geFloat                                 T,              
00393         geQuaternion                    *QT)
00394         // spherical interpolation between q0 and q1.   0<=t<=1 
00395         // resulting quaternion is 'between' q0 and q1
00396         // with t==0 being all q0, and t==1 being all q1.
00397 {
00398         geFloat omega,cosom,sinom,Scale0,Scale1;
00399         geQuaternion QL;
00400         assert( Q0 != NULL );
00401         assert( Q1 != NULL );
00402         assert( QT  != NULL );
00403         assert( ( 0 <= T ) && ( T <= 1.0f ) );
00404         geQuaternion_Assert( geQuaternion_IsUnit(Q0) == GE_TRUE );
00405         geQuaternion_Assert( geQuaternion_IsUnit(Q1) == GE_TRUE );
00406 
00407         cosom =         (Q0->W * Q1->W) + (Q0->X * Q1->X) 
00408                           + (Q0->Y * Q1->Y) + (Q0->Z * Q1->Z);
00409 
00410         if (cosom < 0)
00411                 {
00412                         cosom = -cosom;
00413                         QL.X = -Q1->X;
00414                         QL.Y = -Q1->Y;
00415                         QL.Z = -Q1->Z;
00416                         QL.W = -Q1->W;
00417                 }
00418         else
00419                 {
00420                         QL = *Q1;
00421                 }
00422                         
00423 
00424         if ( (1.0f - cosom) > EPSILON )
00425                 {
00426                         omega  = (geFloat) acos( cosom );
00427                         sinom  = (geFloat) sin( omega );
00428                         Scale0 = (geFloat) sin( (1.0f-T) * omega) / sinom;
00429                         Scale1 = (geFloat) sin( T*omega) / sinom;
00430                 }
00431         else
00432                 {
00433                         // has numerical difficulties around cosom == 0
00434                         // in this case degenerate to linear interpolation
00435                 
00436                         Scale0 = 1.0f - T;
00437                         Scale1 = T;
00438                 }
00439 
00440 
00441         QT-> X = Scale0 * Q0->X + Scale1 * QL.X;
00442         QT-> Y = Scale0 * Q0->Y + Scale1 * QL.Y;
00443         QT-> Z = Scale0 * Q0->Z + Scale1 * QL.Z;
00444         QT-> W = Scale0 * Q0->W + Scale1 * QL.W;
00445         geQuaternion_Assert( geQuaternion_IsUnit(QT) == GE_TRUE );
00446 }
00447 
00448 
00449 
00450 
00451 void GENESISCC geQuaternion_SlerpNotShortest(
00452         const geQuaternion              *Q0, 
00453         const geQuaternion              *Q1, 
00454         geFloat                                 T,              
00455         geQuaternion                    *QT)
00456         // spherical interpolation between q0 and q1.   0<=t<=1 
00457         // resulting quaternion is 'between' q0 and q1
00458         // with t==0 being all q0, and t==1 being all q1.
00459 {
00460         geFloat omega,cosom,sinom,Scale0,Scale1;
00461         assert( Q0 != NULL );
00462         assert( Q1 != NULL );
00463         assert( QT  != NULL );
00464         assert( ( 0 <= T ) && ( T <= 1.0f ) );
00465         geQuaternion_Assert( geQuaternion_IsUnit(Q0) == GE_TRUE );
00466         geQuaternion_Assert( geQuaternion_IsUnit(Q1) == GE_TRUE );
00467 
00468         cosom =         (Q0->W * Q1->W) + (Q0->X * Q1->X) 
00469                           + (Q0->Y * Q1->Y) + (Q0->Z * Q1->Z);
00470         if ( (1.0f + cosom) > EPSILON )
00471                 {
00472                         if ( (1.0f - cosom) > EPSILON )
00473                                 {
00474                                         omega  = (geFloat) acos( cosom );
00475                                         sinom  = (geFloat) sin( omega );
00476                                         // has numerical difficulties around cosom == nPI/2
00477                                         // in this case everything is up for grabs... 
00478                                         //  ...degenerate to linear interpolation
00479                                         if (sinom < EPSILON)
00480                                                 {
00481                                                         Scale0 = 1.0f - T;
00482                                                         Scale1 = T;     
00483                                                 }
00484                                         else
00485                                                 {
00486                                                         Scale0 = (geFloat) sin( (1.0f-T) * omega) / sinom;
00487                                                         Scale1 = (geFloat) sin( T*omega) / sinom;
00488                                                 }
00489                                 }
00490                         else
00491                                 {
00492                                         // has numerical difficulties around cosom == 0
00493                                         // in this case degenerate to linear interpolation
00494                                 
00495                                         Scale0 = 1.0f - T;
00496                                         Scale1 = T;
00497                                 }
00498                         QT-> X = Scale0 * Q0->X + Scale1 * Q1->X;
00499                         QT-> Y = Scale0 * Q0->Y + Scale1 * Q1->Y;
00500                         QT-> Z = Scale0 * Q0->Z + Scale1 * Q1->Z;
00501                         QT-> W = Scale0 * Q0->W + Scale1 * Q1->W;
00502                         //#pragma message (" ack:!!!!!!")
00503                         //geQuaternionNormalize(QT); 
00504                         geQuaternion_Assert( geQuaternion_IsUnit(QT));
00505                 }
00506         else
00507                 {
00508                         QT->X = -Q0->Y; 
00509                         QT->Y =  Q0->X;
00510                         QT->Z = -Q0->W;
00511                         QT->W =  Q0->Z;
00512                         Scale0 = (geFloat) sin( (1.0f - T) * (QUATERNION_PI*0.5) );
00513                         Scale1 = (geFloat) sin( T * (QUATERNION_PI*0.5) );
00514                         QT-> X = Scale0 * Q0->X + Scale1 * QT->X;
00515                         QT-> Y = Scale0 * Q0->Y + Scale1 * QT->Y;
00516                         QT-> Z = Scale0 * Q0->Z + Scale1 * QT->Z;
00517                         QT-> W = Scale0 * Q0->W + Scale1 * QT->W;
00518                         geQuaternion_Assert( geQuaternion_IsUnit(QT));
00519                 }
00520 }
00521 
00522 void GENESISCC geQuaternion_Multiply(
00523         const geQuaternion      *Q1, 
00524         const geQuaternion      *Q2, 
00525         geQuaternion            *Q)
00526         // multiplies q1 * q2, and places the result in q.
00527         // no failure.  renormalization not automatic
00528 
00529 {
00530         geQuaternion Q1L,Q2L;
00531         assert( geQuaternion_IsValid(Q1) != GE_FALSE );
00532         assert( geQuaternion_IsValid(Q2) != GE_FALSE );
00533         assert( Q  != NULL );
00534         Q1L = *Q1;
00535         Q2L = *Q2;
00536 
00537         Q->W  = (  (Q1L.W*Q2L.W) - (Q1L.X*Q2L.X) 
00538                          - (Q1L.Y*Q2L.Y) - (Q1L.Z*Q2L.Z) );
00539 
00540         Q->X  = (  (Q1L.W*Q2L.X) + (Q1L.X*Q2L.W) 
00541                          + (Q1L.Y*Q2L.Z) - (Q1L.Z*Q2L.Y) );
00542 
00543         Q->Y  = (  (Q1L.W*Q2L.Y) - (Q1L.X*Q2L.Z) 
00544                          + (Q1L.Y*Q2L.W) + (Q1L.Z*Q2L.X) );
00545 
00546         Q->Z  = (  (Q1L.W*Q2L.Z) + (Q1L.X*Q2L.Y) 
00547                          - (Q1L.Y*Q2L.X) + (Q1L.Z*Q2L.W) );
00548         geQuaternion_Assert( geQuaternion_IsValid(Q) != GE_FALSE );
00549 
00550 }
00551 
00552 
00553 void GENESISCC geQuaternion_Rotate(
00554         const geQuaternion      *Q, 
00555         const geVec3d         *V, 
00556         geVec3d                         *VRotated)
00557         // Rotates V by the quaternion Q, places the result in VRotated.
00558 {
00559         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00560         assert( geVec3d_IsValid(V)  != GE_FALSE );
00561         assert( VRotated  != NULL );
00562 
00563         geQuaternion_Assert( geQuaternion_IsUnit(Q) == GE_TRUE );
00564 
00565         {
00566                 geQuaternion Qinv,QV,QRotated, QT;
00567                 geFloat zero;
00568                 geQuaternion_SetVec3d(&QV ,0.0f,V);
00569                 geQuaternion_Inverse (Q,&Qinv);
00570                 geQuaternion_Multiply(Q,&QV,&QT);
00571                 geQuaternion_Multiply(&QT,&Qinv,&QRotated);
00572                 geQuaternion_GetVec3d(&QRotated,&zero,VRotated);
00573         }
00574 }
00575 
00576 
00577 
00578 geBoolean GENESISCC geQuaternion_IsUnit(const geQuaternion *Q)
00579         // returns GE_TRUE if Q is a unit geQuaternion.  GE_FALSE otherwise.
00580 {
00581         geFloat magnitude;
00582         assert( Q != NULL );
00583 
00584         magnitude  =   (Q->W * Q->W) + (Q->X * Q->X) 
00585                                           + (Q->Y * Q->Y) + (Q->Z * Q->Z);
00586 
00587         if (( magnitude < 1.0+UNIT_TOLERANCE ) && ( magnitude > 1.0-UNIT_TOLERANCE ))
00588                 return GE_TRUE;
00589         return GE_FALSE;
00590 }
00591 
00592 geFloat GENESISCC geQuaternion_Magnitude(const geQuaternion *Q)
00593         // returns Magnitude of Q.  
00594 {
00595 
00596         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00597         return   (Q->W * Q->W) + (Q->X * Q->X)  + (Q->Y * Q->Y) + (Q->Z * Q->Z);
00598 }
00599 
00600 
00601 GENESISAPI geFloat GENESISCC geQuaternion_Normalize(geQuaternion *Q)
00602         // normalizes Q to be a unit geQuaternion
00603 {
00604         geFloat magnitude,one_over_magnitude;
00605         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00606         
00607         magnitude =   (geFloat) sqrt ((Q->W * Q->W) + (Q->X * Q->X) 
00608                                                           + (Q->Y * Q->Y) + (Q->Z * Q->Z));
00609 
00610         if (( magnitude < QZERO_TOLERANCE ) && ( magnitude > -QZERO_TOLERANCE ))
00611                 {
00612                         return 0.0f;
00613                 }
00614 
00615         one_over_magnitude = 1.0f / magnitude;
00616 
00617         Q->W *= one_over_magnitude;
00618         Q->X *= one_over_magnitude;
00619         Q->Y *= one_over_magnitude;
00620         Q->Z *= one_over_magnitude;
00621         return magnitude;
00622 }
00623 
00624 
00625 GENESISAPI void GENESISCC geQuaternion_Copy(const geQuaternion *QSrc, geQuaternion *QDst)
00626         // copies quaternion QSrc into QDst
00627 {
00628         assert( geQuaternion_IsValid(QSrc) != GE_FALSE );
00629         assert( QDst != NULL );
00630         *QDst = *QSrc;
00631 }
00632 
00633 void GENESISCC geQuaternion_Inverse(const geQuaternion *Q, geQuaternion *QInv)
00634         // sets QInv to the inverse of Q.  
00635 {
00636         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00637         assert( QInv != NULL );
00638 
00639         QInv->W =  Q->W;
00640         QInv->X = -Q->X;
00641         QInv->Y = -Q->Y;
00642         QInv->Z = -Q->Z;
00643 }
00644 
00645 
00646 void GENESISCC geQuaternion_Add(
00647         const geQuaternion *Q1, 
00648         const geQuaternion *Q2, 
00649         geQuaternion *QSum)
00650         // QSum = Q1 + Q2  (result is not generally a unit quaternion!)
00651 {
00652         assert( geQuaternion_IsValid(Q1) != GE_FALSE );
00653         assert( geQuaternion_IsValid(Q2) != GE_FALSE );
00654         assert( QSum != NULL );
00655         QSum->W = Q1->W + Q2->W;
00656         QSum->X = Q1->X + Q2->X;
00657         QSum->Y = Q1->Y + Q2->Y;
00658         QSum->Z = Q1->Z + Q2->Z;
00659 }
00660 
00661 void GENESISCC geQuaternion_Subtract(
00662         const geQuaternion *Q1, 
00663         const geQuaternion *Q2, 
00664         geQuaternion *QSum)
00665         // QSum = Q1 - Q2  (result is not generally a unit quaternion!)
00666 {
00667         assert( geQuaternion_IsValid(Q1) != GE_FALSE );
00668         assert( geQuaternion_IsValid(Q2) != GE_FALSE );
00669         assert( QSum != NULL );
00670         QSum->W = Q1->W - Q2->W;
00671         QSum->X = Q1->X - Q2->X;
00672         QSum->Y = Q1->Y - Q2->Y;
00673         QSum->Z = Q1->Z - Q2->Z;
00674 }
00675 
00676 
00677 #define ZERO_EPSILON (0.0001f)
00678 static int32 geQuaternion_XFormTable[]={1768710981,560296816};
00679 
00680 void GENESISCC geQuaternion_Ln(
00681         const geQuaternion *Q, 
00682         geQuaternion *LnQ)
00683         // ln(Q) for unit quaternion only!
00684 {
00685         geFloat Theta;
00686         geQuaternion QL;
00687         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00688         assert( LnQ != NULL );
00689         geQuaternion_Assert( geQuaternion_IsUnit(Q) == GE_TRUE );
00690         
00691         if (Q->W < 0.0f)
00692                 {
00693                         QL.W = -Q->W;
00694                         QL.X = -Q->X;
00695                         QL.Y = -Q->Y;
00696                         QL.Z = -Q->Z;
00697                 }
00698         else
00699                 {
00700                         QL = *Q;
00701                 }
00702         Theta    = (geFloat)  acos( QL.W  );
00703          //  0 < Theta < pi
00704         if (Theta< ZERO_EPSILON)
00705                 {
00706                         // lim(t->0) of t/sin(t) = 1, so:
00707                         LnQ->W = 0.0f;
00708                         LnQ->X = QL.X;
00709                         LnQ->Y = QL.Y;
00710                         LnQ->Z = QL.Z;
00711                 }
00712         else
00713                 {
00714                         geFloat Theta_Over_sin_Theta =  Theta / (geFloat) sin ( Theta );
00715                         LnQ->W = 0.0f;
00716                         LnQ->X = Theta_Over_sin_Theta * QL.X;
00717                         LnQ->Y = Theta_Over_sin_Theta * QL.Y;
00718                         LnQ->Z = Theta_Over_sin_Theta * QL.Z;
00719                 }
00720 }
00721         
00722 void GENESISCC geQuaternion_Exp(
00723         const geQuaternion *Q,
00724         geQuaternion *ExpQ)
00725         // exp(Q) for pure quaternion only!  (zero scalar part (W))
00726 {
00727         geFloat Theta;
00728         geFloat sin_Theta_over_Theta;
00729 
00730         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00731         assert( ExpQ != NULL);
00732         assert( Q->W == 0.0 );  //check a range?
00733 
00734         Theta = (geFloat) sqrt(Q->X*Q->X  +  Q->Y*Q->Y  +  Q->Z*Q->Z);
00735         if (Theta > ZERO_EPSILON)
00736                 {
00737                         sin_Theta_over_Theta = (geFloat) sin(Theta) / Theta;
00738                 }
00739         else
00740                 {
00741                         sin_Theta_over_Theta = (geFloat) 1.0f;
00742                 }
00743 
00744         ExpQ->W   = (geFloat) cos(Theta);
00745         ExpQ->X   = sin_Theta_over_Theta * Q->X;
00746         ExpQ->Y   = sin_Theta_over_Theta * Q->Y;
00747         ExpQ->Z   = sin_Theta_over_Theta * Q->Z;
00748 }       
00749 
00750 void GENESISCC geQuaternion_Scale(
00751         const geQuaternion *Q,
00752         geFloat Scale,
00753         geQuaternion *QScaled)
00754         // Q = Q * Scale  (result is not generally a unit quaternion!)
00755 {
00756         assert( geQuaternion_IsValid(Q) != GE_FALSE );
00757         assert( (Scale * Scale) >=0.0f );
00758         assert( QScaled != NULL);
00759 
00760         QScaled->W = Q->W * Scale;
00761         QScaled->X = Q->X * Scale;
00762         QScaled->Y = Q->Y * Scale;
00763         QScaled->Z = Q->Z * Scale;
00764 }
00765 
00766 void GENESISCC geQuaternion_SetNoRotation(geQuaternion *Q)
00767         // sets Q to be a quaternion with no rotation (like an identity matrix)
00768 {
00769         Q->W = 1.0f;
00770         Q->X = Q->Y = Q->Z = 0.0f;
00771         
00772         /* this is equivelant to:
00773                 {
00774                         geXForm3d M;
00775                         geXForm3d_SetIdentity(&M);
00776                         geQuaternionFromMatrix(&M,Q);
00777                 }
00778         */
00779 }
00780 
00781 
00782 
00783 geBoolean GENESISCC geQuaternion_Compare( geQuaternion *Q1, geQuaternion *Q2, geFloat Tolerance )
00784 {
00785         assert( geQuaternion_IsValid(Q1) != GE_FALSE );
00786         assert( geQuaternion_IsValid(Q2) != GE_FALSE );
00787         assert ( Tolerance >= 0.0 );
00788 
00789         if (    // they are the same but with opposite signs
00790                         (               (fabs(Q1->X + Q2->X) <= Tolerance )  
00791                                 &&  (fabs(Q1->Y + Q2->Y) <= Tolerance )  
00792                                 &&  (fabs(Q1->Z + Q2->Z) <= Tolerance )  
00793                                 &&  (fabs(Q1->W + Q2->W) <= Tolerance )  
00794                         )
00795                   ||  // they are the same with same signs
00796                         (               (fabs(Q1->X - Q2->X) <= Tolerance )  
00797                                 &&  (fabs(Q1->Y - Q2->Y) <= Tolerance )  
00798                                 &&  (fabs(Q1->Z - Q2->Z) <= Tolerance )  
00799                                 &&  (fabs(Q1->W - Q2->W) <= Tolerance )  
00800                         )
00801                 )
00802                 return GE_TRUE;
00803         else
00804                 return GE_FALSE;
00805 
00806 
00807         
00808 }