Main Page | Alphabetical List | Compound List | File List | Compound Members | File Members

quatern.c File Reference

#include <assert.h>
#include <math.h>
#include "basetype.h"
#include "quatern.h"

Go to the source code of this file.

Defines

#define geQuaternion_Assert   if (geQuaternion_MaximalAssertionMode) assert
#define UNIT_TOLERANCE   0.001
#define QZERO_TOLERANCE   0.00001
#define TRACE_QZERO_TOLERANCE   0.1
#define AA_QZERO_TOLERANCE   0.0001
#define EPSILON   (0.00001)
#define ZERO_EPSILON   (0.0001f)

Functions

void GENESISCC geQuaternion_SetMaximalAssertionMode (geBoolean Enable)
geBoolean GENESISCC geQuaternion_IsValid (const geQuaternion *Q)
void GENESISCC geQuaternion_Set (geQuaternion *Q, geFloat W, geFloat X, geFloat Y, geFloat Z)
void GENESISCC geQuaternion_SetVec3d (geQuaternion *Q, geFloat W, const geVec3d *V)
void GENESISCC geQuaternion_Get (const geQuaternion *Q, geFloat *W, geFloat *X, geFloat *Y, geFloat *Z)
GENESISAPI void GENESISCC geQuaternion_SetFromAxisAngle (geQuaternion *Q, const geVec3d *Axis, geFloat Theta)
geBoolean GENESISCC geQuaternion_GetAxisAngle (const geQuaternion *Q, geVec3d *Axis, geFloat *Theta)
void GENESISCC geQuaternion_GetVec3d (const geQuaternion *Q, geFloat *W, geVec3d *V)
void GENESISCC geQuaternion_FromMatrix (const geXForm3d *M, geQuaternion *Q)
GENESISAPI void GENESISCC geQuaternion_ToMatrix (const geQuaternion *Q, geXForm3d *M)
void GENESISCC geQuaternion_Slerp (const geQuaternion *Q0, const geQuaternion *Q1, geFloat T, geQuaternion *QT)
void GENESISCC geQuaternion_SlerpNotShortest (const geQuaternion *Q0, const geQuaternion *Q1, geFloat T, geQuaternion *QT)
void GENESISCC geQuaternion_Multiply (const geQuaternion *Q1, const geQuaternion *Q2, geQuaternion *Q)
void GENESISCC geQuaternion_Rotate (const geQuaternion *Q, const geVec3d *V, geVec3d *VRotated)
geBoolean GENESISCC geQuaternion_IsUnit (const geQuaternion *Q)
geFloat GENESISCC geQuaternion_Magnitude (const geQuaternion *Q)
GENESISAPI geFloat GENESISCC geQuaternion_Normalize (geQuaternion *Q)
GENESISAPI void GENESISCC geQuaternion_Copy (const geQuaternion *QSrc, geQuaternion *QDst)
void GENESISCC geQuaternion_Inverse (const geQuaternion *Q, geQuaternion *QInv)
void GENESISCC geQuaternion_Add (const geQuaternion *Q1, const geQuaternion *Q2, geQuaternion *QSum)
void GENESISCC geQuaternion_Subtract (const geQuaternion *Q1, const geQuaternion *Q2, geQuaternion *QSum)
void GENESISCC geQuaternion_Ln (const geQuaternion *Q, geQuaternion *LnQ)
void GENESISCC geQuaternion_Exp (const geQuaternion *Q, geQuaternion *ExpQ)
void GENESISCC geQuaternion_Scale (const geQuaternion *Q, geFloat Scale, geQuaternion *QScaled)
void GENESISCC geQuaternion_SetNoRotation (geQuaternion *Q)
geBoolean GENESISCC geQuaternion_Compare (geQuaternion *Q1, geQuaternion *Q2, geFloat Tolerance)

Variables

 geQuaternion_MaximalAssertionMode = GE_TRUE
int32 geQuaternion_XFormTable [] = {1768710981,560296816}

