// ********************************************************************************** // * "a3x_int.c" * // * Written by : Sébastien Rubens * // * Tabulations : 4 char * // ********************************************************************************** #define A3X_INT_C // ********************************************************************************** // Included files #include #include #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 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