1073 lines
30 KiB
C++
1073 lines
30 KiB
C++
// **********************************************************************************
|
|
// * "a3x_int.c" *
|
|
// * Written by : Sébastien Rubens *
|
|
// * Tabulations : 4 char *
|
|
// **********************************************************************************
|
|
#define A3X_INT_C
|
|
|
|
|
|
|
|
// **********************************************************************************
|
|
// Included files
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
|
|
#include "specif\\a3x_pref.h"
|
|
|
|
|
|
#include "a3x_glob.h"
|
|
#include "a3x_int.h"
|
|
|
|
// temp
|
|
SEB_xReal xEps;
|
|
SEB_xReal xOneSubEps;
|
|
|
|
// **********************************************************************************
|
|
signed long slNext[3]= { eQY, eQZ, eQX };
|
|
signed short ax_SinTab[lNbSin];
|
|
tdxQuater4 a4_xQuater1= { xZero, xZero, xZero, xOne };
|
|
|
|
|
|
|
|
// **********************************************************************************
|
|
// Precalculations
|
|
|
|
void fn_v_InitSinTab( void )
|
|
{ register signed long slCnt1;
|
|
|
|
for ( slCnt1=0 ; slCnt1<lNbSinDiv2 ; slCnt1++ )
|
|
ax_SinTab[slCnt1]= (short) ((xCompactValue * sin((xTwoPi * slCnt1) / lNbSin )) + 0.5);
|
|
for ( slCnt1=lNbSinDiv2 ; slCnt1<lNbSin ; slCnt1++ )
|
|
ax_SinTab[slCnt1]= (short) ((xCompactValue * sin((xTwoPi * slCnt1) / lNbSin )) - 0.5);
|
|
}
|
|
|
|
|
|
|
|
// **********************************************************************************
|
|
// Matrices tests
|
|
|
|
// Test if a 3*3 matrix is diagonal
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3a3_xMtx = matrix to test
|
|
// Output :
|
|
// unsigned short = 0 if the matrix is diagonal, 1 else
|
|
unsigned short fn_uw_IsDiagonale( tdxMatrix33 _a3a3_xMtx )
|
|
{
|
|
if ( (fabs(_a3a3_xMtx[0][1]) < xEps ) && (fabs(_a3a3_xMtx[0][2]) < xEps ) &&
|
|
(fabs(_a3a3_xMtx[1][0]) < xEps ) && (fabs(_a3a3_xMtx[1][2]) < xEps ) &&
|
|
(fabs(_a3a3_xMtx[2][0]) < xEps ) && (fabs(_a3a3_xMtx[2][1]) < xEps ) )
|
|
return 0;
|
|
return 1;
|
|
}
|
|
|
|
|
|
|
|
// Test if a 3*3 matrix is orthogonal
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3a3_xMtx = matrix to test
|
|
// Output :
|
|
// unsigned short = 0 if the matrix is orthogonal, 1 else
|
|
unsigned short fn_uw_IsOrthogonale( tdxMatrix33 _a3a3_xMtx )
|
|
{ register SEB_xReal xScal;
|
|
|
|
// Dot product of axis1*axis2
|
|
xScal= ( _a3a3_xMtx[0][0] * _a3a3_xMtx[1][0] +
|
|
_a3a3_xMtx[0][1] * _a3a3_xMtx[1][1] +
|
|
_a3a3_xMtx[0][2] * _a3a3_xMtx[1][2] );
|
|
if ( fabs(xScal) > xEps )
|
|
return 1;
|
|
|
|
// Dot product of axis1*axis3
|
|
xScal= ( _a3a3_xMtx[0][0] * _a3a3_xMtx[2][0] +
|
|
_a3a3_xMtx[0][1] * _a3a3_xMtx[2][1] +
|
|
_a3a3_xMtx[0][2] * _a3a3_xMtx[2][2] );
|
|
if ( fabs(xScal) > xEps )
|
|
return 1;
|
|
|
|
// Dot product of axis2*axis3
|
|
xScal= ( _a3a3_xMtx[1][0] * _a3a3_xMtx[2][0] +
|
|
_a3a3_xMtx[1][1] * _a3a3_xMtx[2][1] +
|
|
_a3a3_xMtx[1][2] * _a3a3_xMtx[2][2] );
|
|
if ( fabs(xScal) > xEps )
|
|
return 1;
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// Test if a 3*3 matrix is a rotation matrix
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3a3_xMtx = matrix to test
|
|
// Output :
|
|
// unsigned short = 0 if the matrix is a rotation matrix, 1 else
|
|
unsigned short fn_uw_IsOrthonormee( tdxMatrix33 _a3a3_xMtx )
|
|
{ register SEB_xReal xSum;
|
|
register signed long slCnt1, slCnt2;
|
|
|
|
// If the matrix is not orthogonal, we don't need to do more
|
|
if ( fn_uw_IsOrthogonale(_a3a3_xMtx) == 1 )
|
|
return 1;
|
|
|
|
// Length of each axis must be equal to 1
|
|
for ( slCnt1=0 ; slCnt1<3 ; slCnt1++ )
|
|
{ xSum= xZero;
|
|
|
|
for ( slCnt2=0 ; slCnt2<3 ; slCnt2++ )
|
|
xSum+= _a3a3_xMtx[slCnt1][slCnt2] * _a3a3_xMtx[slCnt1][slCnt2];
|
|
|
|
if ( fabs(xSum - xOne) > xEps )
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// Test if a quaternion is a unit quaternion
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a4_xQuat = quaternion to test
|
|
// Output :
|
|
// unsigned short = 0 if it is a unit quaternion, 1 else
|
|
unsigned short fn_uw_IsUnitQuat( tdxQuater4 _a4_xQuat )
|
|
{ register signed long slCnt1;
|
|
register SEB_xReal xSum;
|
|
|
|
xSum= xZero;
|
|
for ( slCnt1=0 ; slCnt1<4 ; slCnt1++ )
|
|
xSum+= _a4_xQuat[slCnt1] * _a4_xQuat[slCnt1];
|
|
|
|
if ( fabs(sqrt(xSum) - xOne) > xEps )
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// Test if two 3*3 matrices are the same
|
|
// (Differences between each element is lower than epsilon)
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3a3_xMtx1 = first quaternion to test
|
|
// _a3a3_xMtx2 = second quaternion to test
|
|
// Output :
|
|
// unsigned short = 0 if the matrices are the same, 1 else
|
|
unsigned short fn_uw_AreSameMatrix( tdxMatrix33 _a3a3_xMtx1,
|
|
tdxMatrix33 _a3a3_xMtx2 )
|
|
{ register signed long slCnt1, slCnt2;
|
|
|
|
for ( slCnt1=0 ; slCnt1<3 ; slCnt1++ )
|
|
for ( slCnt2=0 ; slCnt2<3 ; slCnt2++ )
|
|
if ( fabs( _a3a3_xMtx1[slCnt1][slCnt2] - _a3a3_xMtx2[slCnt1][slCnt2] ) > xEps )
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// Test if two quaternions are the same
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a4_xQuat1 = first quaternion to test
|
|
// _a4_xQuat2 = second quaternion to test
|
|
// Output :
|
|
// unsigned short = 0 if the quaternions are the same, 1 else
|
|
unsigned short fn_uw_AreSameQuat( tdxQuater4 _a4_xQuat1,
|
|
tdxQuater4 _a4_xQuat2 )
|
|
{
|
|
if ( ( _a4_xQuat1[0] * _a4_xQuat2[0] + _a4_xQuat1[1] * _a4_xQuat2[1] +
|
|
_a4_xQuat1[2] * _a4_xQuat2[2] + _a4_xQuat1[3] * _a4_xQuat2[3] ) < xOneSubEps )
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// Test if two vectors are the same
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3_xVect1 = first vector to test
|
|
// _a3_xVect2 = second vector to test
|
|
// Output :
|
|
// unsigned short = 0 if the vectors are the same, 1 else
|
|
unsigned short fn_uw_AreSameVect( tdxVector3 _a3_xVect1,
|
|
tdxVector3 _a3_xVect2 )
|
|
{ if ( fabs( _a3_xVect1[0] - _a3_xVect2[0] ) > xEps )
|
|
return 1;
|
|
if ( fabs( _a3_xVect1[1] - _a3_xVect2[1] ) > xEps )
|
|
return 1;
|
|
if ( fabs( _a3_xVect1[2] - _a3_xVect2[2] ) > xEps )
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// Compare if two quaternions
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a4_xQuat1 = first matrix to test
|
|
// _a4_xQuat2 = second matrix to test
|
|
// Output :
|
|
// unsigned short = 0 if the matrices are the same, 1 else
|
|
unsigned short fn_uw_CompQuat( tdxQuater4 _a4_xQuat1,
|
|
tdxQuater4 _a4_xQuat2 )
|
|
{ if ( _a4_xQuat1[0] != _a4_xQuat2[0] )
|
|
return 1;
|
|
if ( _a4_xQuat1[1] != _a4_xQuat2[1] )
|
|
return 1;
|
|
if ( _a4_xQuat1[2] != _a4_xQuat2[2] )
|
|
return 1;
|
|
if ( _a4_xQuat1[3] != _a4_xQuat2[3] )
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// Compare if two vectors
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3_xVect1 = first vector to test
|
|
// _a3_xVect2 = second vector to test
|
|
// Output :
|
|
// unsigned short = 0 if the matrices are the same, 1 else
|
|
unsigned short fn_uw_CompVect( tdxVector3 _a3_xVect1,
|
|
tdxVector3 _a3_xVect2 )
|
|
{ if ( _a3_xVect1[0] != _a3_xVect2[0] )
|
|
return 1;
|
|
if ( _a3_xVect1[1] != _a3_xVect2[1] )
|
|
return 1;
|
|
if ( _a3_xVect1[2] != _a3_xVect2[2] )
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// **********************************************************************************
|
|
// Miscellaneous matrices operations
|
|
|
|
// Set a 3*3 matrix to identity
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3a3_xMatrix = matrix to set
|
|
// Output :
|
|
// void
|
|
void fn_v_SetIdMatrix( tdxMatrix33 _a3a3_xMatrix )
|
|
{ register SEB_xReal r_xZero= xZero;
|
|
register SEB_xReal r_xOne= xOne;
|
|
|
|
_a3a3_xMatrix[0][0]= r_xOne;
|
|
_a3a3_xMatrix[0][1]= r_xZero;
|
|
_a3a3_xMatrix[0][2]= r_xZero;
|
|
|
|
_a3a3_xMatrix[1][0]= r_xZero;
|
|
_a3a3_xMatrix[1][1]= r_xOne;
|
|
_a3a3_xMatrix[1][2]= r_xZero;
|
|
|
|
_a3a3_xMatrix[2][0]= r_xZero;
|
|
_a3a3_xMatrix[2][1]= r_xZero;
|
|
_a3a3_xMatrix[2][2]= r_xOne;
|
|
}
|
|
|
|
|
|
|
|
// Reset a vector
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3_xVector = vector to reset
|
|
// Output :
|
|
// void
|
|
void fn_v_SetZeroVector( tdxVector3 _a3_xVector )
|
|
{ register SEB_xReal r_xZero= xZero;
|
|
|
|
_a3_xVector[0]= r_xZero;
|
|
_a3_xVector[1]= r_xZero;
|
|
_a3_xVector[2]= r_xZero;
|
|
}
|
|
|
|
|
|
|
|
// Set a vector to (1,1,1)
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3_xVector = vector to set
|
|
// Output :
|
|
// void
|
|
void fn_v_SetOneVector( tdxVector3 _a3_xVector )
|
|
{ register SEB_xReal r_xOne= xOne;
|
|
|
|
_a3_xVector[0]= r_xOne;
|
|
_a3_xVector[1]= r_xOne;
|
|
_a3_xVector[2]= r_xOne;
|
|
}
|
|
|
|
|
|
|
|
// Set a quaternion to identity
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a4_xQuater = quaternion to set
|
|
// Output :
|
|
// void
|
|
void fn_v_SetIdQuater( tdxQuater4 _a4_xQuater )
|
|
{ register SEB_xReal r_xZero= xZero;
|
|
register SEB_xReal r_xOne= xOne;
|
|
|
|
_a4_xQuater[0]= r_xZero;
|
|
_a4_xQuater[1]= r_xZero;
|
|
_a4_xQuater[2]= r_xZero;
|
|
_a4_xQuater[3]= r_xOne;
|
|
}
|
|
|
|
|
|
|
|
// Expand a 2.14 fixed point quaternion to floating point quaternion
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a4_xQuatD = expanded quaternion
|
|
// _a4_xQuatS = quaternion to expand
|
|
// Output :
|
|
// void
|
|
void fn_v_ExpandQuat( tdxQuater4 _a4_xQuatD,
|
|
tdxSShortQuater4 _a4_xQuatS )
|
|
{ register SEB_xReal r_xExpandValue = xExpandValue;
|
|
_a4_xQuatD[0]= _a4_xQuatS[0] * r_xExpandValue;
|
|
_a4_xQuatD[1]= _a4_xQuatS[1] * r_xExpandValue;
|
|
_a4_xQuatD[2]= _a4_xQuatS[2] * r_xExpandValue;
|
|
_a4_xQuatD[3]= _a4_xQuatS[3] * r_xExpandValue;
|
|
}
|
|
|
|
|
|
|
|
// Copy a quaternion
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a4_xQuatD = destination quaternion
|
|
// _a4_xQuatS = source quaternion
|
|
// Output :
|
|
// void
|
|
void fn_v_CopyQuat( tdxQuater4 _a4_xQuatD,
|
|
tdxQuater4 _a4_xQuatS )
|
|
{
|
|
_a4_xQuatD[0]= _a4_xQuatS[0];
|
|
_a4_xQuatD[1]= _a4_xQuatS[1];
|
|
_a4_xQuatD[2]= _a4_xQuatS[2];
|
|
_a4_xQuatD[3]= _a4_xQuatS[3];
|
|
}
|
|
|
|
|
|
|
|
// Copy a vector
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3_xVectD = destination vector
|
|
// _a3_xVectS = source vector
|
|
// Output :
|
|
// void
|
|
void fn_v_CopyVect( tdxVector3 _a3_xVectD,
|
|
tdxVector3 _a3_xVectS )
|
|
{
|
|
_a3_xVectD[0]= _a3_xVectS[0];
|
|
_a3_xVectD[1]= _a3_xVectS[1];
|
|
_a3_xVectD[2]= _a3_xVectS[2];
|
|
}
|
|
|
|
|
|
|
|
// Copy a 3*3 matrix
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3a3_xMtxD = destination 3*3 matrix
|
|
// _a3a3_xMtxS = source 3*3 matrix
|
|
// Output :
|
|
// void
|
|
void fn_v_CopyMatrix( tdxMatrix33 _a3a3_xMtxD,
|
|
tdxMatrix33 _a3a3_xMtxS )
|
|
{
|
|
_a3a3_xMtxD[0][0]= _a3a3_xMtxS[0][0];
|
|
_a3a3_xMtxD[0][1]= _a3a3_xMtxS[0][1];
|
|
_a3a3_xMtxD[0][2]= _a3a3_xMtxS[0][2];
|
|
|
|
_a3a3_xMtxD[1][0]= _a3a3_xMtxS[1][0];
|
|
_a3a3_xMtxD[1][1]= _a3a3_xMtxS[1][1];
|
|
_a3a3_xMtxD[1][2]= _a3a3_xMtxS[1][2];
|
|
|
|
_a3a3_xMtxD[2][0]= _a3a3_xMtxS[2][0];
|
|
_a3a3_xMtxD[2][1]= _a3a3_xMtxS[2][1];
|
|
_a3a3_xMtxD[2][2]= _a3a3_xMtxS[2][2];
|
|
}
|
|
|
|
|
|
|
|
// Move diagonal values of a 3*3 matrix to a vector (3 dimensions)
|
|
// By : Sébastien Rubens (tuesday, 97/10/07)
|
|
// Input :
|
|
// _a3_xVec <- vector which contain the diagonal
|
|
// _a3a3_xMtx = matrix
|
|
// Output :
|
|
// void
|
|
void fn_v_MatrixToVecDia( tdxVector3 _a3_xVec,
|
|
tdxMatrix33 _a3a3_xMtx )
|
|
{
|
|
_a3_xVec[0]= _a3a3_xMtx[0][0];
|
|
_a3_xVec[1]= _a3a3_xMtx[1][1];
|
|
_a3_xVec[2]= _a3a3_xMtx[2][2];
|
|
}
|
|
|
|
|
|
|
|
// 3*3 matrix transposition
|
|
// (for rotation matrix, it's same as invert matrix calculation)
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3a3_xMtxD <- destination matrix, equal to transpose( source matrix )
|
|
// _a3a3_xMtxS = source matrix
|
|
// Output :
|
|
// void
|
|
void fn_v_TranMatrix( tdxMatrix33 _a3a3_xMtxD,
|
|
tdxMatrix33 _a3a3_xMtxS )
|
|
{
|
|
_a3a3_xMtxD[0][0]= _a3a3_xMtxS[0][0];
|
|
_a3a3_xMtxD[0][1]= _a3a3_xMtxS[1][0];
|
|
_a3a3_xMtxD[0][2]= _a3a3_xMtxS[2][0];
|
|
|
|
_a3a3_xMtxD[1][0]= _a3a3_xMtxS[0][1];
|
|
_a3a3_xMtxD[1][1]= _a3a3_xMtxS[1][1];
|
|
_a3a3_xMtxD[1][2]= _a3a3_xMtxS[2][1];
|
|
|
|
_a3a3_xMtxD[2][0]= _a3a3_xMtxS[0][2];
|
|
_a3a3_xMtxD[2][1]= _a3a3_xMtxS[1][2];
|
|
_a3a3_xMtxD[2][2]= _a3a3_xMtxS[2][2];
|
|
}
|
|
|
|
|
|
|
|
// 3*3 matrix transposition
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3a3_xMtx <- destination matrix, equal to transpose of itself
|
|
// Output :
|
|
// void
|
|
void fn_v_TranMatrixOne( tdxMatrix33 _a3a3_xMtx )
|
|
{ register SEB_xReal r_xTmp;
|
|
|
|
r_xTmp= _a3a3_xMtx[1][0]; _a3a3_xMtx[1][0]= _a3a3_xMtx[0][1]; _a3a3_xMtx[0][1]= r_xTmp;
|
|
r_xTmp= _a3a3_xMtx[2][0]; _a3a3_xMtx[2][0]= _a3a3_xMtx[0][2]; _a3a3_xMtx[0][2]= r_xTmp;
|
|
r_xTmp= _a3a3_xMtx[1][2]; _a3a3_xMtx[1][2]= _a3a3_xMtx[2][1]; _a3a3_xMtx[2][1]= r_xTmp;
|
|
}
|
|
|
|
|
|
|
|
// Angle between two vectors
|
|
// By : Sébastien Rubens (monday, 97/10/13)
|
|
// Input :
|
|
// _a4_xVec1 = first vector
|
|
// _a4_xVec2 = second vector
|
|
// Output :
|
|
// unsigned short = angle beetween the two quaternions (0 to 65535)
|
|
unsigned short fn_uw_AngleBetweenVect( tdxVector3 _a3_xVec1,
|
|
tdxVector3 _a3_xVec2 )
|
|
{ register SEB_xReal xAngle;
|
|
|
|
xAngle= (SEB_xReal) acos( _a3_xVec1[0] * _a3_xVec2[0] +
|
|
_a3_xVec1[1] * _a3_xVec2[1] +
|
|
_a3_xVec1[2] * _a3_xVec2[2] );
|
|
return ( ((unsigned short) (xAngle * (((SEB_xReal)65536.0) / xTwoPi))) >> lDecSin );
|
|
}
|
|
|
|
|
|
|
|
// Angle between two unit quaternions
|
|
// By : Sébastien Rubens (monday, 97/10/13)
|
|
// Input :
|
|
// _a4_xQuat1 = first quaternion
|
|
// _a4_xQuat2 = second quaternion
|
|
// Output :
|
|
// unsigned short = angle beetween the two quaternions (0 to 65535)
|
|
unsigned short fn_uw_AngleBetweenQuat( tdxQuater4 _a4_xQuat1,
|
|
tdxQuater4 _a4_xQuat2 )
|
|
{ register SEB_xReal xAngle;
|
|
|
|
xAngle= (SEB_xReal) acos( _a4_xQuat1[eQX] * _a4_xQuat2[eQX] +
|
|
_a4_xQuat1[eQY] * _a4_xQuat2[eQY] +
|
|
_a4_xQuat1[eQZ] * _a4_xQuat2[eQZ] +
|
|
_a4_xQuat1[eQW] * _a4_xQuat2[eQW] );
|
|
return ( ((unsigned short) (xAngle * (((SEB_xReal)65536.0) / xTwoPi))) >> lDecSin );
|
|
}
|
|
|
|
|
|
|
|
// Length of a vector
|
|
// By : Sébastien Rubens (monday, 97/10/13)
|
|
// Input :
|
|
// _a3_xVec = vector
|
|
// Output :
|
|
// SEB_xReal = length of the vector
|
|
SEB_xReal fn_x_VectorLength( tdxVector3 _a3_xVect )
|
|
{
|
|
return ( (SEB_xReal) sqrt( _a3_xVect[0] * _a3_xVect[0] +
|
|
_a3_xVect[1] * _a3_xVect[1] +
|
|
_a3_xVect[2] * _a3_xVect[2] ) );
|
|
}
|
|
|
|
|
|
|
|
// Normalize a vector
|
|
// By : Sébastien Rubens (monday, 97/10/13)
|
|
// Input :
|
|
// _a3_xVect = vector
|
|
// Output :
|
|
// SEB_xReal = length of the vector
|
|
void fn_v_NormalizeVector( tdxVector3 _a3_xVect )
|
|
{ register SEB_xReal xLenght;
|
|
|
|
xLenght= fn_x_VectorLength( _a3_xVect );
|
|
if ( fabs(xLenght) < xEps )
|
|
{ register SEB_xReal r_xZero= xZero;
|
|
_a3_xVect[0]= r_xZero;
|
|
_a3_xVect[1]= r_xZero;
|
|
_a3_xVect[2]= r_xZero;
|
|
}
|
|
else
|
|
{ register SEB_xReal xOneDivLen;
|
|
xOneDivLen= xOne / xLenght;
|
|
_a3_xVect[0]*= xOneDivLen;
|
|
_a3_xVect[1]*= xOneDivLen;
|
|
_a3_xVect[2]*= xOneDivLen;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// Normalize a vector and return lenght
|
|
// By : Sébastien Rubens (monday, 97/10/13)
|
|
// Input :
|
|
// _a3_xVect = vector
|
|
// Output :
|
|
// SEB_xReal = length of the vector
|
|
SEB_xReal fn_x_NormalizeVector( tdxVector3 _a3_xVect )
|
|
{ register SEB_xReal xLenght;
|
|
|
|
xLenght= fn_x_VectorLength( _a3_xVect );
|
|
if ( fabs(xLenght) < xEps )
|
|
{ register SEB_xReal r_xZero= xZero;
|
|
_a3_xVect[0]= r_xZero;
|
|
_a3_xVect[1]= r_xZero;
|
|
_a3_xVect[2]= r_xZero;
|
|
}
|
|
else
|
|
{ register SEB_xReal xOneDivLen;
|
|
xOneDivLen= xOne / xLenght;
|
|
_a3_xVect[0]*= xOneDivLen;
|
|
_a3_xVect[1]*= xOneDivLen;
|
|
_a3_xVect[2]*= xOneDivLen;
|
|
}
|
|
|
|
return (xLenght);
|
|
}
|
|
|
|
|
|
|
|
// Length of a quaternion
|
|
// By : Sébastien Rubens (monday, 97/10/13)
|
|
// Input :
|
|
// _a4_xQuat = quaternion
|
|
// Output :
|
|
// SEB_xReal = length of the quaternion
|
|
SEB_xReal fn_x_QuaterLength( tdxQuater4 _a4_xQuat )
|
|
{
|
|
return ( (SEB_xReal) sqrt( _a4_xQuat[0] * _a4_xQuat[0] +
|
|
_a4_xQuat[1] * _a4_xQuat[1] +
|
|
_a4_xQuat[2] * _a4_xQuat[2] +
|
|
_a4_xQuat[3] * _a4_xQuat[3] ) );
|
|
}
|
|
|
|
|
|
|
|
// Normalize a quaternion
|
|
// By : Sébastien Rubens (monday, 97/10/13)
|
|
// Input :
|
|
// _a4_xQuat = quaternion
|
|
// Output :
|
|
// SEB_xReal = length of the quaternion
|
|
SEB_xReal fn_x_NormalizeQuater( tdxQuater4 _a4_xQuat )
|
|
{ register SEB_xReal xLenght;
|
|
|
|
xLenght= fn_x_QuaterLength( _a4_xQuat );
|
|
if ( fabs(xLenght) < xEps )
|
|
{ register SEB_xReal r_xZero= xZero;
|
|
_a4_xQuat[0]= r_xZero;
|
|
_a4_xQuat[1]= r_xZero;
|
|
_a4_xQuat[2]= r_xZero;
|
|
_a4_xQuat[3]= r_xZero;
|
|
}
|
|
else
|
|
{ register SEB_xReal xOneDivLen;
|
|
xOneDivLen= xOne / xLenght;
|
|
_a4_xQuat[0]*= xLenght;
|
|
_a4_xQuat[1]*= xLenght;
|
|
_a4_xQuat[2]*= xLenght;
|
|
_a4_xQuat[3]*= xLenght;
|
|
}
|
|
|
|
return (xLenght);
|
|
}
|
|
|
|
|
|
|
|
// **********************************************************************************
|
|
// Matrices computations
|
|
|
|
// Multiply a 3*3 matrix by a a componants vector
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3_xVecD <- result vector, equal to _xMtxS*_xVecS
|
|
// _a3a3_xMtxS = source matrix (on the left)
|
|
// _a3_xVecS = source vector (on the right)
|
|
// Output :
|
|
// void
|
|
void fn_v_MatrixByVector( tdxVector3 _a3_xVecD,
|
|
tdxMatrix33 _a3a3_xMtxS,
|
|
tdxVector3 _a3_xVecS )
|
|
{
|
|
_a3_xVecD[0]= _a3a3_xMtxS[0][0] * _a3_xVecS[0] +
|
|
_a3a3_xMtxS[1][0] * _a3_xVecS[1] +
|
|
_a3a3_xMtxS[2][0] * _a3_xVecS[2];
|
|
_a3_xVecD[1]= _a3a3_xMtxS[0][1] * _a3_xVecS[0] +
|
|
_a3a3_xMtxS[1][1] * _a3_xVecS[1] +
|
|
_a3a3_xMtxS[2][1] * _a3_xVecS[2];
|
|
_a3_xVecD[2]= _a3a3_xMtxS[0][2] * _a3_xVecS[0] +
|
|
_a3a3_xMtxS[1][2] * _a3_xVecS[1] +
|
|
_a3a3_xMtxS[2][2] * _a3_xVecS[2];
|
|
}
|
|
|
|
|
|
|
|
// Multiply a "scale vector" (i.e. diagonal matrix) by a 3*3 matrix
|
|
// Permit to improve this matrixial calculation
|
|
// By : Sébastien Rubens (monday, 97/10/06)
|
|
// Input :
|
|
// _a3a3_xMtxD <- destination matrix
|
|
// _a3_xVecS = source vector (i.e. coefficients of diagonal matrix)
|
|
// _a3a3_xMtxS = source matrix
|
|
// Output :
|
|
// void
|
|
void fn_v_DiaByMatrix( tdxMatrix33 _a3a3_xMtxD,
|
|
tdxVector3 _a3_xVecS,
|
|
tdxMatrix33 _a3a3_xMtxS )
|
|
{ register SEB_xReal r_xTmp;
|
|
|
|
r_xTmp= _a3_xVecS[0];
|
|
_a3a3_xMtxD[0][0]= r_xTmp * _a3a3_xMtxS[0][0];
|
|
_a3a3_xMtxD[1][0]= r_xTmp * _a3a3_xMtxS[1][0];
|
|
_a3a3_xMtxD[2][0]= r_xTmp * _a3a3_xMtxS[2][0];
|
|
|
|
r_xTmp= _a3_xVecS[1];
|
|
_a3a3_xMtxD[0][1]= r_xTmp * _a3a3_xMtxS[0][1];
|
|
_a3a3_xMtxD[1][1]= r_xTmp * _a3a3_xMtxS[1][1];
|
|
_a3a3_xMtxD[2][1]= r_xTmp * _a3a3_xMtxS[2][1];
|
|
|
|
r_xTmp= _a3_xVecS[2];
|
|
_a3a3_xMtxD[0][2]= r_xTmp * _a3a3_xMtxS[0][2];
|
|
_a3a3_xMtxD[1][2]= r_xTmp * _a3a3_xMtxS[1][2];
|
|
_a3a3_xMtxD[2][2]= r_xTmp * _a3a3_xMtxS[2][2];
|
|
}
|
|
|
|
|
|
|
|
// Multiply a 3*3 matrix by a "scale vector" (i.e. diagonal matrix)
|
|
// Permit to improve this matrixial calculation
|
|
// By : Sébastien Rubens (monday, 97/10/06)
|
|
// Input :
|
|
// _a3a3_xMtxD <- destination matrix
|
|
// _a3a3_xMtxS = source matrix
|
|
// _a3_xVecS = source vector (i.e. coefficients of diagonal matrix)
|
|
// Output :
|
|
// void
|
|
void fn_v_MatrixByDia( tdxMatrix33 _a3a3_xMtxD,
|
|
tdxMatrix33 _a3a3_xMtxS,
|
|
tdxVector3 _a3_xVecS )
|
|
{ register SEB_xReal r_xTmp;
|
|
|
|
r_xTmp= _a3_xVecS[0];
|
|
_a3a3_xMtxD[0][0]= _a3a3_xMtxS[0][0] * r_xTmp;
|
|
_a3a3_xMtxD[0][1]= _a3a3_xMtxS[0][1] * r_xTmp;
|
|
_a3a3_xMtxD[0][2]= _a3a3_xMtxS[0][2] * r_xTmp;
|
|
|
|
r_xTmp= _a3_xVecS[1];
|
|
_a3a3_xMtxD[1][0]= _a3a3_xMtxS[1][0] * r_xTmp;
|
|
_a3a3_xMtxD[1][1]= _a3a3_xMtxS[1][1] * r_xTmp;
|
|
_a3a3_xMtxD[1][2]= _a3a3_xMtxS[1][2] * r_xTmp;
|
|
|
|
r_xTmp= _a3_xVecS[2];
|
|
_a3a3_xMtxD[2][0]= _a3a3_xMtxS[2][0] * r_xTmp;
|
|
_a3a3_xMtxD[2][1]= _a3a3_xMtxS[2][1] * r_xTmp;
|
|
_a3a3_xMtxD[2][2]= _a3a3_xMtxS[2][2] * r_xTmp;
|
|
}
|
|
|
|
|
|
|
|
// Multiply two 3*3 matrices
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3a3_xMtxR <- result of _xMtx1*_xMtx2
|
|
// _a3a3_xMtx1 = first matrix (on the left)
|
|
// _a3a3_xMtx2 = second matrix (ont the right)
|
|
// Output :
|
|
// void
|
|
void fn_v_MultMatrix( tdxMatrix33 _a3a3_xMtxR,
|
|
tdxMatrix33 _a3a3_xMtx1,
|
|
tdxMatrix33 _a3a3_xMtx2 )
|
|
{
|
|
_a3a3_xMtxR[0][0]= _a3a3_xMtx1[0][0] * _a3a3_xMtx2[0][0] +
|
|
_a3a3_xMtx1[1][0] * _a3a3_xMtx2[0][1] +
|
|
_a3a3_xMtx1[2][0] * _a3a3_xMtx2[0][2];
|
|
_a3a3_xMtxR[0][1]= _a3a3_xMtx1[0][1] * _a3a3_xMtx2[0][0] +
|
|
_a3a3_xMtx1[1][1] * _a3a3_xMtx2[0][1] +
|
|
_a3a3_xMtx1[2][1] * _a3a3_xMtx2[0][2];
|
|
_a3a3_xMtxR[0][2]= _a3a3_xMtx1[0][2] * _a3a3_xMtx2[0][0] +
|
|
_a3a3_xMtx1[1][2] * _a3a3_xMtx2[0][1] +
|
|
_a3a3_xMtx1[2][2] * _a3a3_xMtx2[0][2];
|
|
|
|
_a3a3_xMtxR[1][0]= _a3a3_xMtx1[0][0] * _a3a3_xMtx2[1][0] +
|
|
_a3a3_xMtx1[1][0] * _a3a3_xMtx2[1][1] +
|
|
_a3a3_xMtx1[2][0] * _a3a3_xMtx2[1][2];
|
|
_a3a3_xMtxR[1][1]= _a3a3_xMtx1[0][1] * _a3a3_xMtx2[1][0] +
|
|
_a3a3_xMtx1[1][1] * _a3a3_xMtx2[1][1] +
|
|
_a3a3_xMtx1[2][1] * _a3a3_xMtx2[1][2];
|
|
_a3a3_xMtxR[1][2]= _a3a3_xMtx1[0][2] * _a3a3_xMtx2[1][0] +
|
|
_a3a3_xMtx1[1][2] * _a3a3_xMtx2[1][1] +
|
|
_a3a3_xMtx1[2][2] * _a3a3_xMtx2[1][2];
|
|
|
|
_a3a3_xMtxR[2][0]= _a3a3_xMtx1[0][0] * _a3a3_xMtx2[2][0] +
|
|
_a3a3_xMtx1[1][0] * _a3a3_xMtx2[2][1] +
|
|
_a3a3_xMtx1[2][0] * _a3a3_xMtx2[2][2];
|
|
_a3a3_xMtxR[2][1]= _a3a3_xMtx1[0][1] * _a3a3_xMtx2[2][0] +
|
|
_a3a3_xMtx1[1][1] * _a3a3_xMtx2[2][1] +
|
|
_a3a3_xMtx1[2][1] * _a3a3_xMtx2[2][2];
|
|
_a3a3_xMtxR[2][2]= _a3a3_xMtx1[0][2] * _a3a3_xMtx2[2][0] +
|
|
_a3a3_xMtx1[1][2] * _a3a3_xMtx2[2][1] +
|
|
_a3a3_xMtx1[2][2] * _a3a3_xMtx2[2][2];
|
|
}
|
|
|
|
|
|
|
|
// Perform [inverse of rotation matrix]*[rotation matrix]*[diagonal matrix]
|
|
// Permit to improve this very particuliar matrixial calculation
|
|
// By : Sébastien Rubens (thursday 97/10/09)
|
|
// Input :
|
|
// _a3a3_xMtxD <- destination matrix
|
|
// _a3a3_xMtxS = source matrix
|
|
// _a3_xVecS = source vector (i.e. coefficients of diagonal matrix)
|
|
// Output :
|
|
// void
|
|
void fn_v_InvRotDiaRot( tdxMatrix33 _a3a3_xMtxD,
|
|
tdxMatrix33 _a3a3_xMtxS,
|
|
tdxVector3 _a3_xVecS )
|
|
{ static tdxMatrix33 a3a3_xTmpMtx;
|
|
|
|
// Calculate ((j,0,0),(0,k,0),(0,0,l)) * ((a,b,c),(d,e,f),(g,h,i)) = M
|
|
fn_v_DiaByMatrix( a3a3_xTmpMtx, _a3_xVecS, _a3a3_xMtxS );
|
|
|
|
// Calculate ((a,d,g),(b,e,h),(c,f,i)) * M
|
|
_a3a3_xMtxD[0][0]= _a3a3_xMtxS[0][0] * a3a3_xTmpMtx[0][0] +
|
|
_a3a3_xMtxS[0][1] * a3a3_xTmpMtx[0][1] +
|
|
_a3a3_xMtxS[0][2] * a3a3_xTmpMtx[0][2];
|
|
_a3a3_xMtxD[0][1]= _a3a3_xMtxS[1][0] * a3a3_xTmpMtx[0][0] +
|
|
_a3a3_xMtxS[1][1] * a3a3_xTmpMtx[0][1] +
|
|
_a3a3_xMtxS[1][2] * a3a3_xTmpMtx[0][2];
|
|
_a3a3_xMtxD[0][2]= _a3a3_xMtxS[2][0] * a3a3_xTmpMtx[0][0] +
|
|
_a3a3_xMtxS[2][1] * a3a3_xTmpMtx[0][1] +
|
|
_a3a3_xMtxS[2][2] * a3a3_xTmpMtx[0][2];
|
|
|
|
_a3a3_xMtxD[1][0]= _a3a3_xMtxD[0][1];
|
|
_a3a3_xMtxD[1][1]= _a3a3_xMtxS[1][0] * a3a3_xTmpMtx[1][0] +
|
|
_a3a3_xMtxS[1][1] * a3a3_xTmpMtx[1][1] +
|
|
_a3a3_xMtxS[1][2] * a3a3_xTmpMtx[1][2];
|
|
_a3a3_xMtxD[1][2]= _a3a3_xMtxS[2][0] * a3a3_xTmpMtx[1][0] +
|
|
_a3a3_xMtxS[2][1] * a3a3_xTmpMtx[1][1] +
|
|
_a3a3_xMtxS[2][2] * a3a3_xTmpMtx[1][2];
|
|
|
|
_a3a3_xMtxD[2][0]= _a3a3_xMtxD[0][2];
|
|
_a3a3_xMtxD[2][1]= _a3a3_xMtxD[1][2];
|
|
// _a3a3_xMtxD[2][2]= a3a3_xTmpMtx[0][2] * a3a3_xTmpMtx[0][2] +
|
|
// a3a3_xTmpMtx[1][2] * a3a3_xTmpMtx[1][2] +
|
|
// a3a3_xTmpMtx[2][2] * a3a3_xTmpMtx[2][2];
|
|
_a3a3_xMtxD[2][2]= _a3_xVecS[0] + _a3_xVecS[1] + _a3_xVecS[2] -
|
|
_a3a3_xMtxD[0][0] - _a3a3_xMtxD[1][1];
|
|
}
|
|
|
|
|
|
|
|
// **********************************************************************************
|
|
// Matrices / Quaternions conversions
|
|
|
|
// Quaternion -> 3*3 Matrix conversion
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a3a3_xMtx <- matrix
|
|
// _a4_xQuat = quaternion
|
|
// Output :
|
|
// void
|
|
// Docs :
|
|
// Advanced Animation and Rendering Techniques
|
|
// by Alan Watt & Mark Watt (ISBN : 0-201-54412-1)
|
|
void fn_v_QuatToMatrix( tdxMatrix33 _a3a3_xMtx,
|
|
tdxQuater4 _a4_xQuat )
|
|
{ register SEB_xReal r_xOne;
|
|
register SEB_xReal xXS, xYS, xZS;
|
|
static SEB_xReal xWX, xWY, xWZ;
|
|
static SEB_xReal xXX, xXY, xXZ;
|
|
static SEB_xReal xYY, xYZ, xZZ;
|
|
|
|
/* SEB_xReal xS;
|
|
|
|
xS= 2.0 / ( _a4_xQuat[eQX] * _a4_xQuat[eQX] +
|
|
_a4_xQuat[eQY] * _a4_xQuat[eQY] +
|
|
_a4_xQuat[eQZ] * _a4_xQuat[eQZ] +
|
|
_a4_xQuat[eQW] * _a4_xQuat[eQW] );
|
|
// If we have a unit quaternion, xS= 2.0
|
|
xXS= _a4_xQuat[eQX] * xS; xYS= _a4_xQuat[eQY] * xS; xZS= _a4_xQuat[eQZ] * xS;*/
|
|
|
|
|
|
xXS= _a4_xQuat[eQX]; xXS+= xXS;
|
|
xYS= _a4_xQuat[eQY]; xYS+= xYS;
|
|
xZS= _a4_xQuat[eQZ]; xZS+= xZS;
|
|
|
|
xWX= _a4_xQuat[eQW] * xXS; xWY= _a4_xQuat[eQW] * xYS; xWZ= _a4_xQuat[eQW] * xZS;
|
|
xXX= _a4_xQuat[eQX] * xXS; xXY= _a4_xQuat[eQX] * xYS; xXZ= _a4_xQuat[eQX] * xZS;
|
|
xYY= _a4_xQuat[eQY] * xYS; xYZ= _a4_xQuat[eQY] * xZS; xZZ= _a4_xQuat[eQZ] * xZS;
|
|
|
|
r_xOne= xOne;
|
|
_a3a3_xMtx[0][0]= r_xOne - (xYY + xZZ);
|
|
_a3a3_xMtx[1][0]= xXY + xWZ;
|
|
_a3a3_xMtx[2][0]= xXZ - xWY;
|
|
|
|
_a3a3_xMtx[0][1]= xXY - xWZ;
|
|
_a3a3_xMtx[1][1]= r_xOne - (xXX + xZZ);
|
|
_a3a3_xMtx[2][1]= xYZ + xWX;
|
|
|
|
_a3a3_xMtx[0][2]= xXZ + xWY;
|
|
_a3a3_xMtx[1][2]= xYZ - xWX;
|
|
_a3a3_xMtx[2][2]= r_xOne - (xXX + xYY);
|
|
}
|
|
|
|
|
|
|
|
// 3*3 Matrix -> Quaternion conversion
|
|
// By : Sébastien Rubens (thursday, 97/10/03)
|
|
// Input :
|
|
// _a4_xQuat <- quaternion
|
|
// _a3a3_xMtx = matrix
|
|
// Output :
|
|
// void
|
|
// Docs :
|
|
// Advanced Animation and Rendering Techniques
|
|
// by Alan Watt & Mark Watt (ISBN : 0-201-54412-1)
|
|
void fn_v_MatrixToQuat( tdxQuater4 _a4_xQuat,
|
|
tdxMatrix33 _a3a3_xMtx )
|
|
{
|
|
register SEB_xReal xTrace, xS;
|
|
register signed long slNum1, slNum2, slNum3;
|
|
|
|
xTrace= _a3a3_xMtx[0][0] + _a3a3_xMtx[1][1] + _a3a3_xMtx[2][2];
|
|
|
|
if ( xTrace > xZero )
|
|
{ xS= (SEB_xReal) sqrt(xTrace + xOne);
|
|
_a4_xQuat[eQW]= xS * xHalfOne;
|
|
|
|
xS= xHalfOne / xS;
|
|
_a4_xQuat[eQX]= ( _a3a3_xMtx[2][1] - _a3a3_xMtx[1][2] ) * xS;
|
|
_a4_xQuat[eQY]= ( _a3a3_xMtx[0][2] - _a3a3_xMtx[2][0] ) * xS;
|
|
_a4_xQuat[eQZ]= ( _a3a3_xMtx[1][0] - _a3a3_xMtx[0][1] ) * xS;
|
|
}
|
|
else
|
|
{ slNum1= eQX;
|
|
if ( _a3a3_xMtx[eQY][eQY] > _a3a3_xMtx[eQX][eQX] )
|
|
slNum1= eQY;
|
|
if ( _a3a3_xMtx[eQZ][eQZ] > _a3a3_xMtx[slNum1][slNum1] )
|
|
slNum1= eQZ;
|
|
slNum2= slNext[slNum1];
|
|
slNum3= slNext[slNum2];
|
|
xS= (SEB_xReal) sqrt( xOne + _a3a3_xMtx[slNum1][slNum1] - _a3a3_xMtx[slNum2][slNum2]
|
|
- _a3a3_xMtx[slNum3][slNum3] );
|
|
|
|
_a4_xQuat[slNum1]= xS * xHalfOne;
|
|
xS= xHalfOne / xS;
|
|
_a4_xQuat[slNum2]= (_a3a3_xMtx[slNum1][slNum2] + _a3a3_xMtx[slNum2][slNum1]) * xS;
|
|
_a4_xQuat[slNum3]= (_a3a3_xMtx[slNum1][slNum3] + _a3a3_xMtx[slNum3][slNum1]) * xS;
|
|
_a4_xQuat[eQW] = (_a3a3_xMtx[slNum3][slNum2] - _a3a3_xMtx[slNum2][slNum3]) * xS;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// **********************************************************************************
|
|
// Interpolations computations
|
|
|
|
// Linear interpolation of a vector vecteur (3 componants)
|
|
// By : Sébastien Rubens (monday, 97/10/06)
|
|
// Input :
|
|
// _a3_xVecD <- destination vector
|
|
// _a3_xVecS1 = first vector
|
|
// _a3_xVecS2 = second vector
|
|
// _xT = interpolation parameter
|
|
// Output :
|
|
// void
|
|
void fn_v_InterpolVect( tdxVector3 _a3_xVecD,
|
|
tdxVector3 _a3_xVecS1,
|
|
tdxVector3 _a3_xVecS2,
|
|
register SEB_xReal _xT )
|
|
{
|
|
_a3_xVecD[0]= (_a3_xVecS2[0] - _a3_xVecS1[0]) * _xT + _a3_xVecS1[0];
|
|
_a3_xVecD[1]= (_a3_xVecS2[1] - _a3_xVecS1[1]) * _xT + _a3_xVecS1[1];
|
|
_a3_xVecD[2]= (_a3_xVecS2[2] - _a3_xVecS1[2]) * _xT + _a3_xVecS1[2];
|
|
}
|
|
|
|
|
|
|
|
// "Geodesic" interpolation of a quaternion (4 dimensions)
|
|
// Code compatible with A3d !!!
|
|
// By : Sébastien Rubens (monday, 97/10/06)
|
|
// Input :
|
|
// _a4_xQuatD <- destination quaternion
|
|
// _a4_xQuatS1 = first quaternion
|
|
// _a4_xQuatS2 = second quaternion
|
|
// _xT = interpolation parameter
|
|
// Output :
|
|
// void
|
|
// Docs :
|
|
// Advanced Animation and Rendering Techniques
|
|
// by Alan Watt & Mark Watt (ISBN : 0-201-54412-1)
|
|
void fn_v_InterpolQuat( tdxQuater4 _a4_xQuatD,
|
|
tdxQuater4 _a4_xQuatS1,
|
|
tdxQuater4 _a4_xQuatS2,
|
|
register SEB_xReal _xT )
|
|
{ SEB_xReal xOmega, xCosOmega, xSinOmega, xTbyOmega;
|
|
SEB_xReal xCoef1, xCoef2;
|
|
static tdxQuater4 a4_xQuatTmp;
|
|
|
|
fn_v_CopyQuat( a4_xQuatTmp, _a4_xQuatS2 );
|
|
|
|
xCosOmega= ( _a4_xQuatS1[eQX] * a4_xQuatTmp[eQX] +
|
|
_a4_xQuatS1[eQY] * a4_xQuatTmp[eQY] +
|
|
_a4_xQuatS1[eQZ] * a4_xQuatTmp[eQZ] +
|
|
_a4_xQuatS1[eQW] * a4_xQuatTmp[eQW] );
|
|
|
|
if ( xCosOmega < xZero )
|
|
{ xCosOmega= -xCosOmega;
|
|
a4_xQuatTmp[0]= -a4_xQuatTmp[0];
|
|
a4_xQuatTmp[1]= -a4_xQuatTmp[1];
|
|
a4_xQuatTmp[2]= -a4_xQuatTmp[2];
|
|
a4_xQuatTmp[3]= -a4_xQuatTmp[3];
|
|
}
|
|
|
|
if ( xCosOmega > -xOneSubEps )
|
|
{ if ( xOneSubEps > xCosOmega )
|
|
{ xOmega= (SEB_xReal) acos( xCosOmega );
|
|
xSinOmega= xOne / (SEB_xReal) sin( xOmega );
|
|
xTbyOmega= _xT * xOmega;
|
|
xCoef1= (SEB_xReal) sin( xOmega - xTbyOmega ) * xSinOmega;
|
|
xCoef2= (SEB_xReal) sin( xTbyOmega ) * xSinOmega;
|
|
}
|
|
else
|
|
{ xCoef1= xOne - _xT;
|
|
xCoef2= _xT;
|
|
}
|
|
_a4_xQuatD[eQX]= xCoef1 * _a4_xQuatS1[eQX] + xCoef2 * a4_xQuatTmp[eQX];
|
|
_a4_xQuatD[eQY]= xCoef1 * _a4_xQuatS1[eQY] + xCoef2 * a4_xQuatTmp[eQY];
|
|
_a4_xQuatD[eQZ]= xCoef1 * _a4_xQuatS1[eQZ] + xCoef2 * a4_xQuatTmp[eQZ];
|
|
_a4_xQuatD[eQW]= xCoef1 * _a4_xQuatS1[eQW] + xCoef2 * a4_xQuatTmp[eQW];
|
|
}
|
|
else
|
|
{ xTbyOmega= _xT * xHalfPi;
|
|
xCoef1= (SEB_xReal) sin( xHalfPi - xTbyOmega );
|
|
xCoef2= (SEB_xReal) sin( xTbyOmega );
|
|
_a4_xQuatD[eQX]= xCoef1 * _a4_xQuatS1[eQX] - xCoef2 * a4_xQuatTmp[eQY];
|
|
_a4_xQuatD[eQY]= xCoef1 * _a4_xQuatS1[eQY] + xCoef2 * a4_xQuatTmp[eQX];
|
|
_a4_xQuatD[eQZ]= xCoef1 * _a4_xQuatS1[eQZ] - xCoef2 * a4_xQuatTmp[eQW];
|
|
_a4_xQuatD[eQW]= a4_xQuatTmp[eQZ];
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// "Geodesic" interpolation of a quaternion (4 dimensions) "for Nintendo 64"
|
|
// Code compatible with A3d !!!
|
|
// By : Sébastien Rubens (monday, 97/10/06)
|
|
// Input :
|
|
// _a4_xQuatD <- destination quaternion
|
|
// _a4_xQuatS1 = first quaternion
|
|
// _a4_xQuatS2 = second quaternion
|
|
// _xT = interpolation parameter
|
|
// _lOmega = angle between the two quaternions (0 to 65535)
|
|
// Output :
|
|
// void
|
|
// Docs :
|
|
// Advanced Animation and Rendering Techniques
|
|
// by Alan Watt & Mark Watt (ISBN : 0-201-54412-1)
|
|
void fn_v_InterpolQuatWithOmega( tdxQuater4 _a4_xQuatD,
|
|
tdxQuater4 _a4_xQuatS1,
|
|
tdxQuater4 _a4_xQuatS2,
|
|
register SEB_xReal _xT,
|
|
register signed short _swOmega )
|
|
{ register SEB_xReal xSinOmega;
|
|
register SEB_xReal xCoef1, xCoef2;
|
|
register signed short swCosOmega, swTbyOmega;
|
|
static tdxQuater4 a4_xQuatTmp;
|
|
|
|
fn_v_CopyQuat( a4_xQuatTmp, _a4_xQuatS2 );
|
|
|
|
swCosOmega= ax_SinTab[ (_swOmega + lNbSinDiv4) & lMaskNbSin ];
|
|
|
|
if ( swCosOmega < 0 )
|
|
{ swCosOmega= -swCosOmega;
|
|
_swOmega= (lNbSinDiv2 - _swOmega);
|
|
a4_xQuatTmp[0]= -a4_xQuatTmp[0];
|
|
a4_xQuatTmp[1]= -a4_xQuatTmp[1];
|
|
a4_xQuatTmp[2]= -a4_xQuatTmp[2];
|
|
a4_xQuatTmp[3]= -a4_xQuatTmp[3];
|
|
}
|
|
|
|
if ( swCosOmega > -lOneSubEps )
|
|
{ //if ( lOneSubEps > swCosOmega )
|
|
swCosOmega= ax_SinTab[ _swOmega & lMaskNbSin ];
|
|
if ( swCosOmega != 0 )
|
|
{ xSinOmega= xOne / swCosOmega;
|
|
swTbyOmega= (signed short) (_xT * _swOmega); // -_swOmega < slTbyOmega < _swOmega
|
|
xCoef1= ax_SinTab[ (_swOmega - swTbyOmega) & lMaskNbSin ] * xSinOmega;
|
|
xCoef2= ax_SinTab[ swTbyOmega & lMaskNbSin ] * xSinOmega;
|
|
}
|
|
else
|
|
{ xCoef1= xOne - _xT;
|
|
xCoef2= _xT;
|
|
}
|
|
_a4_xQuatD[eQX]= xCoef1 * _a4_xQuatS1[eQX] + xCoef2 * a4_xQuatTmp[eQX];
|
|
_a4_xQuatD[eQY]= xCoef1 * _a4_xQuatS1[eQY] + xCoef2 * a4_xQuatTmp[eQY];
|
|
_a4_xQuatD[eQZ]= xCoef1 * _a4_xQuatS1[eQZ] + xCoef2 * a4_xQuatTmp[eQZ];
|
|
_a4_xQuatD[eQW]= xCoef1 * _a4_xQuatS1[eQW] + xCoef2 * a4_xQuatTmp[eQW];
|
|
}
|
|
else
|
|
{ swTbyOmega= (signed short) (_xT * lNbSinDiv4);
|
|
xCoef1= ax_SinTab[ (lNbSinDiv4 - swTbyOmega) & lMaskNbSin ];
|
|
xCoef2= ax_SinTab[ swTbyOmega & lMaskNbSin ];
|
|
_a4_xQuatD[eQX]= xCoef1 * _a4_xQuatS1[eQX] - xCoef2 * a4_xQuatTmp[eQY];
|
|
_a4_xQuatD[eQY]= xCoef1 * _a4_xQuatS1[eQY] + xCoef2 * a4_xQuatTmp[eQX];
|
|
_a4_xQuatD[eQZ]= xCoef1 * _a4_xQuatS1[eQZ] - xCoef2 * a4_xQuatTmp[eQW];
|
|
_a4_xQuatD[eQW]= a4_xQuatTmp[eQZ];
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// **********************************************************************************
|
|
#undef A3X_INT_C
|