Define Documentation

#define AA_QZERO_TOLERANCE   0.0001
 

Definition at line 53 of file quatern.c.

Referenced by geQuaternion_SetFromAxisAngle().

#define EPSILON   (0.00001)
 

Definition at line 386 of file quatern.c.

Referenced by geQuaternion_Slerp(), and geQuaternion_SlerpNotShortest().

#define geQuaternion_Assert   if (geQuaternion_MaximalAssertionMode) assert
 

Definition at line 30 of file quatern.c.

Referenced by geQuaternion_FromMatrix(), geQuaternion_GetAxisAngle(), geQuaternion_Ln(), geQuaternion_Multiply(), geQuaternion_Rotate(), geQuaternion_SetFromAxisAngle(), geQuaternion_Slerp(), geQuaternion_SlerpNotShortest(), and geQuaternion_ToMatrix().

#define QZERO_TOLERANCE   0.00001
 

Definition at line 45 of file quatern.c.

Referenced by geQuaternion_GetAxisAngle(), and geQuaternion_Normalize().

#define TRACE_QZERO_TOLERANCE   0.1
 

Definition at line 49 of file quatern.c.

Referenced by geQuaternion_FromMatrix().

#define UNIT_TOLERANCE   0.001
 

Definition at line 41 of file quatern.c.

Referenced by geQuaternion_IsUnit().

#define ZERO_EPSILON   (0.0001f)
 

Definition at line 677 of file quatern.c.

Referenced by geQuaternion_Exp(), and geQuaternion_Ln().

Function Documentation

void GENESISCC geQuaternion_Add (  const geQuaternion *  Q1,
const geQuaternion *  Q2,
geQuaternion *  QSum
) 
 

Definition at line 646 of file quatern.c.

References GE_FALSE, GENESISCC, geQuaternion_IsValid(), NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by geQKFrame_ChooseBestQuat(), and geQKFrame_QuadrangleCorner(). 

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 }

geBoolean GENESISCC geQuaternion_Compare (  geQuaternion *  Q1,
geQuaternion *  Q2,
geFloat  Tolerance
) 
 

Definition at line 783 of file quatern.c.

References GE_FALSE, GE_TRUE, geBoolean, GENESISCC, geQuaternion_IsValid(), geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z. 

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 }

GENESISAPI void GENESISCC geQuaternion_Copy (  const geQuaternion *  QSrc,
geQuaternion *  QDst
) 
 

Definition at line 625 of file quatern.c.

References GE_FALSE, GENESISAPI, GENESISCC, geQuaternion_IsValid(), and NULL.

Referenced by gePhysicsObject_GetOrientation(), gePhysicsObject_SetOrientation(), and geQKFrame_SquadRecompute(). 

00627 {
00628         assert( geQuaternion_IsValid(QSrc) != GE_FALSE );
00629         assert( QDst != NULL );
00630         *QDst = *QSrc;
00631 }

void GENESISCC geQuaternion_Exp (  const geQuaternion *  Q,
geQuaternion *  ExpQ
) 
 

Definition at line 722 of file quatern.c.

References GE_FALSE, geFloat, GENESISCC, geQuaternion_IsValid(), NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, geQuaternion::Z, and ZERO_EPSILON.

Referenced by geQKFrame_QuadrangleCorner(). 

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 }

void GENESISCC geQuaternion_FromMatrix (  const geXForm3d *  M,
geQuaternion *  Q
) 
 

Definition at line 182 of file quatern.c.

References A, geXForm3d::AX, geXForm3d::AY, geXForm3d::AZ, geXForm3d::BX, geXForm3d::BY, geXForm3d::BZ, geXForm3d::CX, geXForm3d::CY, geXForm3d::CZ, GE_TRUE, geFloat, GENESISCC, geQuaternion_Assert, geXForm3d_IsOrthonormal(), NULL, s, TRACE_QZERO_TOLERANCE, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by geMotion_GetTransform(), gePath_InsertKeyframe(), gePath_ModifyKeyframe(), gePose_BlendMotion(), gePose_Clear(), gePose_InitializeJoint(), gePose_JointRelativeToParent(), gePose_SetJointAttachment(), gePose_SetJointLocalTransform(), gePose_SetMotion(), and gePose_SetMotionForABone(). 

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 }

void GENESISCC geQuaternion_Get (  const geQuaternion *  Q,
geFloat *  W,
geFloat *  X,
geFloat *  Y,
geFloat *  Z
) 
 

Definition at line 95 of file quatern.c.

References GE_FALSE, GENESISCC, geQuaternion_IsValid(), NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, Z, and geQuaternion::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 }

geBoolean GENESISCC geQuaternion_GetAxisAngle (  const geQuaternion *  Q,
geVec3d *  Axis,
geFloat *  Theta
) 
 

Definition at line 135 of file quatern.c.

References GE_FALSE, GE_TRUE, geBoolean, geFloat, GENESISCC, geQuaternion_Assert, geQuaternion_IsUnit(), geVec3d_IsValid(), NULL, QZERO_TOLERANCE, geQuaternion::W, geVec3d::X, geQuaternion::X, geVec3d::Y, geQuaternion::Y, geVec3d::Z, and geQuaternion::Z.

Referenced by geQKFrame_PathIsHinged(), geQKFrame_WriteToBinaryFile(), and geQKFrame_WriteToFile(). 

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 }

void GENESISCC geQuaternion_GetVec3d (  const geQuaternion *  Q,
geFloat *  W,
geVec3d *  V
) 
 

Definition at line 165 of file quatern.c.

References GE_FALSE, GENESISCC, geQuaternion_IsValid(), NULL, V, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by geQuaternion_Rotate(). 

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 }

void GENESISCC geQuaternion_Inverse (  const geQuaternion *  Q,
geQuaternion *  QInv
) 
 

Definition at line 633 of file quatern.c.

References GE_FALSE, GENESISCC, geQuaternion_IsValid(), NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by geQuaternion_Rotate(). 

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 }

geBoolean GENESISCC geQuaternion_IsUnit (  const geQuaternion *  Q  ) 
 

Definition at line 578 of file quatern.c.

References GE_FALSE, GE_TRUE, geBoolean, geFloat, GENESISCC, NULL, UNIT_TOLERANCE, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by geQKFrame_QuadrangleCorner(), geQKFrame_SquadInterpolation(), geQuaternion_GetAxisAngle(), geQuaternion_Ln(), geQuaternion_Rotate(), geQuaternion_SetFromAxisAngle(), geQuaternion_Slerp(), geQuaternion_SlerpNotShortest(), and geQuaternion_ToMatrix(). 

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 }

geBoolean GENESISCC geQuaternion_IsValid (  const geQuaternion *  Q  ) 
 

Definition at line 56 of file quatern.c.

References GE_FALSE, GE_TRUE, geBoolean, GENESISCC, NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by geQuaternion_Add(), geQuaternion_Compare(), geQuaternion_Copy(), geQuaternion_Exp(), geQuaternion_Get(), geQuaternion_GetVec3d(), geQuaternion_Inverse(), geQuaternion_Ln(), geQuaternion_Magnitude(), geQuaternion_Multiply(), geQuaternion_Normalize(), geQuaternion_Rotate(), geQuaternion_Scale(), geQuaternion_Set(), geQuaternion_Subtract(), and geQuaternion_ToMatrix(). 

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 }

void GENESISCC geQuaternion_Ln (  const geQuaternion *  Q,
geQuaternion *  LnQ
) 
 

Definition at line 680 of file quatern.c.

References GE_FALSE, GE_TRUE, geFloat, GENESISCC, geQuaternion_Assert, geQuaternion_IsUnit(), geQuaternion_IsValid(), NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, geQuaternion::Z, and ZERO_EPSILON.

Referenced by geQKFrame_QuadrangleCorner(). 

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 }

geFloat GENESISCC geQuaternion_Magnitude (  const geQuaternion *  Q  ) 
 

Definition at line 592 of file quatern.c.

References GE_FALSE, geFloat, GENESISCC, geQuaternion_IsValid(), geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z. 

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 }

void GENESISCC geQuaternion_Multiply (  const geQuaternion *  Q1,
const geQuaternion *  Q2,
geQuaternion *  Q
) 
 

Definition at line 522 of file quatern.c.

References GE_FALSE, GENESISCC, geQuaternion_Assert, geQuaternion_IsValid(), NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by gePose_JointRelativeToParent(), gePose_UpdateRelativeToParent(), geQKFrame_ChooseBestQuat(), geQKFrame_QuadrangleCorner(), and geQuaternion_Rotate(). 

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 }

GENESISAPI geFloat GENESISCC geQuaternion_Normalize (  geQuaternion *  Q  ) 
 

Definition at line 601 of file quatern.c.

References GE_FALSE, geFloat, GENESISAPI, GENESISCC, geQuaternion_IsValid(), QZERO_TOLERANCE, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by gePath_InsertKeyframe(), gePath_ModifyKeyframe(), gePhysicsObject_Integrate(), and geQKFrame_LinearInterpolation(). 

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 }

void GENESISCC geQuaternion_Rotate (  const geQuaternion *  Q,
const geVec3d *  V,
geVec3d *  VRotated
) 
 

Definition at line 553 of file quatern.c.

References GE_FALSE, GE_TRUE, geFloat, GENESISCC, geQuaternion_Assert, geQuaternion_GetVec3d(), geQuaternion_Inverse(), geQuaternion_IsUnit(), geQuaternion_IsValid(), geQuaternion_Multiply(), geQuaternion_SetVec3d(), geVec3d_IsValid(), NULL, and V. 

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 }

void GENESISCC geQuaternion_Scale (  const geQuaternion *  Q,
geFloat  Scale,
geQuaternion *  QScaled
) 
 

Definition at line 750 of file quatern.c.

References GE_FALSE, GENESISCC, geQuaternion_IsValid(), NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by geQKFrame_QuadrangleCorner(). 

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 }

void GENESISCC geQuaternion_Set (  geQuaternion *  Q,
geFloat  W,
geFloat  X,
geFloat  Y,
geFloat  Z
) 
 

Definition at line 71 of file quatern.c.

References GE_FALSE, GENESISCC, geQuaternion_IsValid(), NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, geQuaternion::Z, and 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 }

GENESISAPI void GENESISCC geQuaternion_SetFromAxisAngle (  geQuaternion *  Q,
const geVec3d *  Axis,
geFloat  Theta
) 
 

Definition at line 115 of file quatern.c.

References AA_QZERO_TOLERANCE, GE_FALSE, GE_TRUE, geFloat, GENESISAPI, GENESISCC, geQuaternion_Assert, geQuaternion_IsUnit(), geVec3d_IsValid(), geVec3d_Length(), NULL, geQuaternion::W, geQuaternion::X, geVec3d::X, geQuaternion::Y, geVec3d::Y, geQuaternion::Z, and geVec3d::Z.

Referenced by FireRocket(), gePhysicsObject_Create(), geQKFrame_CreateFromBinaryFile(), geQKFrame_CreateFromFile(), and SetupSkyForScene(). 

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 }

void GENESISCC geQuaternion_SetMaximalAssertionMode (  geBoolean  Enable  ) 
 

Definition at line 32 of file quatern.c.

References GE_FALSE, GE_TRUE, GENESISCC, and geQuaternion_MaximalAssertionMode. 

00033         {
00034                 assert( (Enable == GE_TRUE) || (Enable == GE_FALSE) );
00035                 geQuaternion_MaximalAssertionMode = Enable;
00036         }

void GENESISCC geQuaternion_SetNoRotation (  geQuaternion *  Q  ) 
 

Definition at line 766 of file quatern.c.

References GENESISCC, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by geMotion_SampleChannels(), geMotion_SampleChannelsNamed(), gePath_SampleChannels(), gePose_Clear(), gePose_InitializeJoint(), gePose_UpdateRelativeToParent(), and geQKFrame_LinearInterpolation(). 

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 }

void GENESISCC geQuaternion_SetVec3d (  geQuaternion *  Q,
geFloat  W,
const geVec3d *  V
) 
 

Definition at line 83 of file quatern.c.

References GE_FALSE, GENESISCC, geVec3d_IsValid(), NULL, V, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by geQuaternion_Rotate(). 

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 }

void GENESISCC geQuaternion_Slerp (  const geQuaternion *  Q0,
const geQuaternion *  Q1,
geFloat  T,
geQuaternion *  QT
) 
 

Definition at line 389 of file quatern.c.

References EPSILON, GE_TRUE, geFloat, GENESISCC, geQuaternion_Assert, geQuaternion_IsUnit(), NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, geQuaternion::Z, and Z.

Referenced by geMotion_GetTransform(), geMotion_SampleChannels(), geMotion_SampleChannelsNamed(), and gePose_BlendMotion(). 

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 }

void GENESISCC geQuaternion_SlerpNotShortest (  const geQuaternion *  Q0,
const geQuaternion *  Q1,
geFloat  T,
geQuaternion *  QT
) 
 

Definition at line 451 of file quatern.c.

References EPSILON, GE_TRUE, geFloat, GENESISCC, geQuaternion_Assert, geQuaternion_IsUnit(), NULL, QUATERNION_PI, geQuaternion::W, geQuaternion::X, geQuaternion::Y, geQuaternion::Z, and Z.

Referenced by geQKFrame_SlerpInterpolation(), and geQKFrame_SquadInterpolation(). 

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 }

void GENESISCC geQuaternion_Subtract (  const geQuaternion *  Q1,
const geQuaternion *  Q2,
geQuaternion *  QSum
) 
 

Definition at line 661 of file quatern.c.

References GE_FALSE, GENESISCC, geQuaternion_IsValid(), NULL, geQuaternion::W, geQuaternion::X, geQuaternion::Y, and geQuaternion::Z.

Referenced by geQKFrame_ChooseBestQuat(). 

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 }

GENESISAPI void GENESISCC geQuaternion_ToMatrix (  const geQuaternion *  Q,
geXForm3d *  M
) 
 

Definition at line 336 of file quatern.c.

References geXForm3d::AX, geXForm3d::AY, geXForm3d::AZ, geXForm3d::BX, geXForm3d::BY, geXForm3d::BZ, geXForm3d::CX, geXForm3d::CY, geXForm3d::CZ, GE_FALSE, GE_TRUE, geFloat, GENESISAPI, GENESISCC, geQuaternion_Assert, geQuaternion_IsUnit(), geQuaternion_IsValid(), geXForm3d_IsOrthonormal(), NULL, geXForm3d::Translation, geQuaternion::W, geQuaternion::X, geVec3d::X, geQuaternion::Y, geVec3d::Y, geQuaternion::Z, and geVec3d::Z.

Referenced by FireRocket(), geMotion_GetTransform(), geMotion_Sample(), geMotion_SampleNamed(), gePath_GetKeyframe(), gePath_Sample(), gePhysicsObject_Integrate(), gePose_GetJointLocalTransform(), gePose_JointRelativeToParent(), gePose_UpdateRelativeToParent(), and SetupSkyForScene(). 

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 }

Variable Documentation

geQuaternion_MaximalAssertionMode = GE_TRUE [static]
 

Definition at line 29 of file quatern.c.

Referenced by geQuaternion_SetMaximalAssertionMode().

int32 geQuaternion_XFormTable[] = {1768710981,560296816} [static]
 

Definition at line 678 of file quatern.c.

Generated on Tue Sep 30 12:37:59 2003 for GTestAndEngine by doxygen 1.3.2