1663 lines
59 KiB
C
1663 lines
59 KiB
C
/* (c) Ubi R&D 1997*/
|
|
/* See Philippe Vimont & Alain Robin for any comment or question*/
|
|
|
|
#ifdef __cplusplus
|
|
extern "C"
|
|
{
|
|
#endif
|
|
|
|
/****************************************************************************/
|
|
/* INCLUDES */
|
|
/****************************************************************************/
|
|
#include "ACP_base.h"
|
|
#include "cpa_std.h"
|
|
|
|
#include "MTH.h"
|
|
|
|
#include "PosPubSt.h"
|
|
#include "PosProto.h"
|
|
|
|
#include "safe.h"
|
|
|
|
#define __DeclareGlobalVariableMmgPos_h__
|
|
#define __DeclareGlobalVariableErmPos_h__
|
|
#include "ErrPos.h"
|
|
#include "MmgPos.h"
|
|
#undef __DeclareGlobalVariableMmgPos_h__
|
|
#undef __DeclareGlobalVariableErmPos_h__
|
|
|
|
#include "GEO.h"
|
|
#ifdef _DEBUG /* {*/
|
|
|
|
#define POS_M_MustBeValid(handle) \
|
|
{ \
|
|
SAF_M_AssertWithMsg((((handle) != (POS_tdxHandleToPosition)POS_C_ulInvalidHandle)&&(handle != NULL))," Invalid handle ") \
|
|
if(((handle) == (POS_tdxHandleToPosition)POS_C_ulInvalidHandle)||(handle == NULL)) \
|
|
Erm_M_UpdateLastError(Pos, C_ucErmDefaultChannel, E_uwPosFatalError1, C_lErmNoDebugData, C_ucAllowStopForDebug, C_ucAllowStopForDebug, NULL); \
|
|
}
|
|
|
|
#define POS_M_MustBeModifiable(handle) \
|
|
{ \
|
|
SAF_M_AssertWithMsg((handle != POS_g_p_stIdentityMatrix)," Constant matrix can't be modified") \
|
|
if(handle == POS_g_p_stIdentityMatrix) \
|
|
Erm_M_UpdateLastError(Pos, C_ucErmDefaultChannel, E_uwPosFatalError1, C_lErmNoDebugData, C_ucAllowStopForDebug, C_ucAllowStopForDebug, NULL); \
|
|
}
|
|
|
|
#endif /* _DEBUG }*/
|
|
|
|
#define POS_C_xDeltaScale ((MTH_tdxReal)0.005) /* 0.5% of precision loss*/
|
|
#define POS_M_bIsDeltaNull(a) MTH_M_bLess(MTH_M_xAbs((a)),POS_C_xDeltaScale)
|
|
#define POS_M_xEpsilonEqual ((MTH_tdxReal)0.005)
|
|
#define POS_M_bIsVectorNull(p_stVect) \
|
|
(MTH_M_bIsNullWithEpsilon((p_stVect)->xX,POS_M_xEpsilonEqual) && \
|
|
MTH_M_bIsNullWithEpsilon((p_stVect)->xY,POS_M_xEpsilonEqual) && \
|
|
MTH_M_bIsNullWithEpsilon((p_stVect)->xZ,POS_M_xEpsilonEqual))
|
|
|
|
#define POS_M_bIsMatrixIdentity(p_stMatrix) \
|
|
(MTH_M_bEqualWithEpsilon((p_stMatrix)->stCol_0.xX,MTH_C_ONE,POS_M_xEpsilonEqual) && \
|
|
MTH_M_bIsNullWithEpsilon((p_stMatrix)->stCol_0.xY,POS_M_xEpsilonEqual) && \
|
|
MTH_M_bIsNullWithEpsilon((p_stMatrix)->stCol_0.xZ,POS_M_xEpsilonEqual) && \
|
|
MTH_M_bIsNullWithEpsilon((p_stMatrix)->stCol_1.xX,POS_M_xEpsilonEqual) && \
|
|
MTH_M_bEqualWithEpsilon((p_stMatrix)->stCol_1.xY,MTH_C_ONE,POS_M_xEpsilonEqual) && \
|
|
MTH_M_bIsNullWithEpsilon((p_stMatrix)->stCol_1.xZ,POS_M_xEpsilonEqual) && \
|
|
MTH_M_bIsNullWithEpsilon((p_stMatrix)->stCol_2.xX,POS_M_xEpsilonEqual) && \
|
|
MTH_M_bIsNullWithEpsilon((p_stMatrix)->stCol_2.xY,POS_M_xEpsilonEqual) && \
|
|
MTH_M_bEqualWithEpsilon((p_stMatrix)->stCol_2.xZ,MTH_C_ONE,POS_M_xEpsilonEqual))
|
|
|
|
/***************************/
|
|
|
|
unsigned long POS_ulIsMatrixDeltaNull(MTH3D_tdstMatrix* _p_stMatrix);
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
|
|
ACP_tdxBool POS_g_bAlreadyInitialized=FALSE;
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
|
|
POS_tdstCompletePosition *POS_g_p_stIdentityMatrix;
|
|
|
|
|
|
/*for internal use: use the constant identity matrix to reset other matrixes more easily*/
|
|
|
|
INLINE
|
|
void fn_vSetMatrixAsIdentity(MTH3D_tdstMatrix *_hMatrix)
|
|
{
|
|
*_hMatrix = POS_g_p_stIdentityMatrix->stRotationMatrix;
|
|
}
|
|
|
|
INLINE
|
|
void fn_vSetVectorAsNull(MTH3D_tdstVector *_hVector)
|
|
{
|
|
*_hVector = POS_g_p_stIdentityMatrix->stTranslationVector;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vInitPos
|
|
// Init Error & Memory
|
|
----------------------------------------------------------------------------//
|
|
// Methods : None
|
|
----------------------------------------------------------------------------//
|
|
// Input :
|
|
// Output :
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : May 97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vInitPos()
|
|
{
|
|
/* circumvent const qualifier*/
|
|
POS_tdstCompletePosition *_hConstPos;
|
|
|
|
GEO_M_CPAMalloc(POS_g_p_stIdentityMatrix,POS_tdstCompletePosition *,sizeof(struct POS_stCompletePosition),E_uwGEONotEnoughtMemory );
|
|
|
|
_hConstPos = (POS_tdstCompletePosition *) POS_g_p_stIdentityMatrix;
|
|
|
|
/* Fill the reference identity matrix*/
|
|
MTH3D_M_vNullVector(&(_hConstPos->stTranslationVector));
|
|
MTH3D_M_vSetIdentityMatrix(&(_hConstPos->stRotationMatrix));
|
|
MTH3D_M_vSetIdentityMatrix(&(_hConstPos->stTransformMatrix));
|
|
_hConstPos->ulType = POS_C_ulIdentityMatrixFlag;
|
|
}
|
|
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
|
|
void POS_fn_vFirstInit()
|
|
{
|
|
Erm_M_InitErrMsg(Pos);
|
|
Mmg_M_InitMmg(Pos);
|
|
Mmg_fn_cCheckAlignmentValidity();
|
|
}
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vInitPos
|
|
// Init Error & Memory
|
|
----------------------------------------------------------------------------//
|
|
// Methods : None
|
|
----------------------------------------------------------------------------//
|
|
// Input :
|
|
// Output :
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : May 97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
|
|
void POS_fn_vInitPosMemory(unsigned long _ulMemorySize)
|
|
{
|
|
if(!POS_g_bAlreadyInitialized)
|
|
{
|
|
#ifdef _POS_USE_POSMEM
|
|
/* MR2207*/
|
|
Mmg_M_InitBlock(Pos,E_ucPosBlock1,_ulMemorySize); /* must be into the scripts*/
|
|
g_ucPOSMMemMallocMode=E_ucPosBlock1;
|
|
Mmg_fn_cCheckAlignmentValidity();
|
|
#endif
|
|
POS_g_bAlreadyInitialized=TRUE;
|
|
}
|
|
}
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_hCreateMatrix
|
|
// To create an empty matrix handle (initialized to identity)
|
|
----------------------------------------------------------------------------//
|
|
// Methods : None
|
|
----------------------------------------------------------------------------//
|
|
// Input : None
|
|
// Output : Handle to the created matrix
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : 29 Jan 97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
/*oooo*/
|
|
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
|
|
POS_tdxHandleToPosition POS_fn_hCreateMatrix()
|
|
{
|
|
POS_tdxHandleToPosition hNewPosMatrix;
|
|
|
|
#ifdef _POS_USE_POSMEM
|
|
Mmg_M_SetModeAlloc4Ch(Pos, g_ucPOSMMemMallocMode, C_ucMmgDefaultChannel);
|
|
#endif
|
|
|
|
/* alloc matrix*/
|
|
MMG_fn_vAddMemoryInfo( MMG_C_lTypePosition , MMG_C_lSubTypePosition , 0 );
|
|
hNewPosMatrix=(POS_tdxHandleToPosition)Mmg_fn_p_vAlloc4Ch(sizeof(struct POS_stCompletePosition),C_ucMmgDefaultChannel );
|
|
|
|
/* set to Identity*/
|
|
#ifdef _DEBUG
|
|
if(hNewPosMatrix)
|
|
#endif
|
|
{
|
|
/*circumvent const qualifier*/
|
|
POS_fn_vCopyMatrix (hNewPosMatrix,(POS_tdstCompletePosition *) POS_g_p_stIdentityMatrix);
|
|
}
|
|
|
|
return hNewPosMatrix;
|
|
}
|
|
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vSetMatrixToIdentityConst
|
|
// To set given handle to the Identity Matrix (the matrix can't be modifier later)
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : a pointer to the handle to a position
|
|
// Output :
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vSetMatrixToIdentityConst(POS_tdxHandleToPosition *_p_hMatrix)
|
|
{
|
|
/*circumvent const qualifier*/
|
|
*_p_hMatrix = (POS_tdstCompletePosition *) POS_g_p_stIdentityMatrix;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vIsMatrixIdentityConst
|
|
//
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : a pointer to the handle to a position
|
|
// Output :
|
|
----------------------------------------------------------------------------*/
|
|
ACP_tdxBool POS_fn_vIsMatrixIdentityConst(POS_tdxHandleToPosition _hMatrix)
|
|
{
|
|
return (ACP_tdxBool)((_hMatrix == POS_g_p_stIdentityMatrix) ? TRUE : FALSE);
|
|
}
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
|
|
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_hGetMatrixIdentityConst
|
|
//
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : a pointer to the handle to a position
|
|
// Output :
|
|
----------------------------------------------------------------------------*/
|
|
POS_tdxHandleToPosition POS_fn_hGetMatrixIdentityConst()
|
|
{
|
|
/*circumvent const qualifier*/
|
|
return (POS_tdstCompletePosition *) POS_g_p_stIdentityMatrix;
|
|
}
|
|
|
|
/****************************************************************************/
|
|
/* INITIALISATION FUNCTIONS */
|
|
/****************************************************************************/
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vResetScaleMatrix
|
|
// reset the scale of a matrix
|
|
----------------------------------------------------------------------------//
|
|
// Methods : none
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : matrix to be initialized
|
|
// Output :
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : ? Author: ?
|
|
----------------------------------------------------------------------------//
|
|
// Modifications : Remove the transformation matrix computation
|
|
// Modification date : 07/02/97 Modification author : Alain Robin
|
|
------------------------------------------------------------------------------
|
|
// Modifications : With the new structure, we have just to copy the rotation
|
|
// matrix into the transform matrix.
|
|
// This function is used only for the complete positions.
|
|
// Modification date : 12/03/97 Modification author : Alain Robin
|
|
// Modification date : 03/07/98 Modification author : Marc Trabucato
|
|
----------------------------------------------------------------------------*/
|
|
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
|
|
void POS_fn_vResetScaleMatrix ( POS_tdxHandleToPosition _hMatrix )
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
POS_M_MustBeModifiable(_hMatrix);
|
|
#endif
|
|
|
|
/* We have just to copy the rotation matrix into the transformation matrix */
|
|
if ( _hMatrix->ulType > POS_C_ulRotationMatrixFlag )
|
|
{
|
|
MTH3D_M_vCopyMatrix(&(_hMatrix->stTransformMatrix), &(_hMatrix->stRotationMatrix));
|
|
_hMatrix->ulType = POS_C_ulRotationMatrixFlag;
|
|
}
|
|
}
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
|
|
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vSetIdentityMatrix
|
|
// SETTING A MATRIX TO IDENTITY
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : matrix to be initialized
|
|
// Output : None
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : ???/97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : 03/07/98 Modification author : Marc Trabucato
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vSetIdentityMatrix ( POS_tdxHandleToPosition _hMatrix )
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
POS_M_MustBeModifiable(_hMatrix);
|
|
|
|
MTH3D_M_vNullVector(&(_hMatrix->stTranslationVector));
|
|
MTH3D_M_vSetIdentityMatrix(&(_hMatrix->stRotationMatrix));
|
|
MTH3D_M_vSetIdentityMatrix(&(_hMatrix->stTransformMatrix));
|
|
_hMatrix->ulType = POS_C_ulIdentityMatrixFlag;
|
|
#else
|
|
/*circumvent const qualifier*/
|
|
POS_fn_vCopyMatrix(_hMatrix, (POS_tdstCompletePosition *) POS_g_p_stIdentityMatrix);
|
|
#endif
|
|
|
|
}
|
|
|
|
/****************************************************************************/
|
|
/* COMPLEX FUNCTIONS */
|
|
/****************************************************************************/
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vMulMatrixVertex
|
|
// MULTIPLYING A MATRIX WITH A VERTEX
|
|
----------------------------------------------------------------------------//
|
|
// Methods : none
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : matrix
|
|
// _p_stSource : source vertex
|
|
// Output : _p_stDest : pointer to the transformed vertex
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : ???/97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : 03/07/98 Modification author : Marc Trabucato
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vMulMatrixVertex(
|
|
MTH3D_tdstVector *_p_stDest,
|
|
POS_tdxHandleToPosition _hMatrix,
|
|
MTH3D_tdstVector *_p_stSource
|
|
)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
|
|
switch ( _hMatrix->ulType )
|
|
{
|
|
case POS_C_ulIdentityMatrixFlag:
|
|
if ( _p_stDest != _p_stSource )
|
|
{
|
|
MTH3D_M_vCopyVector(_p_stDest, _p_stSource);
|
|
}
|
|
break;
|
|
|
|
case POS_C_ulTranslationMatrixFlag: /*only the translation vector is safe, the rest might be trash*/
|
|
MTH3D_M_vAddVector(_p_stDest, &(_hMatrix->stTranslationVector), _p_stSource);
|
|
break;
|
|
|
|
case POS_C_ulRotationMatrixFlag: /*only the translation and rotation are safe, the transform data might be trash*/
|
|
MTH3D_M_vMulMatrixVector(_p_stDest, &(_hMatrix->stRotationMatrix), _p_stSource);
|
|
MTH3D_M_vAddVector(_p_stDest, &(_hMatrix->stTranslationVector), _p_stDest);
|
|
break;
|
|
|
|
case POS_C_ulCompleteMatrixFlag:
|
|
MTH3D_M_vMulMatrixVector(_p_stDest, &(_hMatrix->stTransformMatrix), _p_stSource);
|
|
MTH3D_M_vAddVector(_p_stDest, &(_hMatrix->stTranslationVector), _p_stDest);
|
|
break;
|
|
|
|
default:
|
|
/*we should never work with uninitialized matrixes!*/
|
|
* (long *) 0x00000000 = 5;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vMulMatrixVector
|
|
// MULTIPLYING A MATRIX WITH A VECTOR
|
|
----------------------------------------------------------------------------//
|
|
// Methods : none
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : matrix
|
|
// _p_stSource : source vertor
|
|
// Output : _p_stDest : pointer to the transformed vertor
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : ???/97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : 03/07/98 Modification author : Marc Trabucato
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vMulMatrixVector(
|
|
MTH3D_tdstVector *_p_stDest,
|
|
POS_tdxHandleToPosition _hMatrix,
|
|
MTH3D_tdstVector *_p_stSource
|
|
)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
|
|
M_CheckPointer(_p_stDest);
|
|
M_CheckPointer(_hMatrix);
|
|
M_CheckPointer(_p_stSource);
|
|
switch ( _hMatrix->ulType )
|
|
{
|
|
case POS_C_ulIdentityMatrixFlag:
|
|
case POS_C_ulTranslationMatrixFlag: /*only the translation vector is relevant*/
|
|
/*the vector remains unchanged*/
|
|
if ( _p_stDest != _p_stSource )
|
|
{
|
|
MTH3D_M_vCopyVector(_p_stDest, _p_stSource);
|
|
}
|
|
break;
|
|
|
|
case POS_C_ulRotationMatrixFlag: /*only the translation and rotation are relevant*/
|
|
MTH3D_M_vMulMatrixVector(_p_stDest, &(_hMatrix->stRotationMatrix), _p_stSource);
|
|
break;
|
|
|
|
case POS_C_ulCompleteMatrixFlag:
|
|
MTH3D_M_vMulMatrixVector(_p_stDest, &(_hMatrix->stTransformMatrix), _p_stSource);
|
|
break;
|
|
|
|
default:
|
|
/*we should never work with uninitialized matrixes!*/
|
|
* (long *) 0x00000000 = 5;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vRotateVector
|
|
// ROTATE A VECTOR
|
|
----------------------------------------------------------------------------//
|
|
// Methods : multiply a vector with the rotate component
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : matrix
|
|
// _p_stSource : source vertor
|
|
// Output : _p_stDest : pointer to the transformed vertor
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : 03/06/97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : 03/07/98 Modification author : Marc Trabucato
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vRotateVector(
|
|
MTH3D_tdstVector *_p_stDest,
|
|
POS_tdxHandleToPosition _hMatrix,
|
|
MTH3D_tdstVector *_p_stSource
|
|
)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
|
|
|
|
switch (_hMatrix->ulType )
|
|
{
|
|
case POS_C_ulIdentityMatrixFlag:
|
|
case POS_C_ulTranslationMatrixFlag:
|
|
/*the vector remains unchanged*/
|
|
if ( _p_stDest != _p_stSource )
|
|
{
|
|
MTH3D_M_vCopyVector(_p_stDest,_p_stSource);
|
|
}
|
|
break;
|
|
|
|
case POS_C_ulRotationMatrixFlag: /*we use only the rotation, whether the matrix is scaled or not*/
|
|
case POS_C_ulCompleteMatrixFlag:
|
|
MTH3D_M_vMulMatrixVector(_p_stDest, &(_hMatrix->stRotationMatrix), _p_stSource);
|
|
break;
|
|
|
|
default:
|
|
/*we should never work with uninitialized matrixes!*/
|
|
* (long *) 0x00000000 = 5;
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/******************** RESULT = INV(A)*B ***********************************/
|
|
|
|
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
|
|
void POS_fn_vMulInvMatrixMatrix(
|
|
POS_tdxHandleToPosition _hDest,
|
|
POS_tdxHandleToPosition _hA,
|
|
POS_tdxHandleToPosition _hB
|
|
)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hA);
|
|
POS_M_MustBeValid(_hB);
|
|
POS_M_MustBeValid(_hDest);
|
|
POS_M_MustBeModifiable(_hDest);
|
|
#endif
|
|
|
|
if ( _hA->ulType == POS_C_ulIdentityMatrixFlag )
|
|
{
|
|
if ( _hDest != _hB )
|
|
{
|
|
POS_fn_vCopyMatrix(_hDest, _hB);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
POS_tdstCompletePosition stInvA;
|
|
POS_fn_vInvertMatrix(&stInvA, _hA);
|
|
POS_fn_vMulMatrixMatrix(_hDest, &stInvA, _hB);
|
|
}
|
|
}
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
|
|
|
|
|
|
/******************** INVERTING AN ISOMETRIC MATRIX *********************/
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : INVERTING AN ISOMETRIC MATRIX
|
|
// Invert a position with an isometric scale -> the result matrix has no scale!
|
|
------------------------------------------------------------------------------
|
|
// Methods : See code
|
|
------------------------------------------------------------------------------
|
|
// Input : _hSource : position to invert
|
|
// Output : _hDest : destination
|
|
------------------------------------------------------------------------------
|
|
// Creation date : ??? Author: Alain Robin
|
|
------------------------------------------------------------------------------
|
|
// Modification date : May 98 Modification author : AR
|
|
// Modifications : Optimisations and correction of bugs
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vInvertIsoMatrix(POS_tdxHandleToPosition _hDest, POS_tdxHandleToPosition _hSource)
|
|
{
|
|
/* Validity tests*/
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hSource);
|
|
POS_M_MustBeValid(_hDest);
|
|
POS_M_MustBeModifiable(_hDest);
|
|
#endif
|
|
|
|
switch ( _hSource->ulType )
|
|
{
|
|
case POS_C_ulIdentityMatrixFlag:
|
|
if( _hDest != _hSource )
|
|
{
|
|
/*_hDest->ulType = POS_C_ulIdentityMatrixFlag;*/
|
|
POS_fn_vSetIdentityMatrix( _hDest );
|
|
}
|
|
break;
|
|
|
|
case POS_C_ulTranslationMatrixFlag:
|
|
MTH3D_M_vNegVector( &(_hDest->stTranslationVector), &(_hSource->stTranslationVector) );
|
|
if( _hDest != _hSource )
|
|
{
|
|
fn_vSetMatrixAsIdentity(&(_hDest->stTransformMatrix));
|
|
fn_vSetMatrixAsIdentity(&(_hDest->stRotationMatrix));
|
|
_hDest->ulType = POS_C_ulTranslationMatrixFlag;
|
|
}
|
|
break;
|
|
|
|
default: /*rotation or complete matrixes*/
|
|
{
|
|
MTH3D_tdstVector stVertex;
|
|
/* Invert rotation*/
|
|
if( _hDest != _hSource )
|
|
{
|
|
MTH3D_M_vTranspMatrixWithoutBuffer(&(_hDest->stRotationMatrix),&(_hSource->stRotationMatrix));
|
|
}
|
|
else
|
|
{
|
|
MTH3D_M_vTranspMatrix(&(_hDest->stRotationMatrix),&(_hSource->stRotationMatrix));
|
|
}
|
|
/*the transform matrix remains equal to the rotation matrix*/
|
|
MTH3D_M_vCopyMatrix(&(_hDest->stTransformMatrix),&(_hDest->stRotationMatrix));
|
|
|
|
/*invert translation*/
|
|
MTH3D_M_vNegVector(&stVertex, &(_hSource->stTranslationVector) );
|
|
/*if ( _hSource->ulType > POS_C_ulRotationMatrixFlag )
|
|
{
|
|
MTH3D_M_vMulMatrixVector(&(_hDest->stTranslationVector),&(_hDest->stTransformMatrix),&stVertex);
|
|
}
|
|
else
|
|
{*/
|
|
MTH3D_M_vMulMatrixVector(&(_hDest->stTranslationVector),&(_hDest->stTransformMatrix),&stVertex);
|
|
/*}*/
|
|
|
|
_hDest->ulType = POS_C_ulRotationMatrixFlag;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : INVERTING A POSITION
|
|
// Invert a position (general case)
|
|
------------------------------------------------------------------------------
|
|
// Methods : See code
|
|
------------------------------------------------------------------------------
|
|
// Input : _hSource : position to invert
|
|
// Output : _hDest : destination
|
|
------------------------------------------------------------------------------
|
|
// Creation date : ??? Author: Alain Robin
|
|
------------------------------------------------------------------------------
|
|
// Modification date : May 98 Modification author : AR
|
|
// Modifications : Optimisations and correction of bugs
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vInvertMatrix(POS_tdxHandleToPosition _hDest, POS_tdxHandleToPosition _hSource)
|
|
{
|
|
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hSource);
|
|
POS_M_MustBeValid(_hDest);
|
|
POS_M_MustBeModifiable(_hDest);
|
|
#endif
|
|
|
|
if(_hSource->ulType <= POS_C_ulRotationMatrixFlag )
|
|
{
|
|
POS_fn_vInvertIsoMatrix(_hDest, _hSource);
|
|
}
|
|
else
|
|
{
|
|
MTH3D_tdstVector stVertex;
|
|
/*invert the rotation by transposing it*/
|
|
if ( _hDest != _hSource )
|
|
{
|
|
MTH3D_M_vTranspMatrixWithoutBuffer(&(_hDest->stRotationMatrix),&(_hSource->stRotationMatrix));
|
|
}
|
|
else
|
|
{
|
|
MTH3D_M_vTranspMatrix(&(_hDest->stRotationMatrix),&(_hSource->stRotationMatrix));
|
|
}
|
|
|
|
/*invert the transform matrix*/
|
|
MTH3D_M_vInverMatrix(&(_hDest->stTransformMatrix),&(_hSource->stTransformMatrix));
|
|
|
|
/*invert the translation accordingly*/
|
|
MTH3D_M_vNegVector(&stVertex, &(_hSource->stTranslationVector) );
|
|
MTH3D_M_vMulMatrixVector(&(_hDest->stTranslationVector), &(_hDest->stTransformMatrix), &stVertex);
|
|
|
|
/*do not forget to set the matrix type!*/
|
|
_hDest->ulType = POS_C_ulCompleteMatrixFlag;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/************ NORMALIZING THE 3x3 ROTATION COMPONENT OF A MATRIX **********/
|
|
|
|
void POS_fn_vNormalizeMatrix(POS_tdxHandleToPosition _hMatrix)
|
|
{
|
|
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
POS_M_MustBeModifiable(_hMatrix);
|
|
#endif
|
|
|
|
if ( _hMatrix->ulType >= POS_C_ulRotationMatrixFlag )
|
|
{
|
|
MTH3D_tdstVector *p_stI , *p_stJ , *p_stK ;
|
|
/*normalize the rotation*/
|
|
POS_fn_vGetRotationVector( _hMatrix , &p_stI , &p_stJ , &p_stK ) ;
|
|
|
|
MTH3D_M_vCrossProductVectorWithoutBuffer( p_stJ , p_stK , p_stI ) ; /* J= K*I*/
|
|
SAF_M_AssertWithMsg((! MTH3D_M_bIsNullVector(p_stI)),"Normalisation d'une matrice incorrecte")
|
|
MTH3D_M_vNormalizeVector( p_stI,p_stI ) ; /* |I|= 1*/
|
|
SAF_M_AssertWithMsg((! MTH3D_M_bIsNullVector(p_stJ)),"Normalisation d'une matrice incorrecte")
|
|
MTH3D_M_vNormalizeVector( p_stJ,p_stJ ) ; /* |J|= 1*/
|
|
MTH3D_M_vCrossProductVectorWithoutBuffer( p_stK , p_stI , p_stJ ) ; /* K= I*J ; |K|= 1*/
|
|
/*MTH3D_M_vNormalizeVector( &stK,&stK ) ; // useless !*/
|
|
|
|
/*since we worked directly in the position's rotation matrix, it is up-to-date ->*/
|
|
/*a call to POS_fn_vSetRotationMatrix() is unnecessary, which */
|
|
/*the matrix is not scaled anymore*/
|
|
MTH3D_M_vCopyMatrix( &(_hMatrix->stTransformMatrix), &(_hMatrix->stRotationMatrix) );
|
|
_hMatrix->ulType = POS_C_ulRotationMatrixFlag;
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vMulMatrixMatrix
|
|
// Multiply 2 positions
|
|
----------------------------------------------------------------------------//
|
|
// Method : Same than MulMatrixMatrix
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hA, _hB : Handle to the positions to be multiplied
|
|
// Output : _hDest
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : May 97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
// Alexis Vaisse - May 28 1998 : use a temporary matrix only if necesary (-> 10-15 % faster)
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vMulMatrixMatrix(
|
|
POS_tdxHandleToPosition _hDest,
|
|
POS_tdxHandleToPosition _hA,
|
|
POS_tdxHandleToPosition _hB
|
|
)
|
|
{
|
|
POS_tdstCompletePosition stTempMatrix , *p_stResult;
|
|
unsigned long ulResultType;
|
|
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hA);
|
|
POS_M_MustBeValid(_hB);
|
|
POS_M_MustBeValid(_hDest);
|
|
POS_M_MustBeModifiable(_hDest);
|
|
#endif
|
|
|
|
M_CheckPointer(_hDest);
|
|
M_CheckPointer(_hA);
|
|
M_CheckPointer(_hB);
|
|
if (_hA->ulType == POS_C_ulIdentityMatrixFlag )
|
|
{
|
|
if ( _hB != _hDest )
|
|
{
|
|
POS_fn_vCopyMatrix(_hDest, _hB);
|
|
}
|
|
return;
|
|
}
|
|
|
|
if ( _hB->ulType == POS_C_ulIdentityMatrixFlag )
|
|
{
|
|
if ( _hA != _hDest )
|
|
{
|
|
POS_fn_vCopyMatrix(_hDest, _hA);
|
|
}
|
|
return;
|
|
}
|
|
|
|
if ( (_hDest == _hA) || (_hDest == _hB) )
|
|
{
|
|
p_stResult = &stTempMatrix;
|
|
}
|
|
else
|
|
{
|
|
p_stResult = _hDest;
|
|
}
|
|
|
|
ulResultType = max(_hA->ulType, _hB->ulType);
|
|
|
|
/* multiply translation vector*/
|
|
MTH3D_M_vTransformVectorWithoutBuffer(&(p_stResult->stTranslationVector), &(_hA->stTransformMatrix), &(_hB->stTranslationVector), &(_hA->stTranslationVector));
|
|
/* multiply rotation matrices*/
|
|
MTH3D_M_vMulMatrixMatrixWithoutBuffer(&(p_stResult->stRotationMatrix), &(_hA->stRotationMatrix), &(_hB->stRotationMatrix));
|
|
|
|
/*if no source matrix has scale, the destination won't have either*/
|
|
if ( ulResultType <= POS_C_ulRotationMatrixFlag )
|
|
{
|
|
MTH3D_M_vCopyMatrix( &(p_stResult->stTransformMatrix), &(p_stResult->stRotationMatrix) );
|
|
}
|
|
else
|
|
{
|
|
MTH3D_M_vMulMatrixMatrixWithoutBuffer(&(p_stResult->stTransformMatrix), &(_hA->stTransformMatrix), &(_hB->stTransformMatrix) );
|
|
}
|
|
|
|
if( _hDest != p_stResult )
|
|
POS_fn_vCopyMatrix(_hDest, p_stResult);
|
|
|
|
_hDest->ulType = ulResultType;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vRotatePositionAroundAxis
|
|
// Rotate a position around an axis passing by the origin with a given angle
|
|
------------------------------------------------------------------------------
|
|
// Method : create a quaternion, decompress it and multiply the matrices
|
|
------------------------------------------------------------------------------
|
|
// Input : _hPos : Position to be rotated
|
|
// _p_stAxisVector : components of the axis
|
|
// _xAngle : Angle of the rotation
|
|
// WARNING : THE VECTOR (X,Y,Z) must be normed and the angle is defined with
|
|
// the antitrigonometric sense
|
|
// Output :
|
|
------------------------------------------------------------------------------
|
|
// Creation date : Jul 97 Author: Alain Robin
|
|
------------------------------------------------------------------------------
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vRotatePositionAroundAxis(
|
|
POS_tdxHandleToPosition _hPos,
|
|
MTH3D_tdstVector* _p_stAxisVector,
|
|
MTH_tdxReal _xAngle
|
|
)
|
|
{
|
|
MTH3D_tdstMatrix stMatrix;
|
|
MTH3D_tdstVector stAxis ;
|
|
MTH_tdxReal xSinAngle;
|
|
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hPos);
|
|
POS_M_MustBeModifiable(_hPos);
|
|
#endif
|
|
|
|
/* Normalize the quaternion */
|
|
_xAngle = MTH_M_xMul(_xAngle,MTH_C_Inv2);
|
|
xSinAngle = MTH_M_xSin(_xAngle);
|
|
_xAngle = MTH_M_xCos(_xAngle);
|
|
|
|
MTH3D_M_vMulScalarVector ( & stAxis , xSinAngle , _p_stAxisVector ) ;
|
|
|
|
POS_fn_vQuatToMat ( & stMatrix , stAxis . xX , stAxis . xY , stAxis . xZ , _xAngle ) ;
|
|
|
|
/* rotation matrix */
|
|
if ( _hPos->ulType <= POS_C_ulTranslationMatrixFlag ) /* no rotation*/
|
|
{
|
|
MTH3D_M_vCopyMatrix(&(_hPos->stRotationMatrix), &stMatrix);
|
|
}
|
|
else
|
|
{
|
|
MTH3D_M_vMulMatrixMatrix(&(_hPos->stRotationMatrix), &(_hPos->stRotationMatrix), &stMatrix);
|
|
}
|
|
|
|
/* transform matrix */
|
|
if ( _hPos->ulType <= POS_C_ulRotationMatrixFlag ) /* no scale*/
|
|
{
|
|
MTH3D_M_vCopyMatrix(&(_hPos->stTransformMatrix), &(_hPos->stRotationMatrix));
|
|
}
|
|
else
|
|
{
|
|
MTH3D_M_vMulMatrixMatrix(&(_hPos->stTransformMatrix), &(_hPos->stTransformMatrix), &stMatrix);
|
|
}
|
|
|
|
/* translation */
|
|
MTH3D_M_vMulMatrixVector(&(_hPos->stTranslationVector), &stMatrix, &(_hPos->stTranslationVector));
|
|
|
|
_hPos->ulType = max(_hPos->ulType, POS_C_ulRotationMatrixFlag);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vRotatePositionAroundAxisNoTranslation
|
|
// Rotate a position around an axis passing by the origin with a given angle
|
|
------------------------------------------------------------------------------
|
|
// Method : create a quaternion, decompress it and multiply the matrices
|
|
------------------------------------------------------------------------------
|
|
// Input : _hPos : Position to be rotated
|
|
// _p_stAxisVector : components of the axis
|
|
// _xAngle : Angle of the rotation
|
|
// WARNING : THE VECTOR (X,Y,Z) must be normed and the angle is defined with
|
|
// the antitrigonometric sense
|
|
// Output :
|
|
------------------------------------------------------------------------------
|
|
// Creation date : Feb 98 Author: Benoit Germain
|
|
------------------------------------------------------------------------------
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vRotatePositionAroundAxisNoTranslation(
|
|
POS_tdxHandleToPosition _hPos,
|
|
MTH3D_tdstVector* _p_stAxisVector,
|
|
MTH_tdxReal _xAngle
|
|
)
|
|
{
|
|
MTH3D_tdstMatrix stMatrix;
|
|
MTH3D_tdstVector stAxis ;
|
|
MTH_tdxReal xSinAngle;
|
|
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hPos);
|
|
POS_M_MustBeModifiable(_hPos);
|
|
#endif
|
|
|
|
/* Normalize the quaternion */
|
|
_xAngle = MTH_M_xMul(_xAngle,MTH_C_Inv2);
|
|
xSinAngle = MTH_M_xSin(_xAngle);
|
|
_xAngle = MTH_M_xCos(_xAngle);
|
|
|
|
MTH3D_M_vMulScalarVector ( & stAxis , xSinAngle , _p_stAxisVector ) ;
|
|
|
|
POS_fn_vQuatToMat ( & stMatrix , stAxis . xX , stAxis . xY , stAxis . xZ , _xAngle ) ;
|
|
|
|
/* rotation matrix */
|
|
if ( _hPos->ulType <= POS_C_ulTranslationMatrixFlag ) /* no rotation*/
|
|
{
|
|
/*rotation is identity: copy the new rotation immediately*/
|
|
MTH3D_M_vCopyMatrix(&(_hPos->stRotationMatrix), &stMatrix);
|
|
}
|
|
else /*rotation is not identity: multiply matrixes*/
|
|
{
|
|
MTH3D_M_vMulMatrixMatrix(&(_hPos->stRotationMatrix), &(_hPos->stRotationMatrix), &stMatrix);
|
|
}
|
|
|
|
/* transform matrix */
|
|
if ( _hPos->ulType <= POS_C_ulRotationMatrixFlag ) /* no scale*/
|
|
{
|
|
MTH3D_M_vCopyMatrix(&(_hPos->stTransformMatrix),&(_hPos->stRotationMatrix));
|
|
}
|
|
else
|
|
{
|
|
MTH3D_M_vMulMatrixMatrix(&(_hPos->stTransformMatrix), &(_hPos->stTransformMatrix), &stMatrix);
|
|
}
|
|
|
|
_hPos->ulType = max(_hPos->ulType, POS_C_ulRotationMatrixFlag);
|
|
}
|
|
|
|
/*ANNECY BBB }*/
|
|
|
|
|
|
|
|
/****************************************************************************/
|
|
void POS_fn_vRotateX(POS_tdxHandleToPosition hMatrix, MTH_tdxReal xXAngle)
|
|
{
|
|
MTH3D_tdstVector stI;
|
|
MTH3D_tdstVector stJ;
|
|
MTH3D_tdstVector stK;
|
|
MTH_tdxReal xSin = MTH_M_xSin(xXAngle);
|
|
MTH_tdxReal xCos = MTH_M_xCos(xXAngle);
|
|
|
|
/* POS_fn_vGetRotationMatrix(hMatrix, &stI, &stJ, &stK); //utility ??? AR9808*/
|
|
|
|
MTH3D_M_vSetVectorElements(&stI, MTH_C_ONE, MTH_C_ZERO, MTH_C_ZERO);
|
|
MTH3D_M_vSetVectorElements(&stJ, MTH_C_ZERO, xCos, xSin);
|
|
MTH3D_M_vSetVectorElements(&stK, MTH_C_ZERO, MTH_M_xNeg(xSin), xCos);
|
|
|
|
POS_fn_vSetRotationMatrix(hMatrix, &stI, &stJ, &stK);
|
|
}
|
|
|
|
/****************************************************************************/
|
|
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
|
|
void POS_fn_vRotateY(POS_tdxHandleToPosition hMatrix,MTH_tdxReal xYAngle)
|
|
{
|
|
MTH3D_tdstVector stI;
|
|
MTH3D_tdstVector stJ;
|
|
MTH3D_tdstVector stK;
|
|
MTH_tdxReal xSin = MTH_M_xSin(xYAngle);
|
|
MTH_tdxReal xCos = MTH_M_xCos(xYAngle);
|
|
|
|
/* POS_fn_vGetRotationMatrix(hMatrix, &stI, &stJ, &stK); //utility ??? AR9808*/
|
|
|
|
MTH3D_M_vSetVectorElements(&stI, xCos, MTH_C_ZERO, MTH_M_xNeg(xSin));
|
|
MTH3D_M_vSetVectorElements(&stJ, MTH_C_ZERO, MTH_C_ONE, MTH_C_ZERO);
|
|
MTH3D_M_vSetVectorElements(&stK, xSin, MTH_C_ZERO, xCos);
|
|
|
|
POS_fn_vSetRotationMatrix(hMatrix, &stI, &stJ, &stK);
|
|
}
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
|
|
|
|
|
|
/****************************************************************************/
|
|
void POS_fn_vRotateZ(POS_tdxHandleToPosition hMatrix,MTH_tdxReal xZAngle)
|
|
{
|
|
MTH3D_tdstVector stI;
|
|
MTH3D_tdstVector stJ;
|
|
MTH3D_tdstVector stK;
|
|
MTH_tdxReal xSin = MTH_M_xSin(xZAngle);
|
|
MTH_tdxReal xCos = MTH_M_xCos(xZAngle);
|
|
|
|
/* POS_fn_vGetRotationMatrix(hMatrix, &stI, &stJ, &stK); //utility ??? AR9808*/
|
|
|
|
MTH3D_M_vSetVectorElements(&stI, xCos, xSin, MTH_C_ZERO);
|
|
MTH3D_M_vSetVectorElements(&stJ, MTH_M_xNeg(xSin), xCos, MTH_C_ZERO);
|
|
MTH3D_M_vSetVectorElements(&stK, MTH_C_ZERO, MTH_C_ZERO, MTH_C_ONE);
|
|
|
|
POS_fn_vSetRotationMatrix(hMatrix, &stI, &stJ, &stK);
|
|
}
|
|
|
|
/****************************************************************************/
|
|
/* ACCESSORS */
|
|
/****************************************************************************/
|
|
|
|
/************* SETTING THE 3x3 ROTATION COMPONENT OF A MATRIX ***************/
|
|
|
|
void POS_fn_vSetRotationMatrix(
|
|
POS_tdxHandleToPosition _hMatrix,
|
|
MTH3D_tdstVector *_p_stI,
|
|
MTH3D_tdstVector *_p_stJ,
|
|
MTH3D_tdstVector *_p_stK
|
|
)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
POS_M_MustBeModifiable(_hMatrix);
|
|
#endif
|
|
|
|
switch(_hMatrix->ulType)
|
|
{
|
|
case POS_C_ulIdentityMatrixFlag:
|
|
case POS_C_ulTranslationMatrixFlag:
|
|
case POS_C_ulRotationMatrixFlag:
|
|
MTH3D_M_vSetVectorsInMatrix( &(_hMatrix->stRotationMatrix), _p_stI, _p_stJ, _p_stK );
|
|
MTH3D_M_vCopyMatrix(&(_hMatrix->stTransformMatrix), &(_hMatrix->stRotationMatrix));
|
|
_hMatrix->ulType = POS_C_ulRotationMatrixFlag;
|
|
break;
|
|
|
|
default: /*matrix has a complex scale*/
|
|
{
|
|
MTH3D_tdstMatrix stScaleMatrix;
|
|
/* First, we must Get the scale component */
|
|
POS_fn_vGetScaleMatrix(
|
|
_hMatrix,
|
|
&(stScaleMatrix.stCol_0),
|
|
&(stScaleMatrix.stCol_1),
|
|
&(stScaleMatrix.stCol_2)
|
|
);
|
|
|
|
/*apply the new rotation*/
|
|
MTH3D_M_vSetVectorsInMatrix( &(_hMatrix->stRotationMatrix), _p_stI, _p_stJ, _p_stK );
|
|
|
|
/* After that, we can compute the new transform matrix */
|
|
MTH3D_M_vMulMatrixMatrixWithoutBuffer(&(_hMatrix->stTransformMatrix), &(_hMatrix->stRotationMatrix), &stScaleMatrix);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
/************* GETTING THE COLUMNS OF THE 3x3 ROTATION COMPONENT OF A MATRIX *************/
|
|
/* POS_fn_vGetRotationMatrixCol2(p_stCurrentMatrix, &stZ);*/
|
|
|
|
void
|
|
POS_fn_vGetRotationMatrixCol2(
|
|
POS_tdxHandleToPosition _hMatrix,
|
|
MTH3D_tdstVector *_p_stCol2
|
|
)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
MTH3D_M_vGetColumnInMatrix(_p_stCol2,&(_hMatrix->stRotationMatrix),2);
|
|
}
|
|
|
|
|
|
|
|
void
|
|
POS_fn_vGetRotationMatrix(
|
|
POS_tdxHandleToPosition _hMatrix,
|
|
MTH3D_tdstVector *_p_stI,
|
|
MTH3D_tdstVector *_p_stJ,
|
|
MTH3D_tdstVector *_p_stK
|
|
)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
|
|
MTH3D_M_vGetVectorsInMatrix( _p_stI, _p_stJ, _p_stK, &(_hMatrix->stRotationMatrix) );
|
|
}
|
|
|
|
void POS_fn_vGetRotationVector(
|
|
POS_tdxHandleToPosition _hMatrix ,
|
|
MTH3D_tdstVector **_p_p_stI,
|
|
MTH3D_tdstVector **_p_p_stJ,
|
|
MTH3D_tdstVector **_p_p_stK
|
|
)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
|
|
*_p_p_stI = &(_hMatrix->stRotationMatrix.stCol_0);
|
|
*_p_p_stJ = &(_hMatrix->stRotationMatrix.stCol_1);
|
|
*_p_p_stK = &(_hMatrix->stRotationMatrix.stCol_2);
|
|
}
|
|
|
|
|
|
|
|
|
|
/************* SETTING THE 3x3 SCALING COMPONENT OF A MATRIX *************/
|
|
|
|
void
|
|
POS_fn_vSetScaleMatrix(
|
|
POS_tdxHandleToPosition _hMatrix,
|
|
MTH3D_tdstVector *_p_stI,
|
|
MTH3D_tdstVector *_p_stJ,
|
|
MTH3D_tdstVector *_p_stK
|
|
)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
POS_M_MustBeModifiable(_hMatrix);
|
|
#endif
|
|
|
|
if
|
|
(
|
|
MTH_M_bEqualWithEpsilon(_p_stI->xX,MTH_C_ONE,MTH_M_xFloatToReal(0.01)) &&
|
|
MTH_M_bEqualWithEpsilon(_p_stJ->xY,MTH_C_ONE,MTH_M_xFloatToReal(0.01)) &&
|
|
MTH_M_bEqualWithEpsilon(_p_stK->xZ,MTH_C_ONE,MTH_M_xFloatToReal(0.01))
|
|
)
|
|
{
|
|
/*the matrix looses its scale attribute*/
|
|
MTH3D_M_vCopyMatrix(&(_hMatrix->stTransformMatrix),&(_hMatrix->stRotationMatrix));
|
|
/*it is a simple rotation matrix*/
|
|
_hMatrix->ulType = POS_C_ulRotationMatrixFlag;
|
|
return;
|
|
}
|
|
|
|
switch ( _hMatrix->ulType )
|
|
{
|
|
case POS_C_ulIdentityMatrixFlag:
|
|
case POS_C_ulTranslationMatrixFlag:
|
|
MTH3D_M_vSetVectorsInMatrix( &(_hMatrix->stTransformMatrix), _p_stI, _p_stJ, _p_stK );
|
|
_hMatrix->ulType = POS_C_ulCompleteMatrixFlag;
|
|
break;
|
|
|
|
default: /*zoomed matrix*/
|
|
{
|
|
MTH3D_tdstMatrix stScaleMatrix;
|
|
MTH3D_M_vSetVectorsInMatrix( &stScaleMatrix, _p_stI, _p_stJ, _p_stK );
|
|
MTH3D_M_vMulMatrixMatrixWithoutBuffer( &(_hMatrix->stTransformMatrix), &(_hMatrix->stRotationMatrix), &stScaleMatrix);
|
|
_hMatrix->ulType = POS_C_ulCompleteMatrixFlag;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_xGetXScale
|
|
// Get the X component of the scale matrix - derived from the transform matrix
|
|
----------------------------------------------------------------------------//
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : Handle to the complete matrix
|
|
// Output : returns the value of the X scaling component
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : Aug 98 Author: Steve McCalla
|
|
----------------------------------------------------------------------------*/
|
|
|
|
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
|
|
MTH_tdxReal
|
|
POS_fn_xGetXScale( POS_tdxHandleToPosition _hMatrix )
|
|
{
|
|
MTH_tdxReal xScaleX = 1.0f;
|
|
|
|
if ( _hMatrix->ulType > POS_C_ulRotationMatrixFlag ) {
|
|
xScaleX = MTH_M_xSqrt((_hMatrix->stTransformMatrix.stCol_0.xX * _hMatrix->stTransformMatrix.stCol_0.xX)
|
|
+ (_hMatrix->stTransformMatrix.stCol_1.xX * _hMatrix->stTransformMatrix.stCol_1.xX)
|
|
+ (_hMatrix->stTransformMatrix.stCol_2.xX * _hMatrix->stTransformMatrix.stCol_2.xX));
|
|
}
|
|
return xScaleX;
|
|
}
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_xGetYScale
|
|
// Get the Y component of the scale matrix - derived from the transform matrix
|
|
----------------------------------------------------------------------------//
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : Handle to the complete matrix
|
|
// Output : returns the value of the X scaling component
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : Aug 98 Author: Steve McCalla
|
|
----------------------------------------------------------------------------*/
|
|
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
|
|
MTH_tdxReal
|
|
POS_fn_xGetYScale( POS_tdxHandleToPosition _hMatrix )
|
|
{
|
|
MTH_tdxReal xScaleY = 1.0f;
|
|
|
|
if ( _hMatrix->ulType > POS_C_ulRotationMatrixFlag ) {
|
|
xScaleY = MTH_M_xSqrt((_hMatrix->stTransformMatrix.stCol_0.xY * _hMatrix->stTransformMatrix.stCol_0.xY)
|
|
+ (_hMatrix->stTransformMatrix.stCol_1.xY * _hMatrix->stTransformMatrix.stCol_1.xY)
|
|
+ (_hMatrix->stTransformMatrix.stCol_2.xY * _hMatrix->stTransformMatrix.stCol_2.xY));
|
|
}
|
|
return xScaleY;
|
|
}
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_xGetZScale
|
|
// Get the Z component of the scale matrix - derived from the transform matrix
|
|
----------------------------------------------------------------------------//
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : Handle to the complete matrix
|
|
// Output : returns the value of the Z scaling component
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : Aug 98 Author: Steve McCalla
|
|
----------------------------------------------------------------------------*/
|
|
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
|
|
MTH_tdxReal
|
|
POS_fn_xGetZScale( POS_tdxHandleToPosition _hMatrix )
|
|
{
|
|
MTH_tdxReal xScaleZ = 1.0f;
|
|
|
|
if ( _hMatrix->ulType > POS_C_ulRotationMatrixFlag ) {
|
|
xScaleZ = MTH_M_xSqrt((_hMatrix->stTransformMatrix.stCol_0.xZ * _hMatrix->stTransformMatrix.stCol_0.xZ)
|
|
+ (_hMatrix->stTransformMatrix.stCol_1.xZ * _hMatrix->stTransformMatrix.stCol_1.xZ)
|
|
+ (_hMatrix->stTransformMatrix.stCol_2.xZ * _hMatrix->stTransformMatrix.stCol_2.xZ));
|
|
}
|
|
return xScaleZ;
|
|
}
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */
|
|
|
|
|
|
|
|
/************* GETTING THE 3x3 SCALING COMPONENT OF A MATRIX *************/
|
|
/* Modif : Check if scal is used and return ID*/
|
|
void POS_fn_vGetScaleMatrix(
|
|
POS_tdxHandleToPosition _hMatrix ,
|
|
MTH3D_tdstVector *_p_stI ,
|
|
MTH3D_tdstVector *_p_stJ ,
|
|
MTH3D_tdstVector *_p_stK
|
|
)
|
|
{
|
|
/* MTH_tdxReal xScaleX;*/
|
|
/* MTH_tdxReal xScaleY;*/
|
|
/* MTH_tdxReal xScaleZ;*/
|
|
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
|
|
MTH3D_M_vSetBaseIVector(_p_stI);
|
|
MTH3D_M_vSetBaseJVector(_p_stJ);
|
|
MTH3D_M_vSetBaseKVector(_p_stK);
|
|
|
|
if ( _hMatrix->ulType > POS_C_ulRotationMatrixFlag )
|
|
{
|
|
/* */
|
|
/*#define NEW_CODE
|
|
#ifdef NEW_CODE
|
|
xScaleX = MTH_M_xSqrt((_hMatrix->stTransformMatrix.stCol_0.xX * _hMatrix->stTransformMatrix.stCol_0.xX)
|
|
+ (_hMatrix->stTransformMatrix.stCol_1.xX * _hMatrix->stTransformMatrix.stCol_1.xX)
|
|
+ (_hMatrix->stTransformMatrix.stCol_2.xX * _hMatrix->stTransformMatrix.stCol_2.xX));
|
|
|
|
xScaleY = MTH_M_xSqrt((_hMatrix->stTransformMatrix.stCol_0.xY * _hMatrix->stTransformMatrix.stCol_0.xY)
|
|
+ (_hMatrix->stTransformMatrix.stCol_1.xY * _hMatrix->stTransformMatrix.stCol_1.xY)
|
|
+ (_hMatrix->stTransformMatrix.stCol_2.xY * _hMatrix->stTransformMatrix.stCol_2.xY));
|
|
|
|
xScaleZ = MTH_M_xSqrt((_hMatrix->stTransformMatrix.stCol_0.xZ * _hMatrix->stTransformMatrix.stCol_0.xZ)
|
|
+ (_hMatrix->stTransformMatrix.stCol_1.xZ * _hMatrix->stTransformMatrix.stCol_1.xZ)
|
|
+ (_hMatrix->stTransformMatrix.stCol_2.xZ * _hMatrix->stTransformMatrix.stCol_2.xZ));
|
|
|
|
// non-diagonal members were already set to zero above
|
|
_p_stI->xX = xScaleX;
|
|
_p_stJ->xY = xScaleY;
|
|
_p_stK->xZ = xScaleZ;
|
|
#else
|
|
*/ MTH3D_tdstMatrix stScaleMatrix;
|
|
|
|
MTH3D_M_vMulTranspMatrixMatrixWithoutBuffer(&stScaleMatrix,
|
|
&(_hMatrix->stRotationMatrix),
|
|
&(_hMatrix->stTransformMatrix));
|
|
|
|
MTH3D_M_vGetVectorsInMatrix(_p_stI,_p_stJ,_p_stK,&stScaleMatrix);
|
|
/*#endif*/
|
|
}
|
|
|
|
}
|
|
|
|
/***************************************************************************/
|
|
void POS_fn_vSetZoomMatrix( POS_tdxHandleToPosition _hMatrix , MTH_tdxReal _xZoom)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
POS_M_MustBeModifiable(_hMatrix);
|
|
#endif
|
|
|
|
MTH3D_M_vMulScalarMatrix(&(_hMatrix->stTransformMatrix), _xZoom, &(_hMatrix->stRotationMatrix));
|
|
_hMatrix->ulType = POS_C_ulCompleteMatrixFlag;
|
|
}
|
|
|
|
/************* GETTING THE 3x3 TRANSFORM COMPONENT OF A MATRIX *************/
|
|
|
|
|
|
/*macros are faster, but kind of hard/impossible to debug*/
|
|
#ifndef USE_POS_MACROS
|
|
/*
|
|
// POS_fn_vGetTransformMatrixCol0 - returns the first column (column 0) of the transform matrix
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : Handle to the complete matrix
|
|
// _p_Vec : pointer to the Col 0 vector
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : Aug 98 Author: Steve McCalla
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
void
|
|
POS_fn_vGetTransformMatrixCol0 ( POS_tdxHandleToPosition _hMatrix ,
|
|
MTH3D_tdstVector *_p_Vec)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
MTH3D_M_vGetColumn0InMatrix(_p_Vec, &(_hMatrix->stTransformMatrix));
|
|
}
|
|
|
|
/*
|
|
// POS_fn_vGetTransformMatrixCol1 - returns the second column (column 1) of the transform matrix
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : Handle to the complete matrix
|
|
// _p_Vec : pointer to the Col 1 vector
|
|
// Output :
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : Aug 98 Author: Steve McCalla
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
void
|
|
POS_fn_vGetTransformMatrixCol1 ( POS_tdxHandleToPosition _hMatrix ,
|
|
MTH3D_tdstVector *_p_Vec)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
MTH3D_M_vGetColumn1InMatrix(_p_Vec, &(_hMatrix->stTransformMatrix));
|
|
}
|
|
|
|
/*
|
|
// POS_fn_vGetTransformMatrixCol2 - returns the third column (column 2) of the transform matrix
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : Handle to the complete matrix
|
|
// _p_Vec : pointer to the Col 2 vector
|
|
// Output :
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : Aug 98 Author: Steve McCalla
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
void
|
|
POS_fn_vGetTransformMatrixCol2 ( POS_tdxHandleToPosition _hMatrix ,
|
|
MTH3D_tdstVector *_p_Vec)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
MTH3D_M_vGetColumn2InMatrix(_p_Vec, &(_hMatrix->stTransformMatrix));
|
|
}
|
|
|
|
#endif /* USE_POS_MACROS*/
|
|
|
|
|
|
|
|
void
|
|
POS_fn_vGetTransformMatrix ( POS_tdxHandleToPosition _hMatrix ,
|
|
MTH3D_tdstVector *_p_stI ,
|
|
MTH3D_tdstVector *_p_stJ ,
|
|
MTH3D_tdstVector *_p_stK )
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
|
|
MTH3D_M_vGetVectorsInMatrix(_p_stI,_p_stJ,_p_stK,&(_hMatrix->stTransformMatrix));
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vSetTransformMatrix
|
|
// Used to set the transform part (calculated matrix) of the complete matrix
|
|
----------------------------------------------------------------------------//
|
|
// Method : nothing to say
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : Handle to the complete matrix
|
|
// _p_stI, _p_stJ, _p_stK : pointer to the transformation matrix
|
|
// Output :
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : Mar 97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vSetTransformMatrix ( POS_tdxHandleToPosition _hMatrix ,
|
|
MTH3D_tdstVector *_p_stI ,
|
|
MTH3D_tdstVector *_p_stJ ,
|
|
MTH3D_tdstVector *_p_stK )
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
POS_M_MustBeModifiable(_hMatrix);
|
|
#endif
|
|
|
|
/* assert that this function never does anything useful*/
|
|
|
|
MTH3D_M_vSetVectorsInMatrix(&(_hMatrix->stTransformMatrix),_p_stI,_p_stJ,_p_stK);
|
|
_hMatrix->ulType = POS_C_ulCompleteMatrixFlag;
|
|
}
|
|
|
|
/**************** SETTING THE TRANSLATION VERTEX OF A MATRIX *************/
|
|
void POS_fn_vSetTranslationVector(
|
|
POS_tdxHandleToPosition _hMatrix,
|
|
MTH3D_tdstVector *_p_stTrs
|
|
)
|
|
{
|
|
/* unsigned short uwType;*/
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
POS_M_MustBeModifiable(_hMatrix);
|
|
#endif
|
|
|
|
MTH3D_M_vCopyVector( &(_hMatrix->stTranslationVector), _p_stTrs );
|
|
if ( _hMatrix->ulType == POS_C_ulIdentityMatrixFlag )
|
|
_hMatrix->ulType = POS_C_ulTranslationMatrixFlag;
|
|
}
|
|
|
|
/**************** GETTING THE TRANSLATION VERTEX OF A MATRIX *************/
|
|
void POS_fn_vGetTranslationVector(
|
|
POS_tdxHandleToPosition _hMatrix,
|
|
MTH3D_tdstVector *_p_stTrs
|
|
)
|
|
{
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
MTH3D_M_vCopyVector( _p_stTrs, &(_hMatrix->stTranslationVector) );
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/**************** GETTING THE SCALE STATE ********************************/
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vGetMaxScale
|
|
// Used to get the maximum scale factor of the 3 axes
|
|
----------------------------------------------------------------------------//
|
|
// Method : nothing to say
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : Handle to the complete matrix
|
|
// Output : the maximum scale factor
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : May 97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
MTH_tdxReal POS_fn_xGetMaxScale(POS_tdxHandleToPosition _hMatrix )
|
|
{
|
|
/* MTH3D_tdstMatrix stScaleMatrix;*/
|
|
MTH_tdxReal xRet;
|
|
MTH_tdxReal xL1,xL2,xL3;
|
|
|
|
#define C_2DIV27 ((MTH_tdxReal) 0.07407407407407f)
|
|
|
|
#ifdef _DEBUG
|
|
POS_M_MustBeValid(_hMatrix);
|
|
#endif
|
|
|
|
if ( _hMatrix->ulType <= POS_C_ulRotationMatrixFlag )
|
|
return MTH_C_ONE;
|
|
|
|
/* STM - from testing on several maps, it looks like scale matrix is always _nearly_ diagonal*/
|
|
#define DIAGONAL_SCALE_MATRIX
|
|
#ifdef DIAGONAL_SCALE_MATRIX
|
|
/* We suppose we have a diagonal matrix */
|
|
xL1 = MTH3D_M_xSqrVector(&_hMatrix->stTransformMatrix.stCol_0);
|
|
xL2 = MTH3D_M_xSqrVector(&_hMatrix->stTransformMatrix.stCol_1);
|
|
xL3 = MTH3D_M_xSqrVector(&_hMatrix->stTransformMatrix.stCol_2);
|
|
xRet = MTH_M_xMax(xL1, xL2);
|
|
xRet = MTH_M_xMax(xRet, xL3);
|
|
return MTH_M_xSqrt(xRet);
|
|
#else
|
|
/* get the scale matrix*/
|
|
MTH3D_M_vMulTranspMatrixMatrixWithoutBuffer(&stScaleMatrix, &(_hMatrix->stRotationMatrix), &(_hMatrix->stTransformMatrix));
|
|
/* We test if we have a diagonal matrix */
|
|
if
|
|
(
|
|
(!POS_M_bIsDeltaNull(stScaleMatrix.stCol_0.xY)) || (!POS_M_bIsDeltaNull(stScaleMatrix.stCol_0.xZ))
|
|
|| (!POS_M_bIsDeltaNull(stScaleMatrix.stCol_1.xX)) || (!POS_M_bIsDeltaNull(stScaleMatrix.stCol_1.xZ))
|
|
|| (!POS_M_bIsDeltaNull(stScaleMatrix.stCol_2.xX)) || (!POS_M_bIsDeltaNull(stScaleMatrix.stCol_2.xY))
|
|
)
|
|
{ /*the matrix is not diagonal*/
|
|
MTH_tdxReal xA,xB,xC,xD,xE,xF,xM,xP,xQ,xR,xTeta,xCosTeta,xSinTeta,xCT,xP2;
|
|
/* Here, we have a complex scale matrix */
|
|
/* We compute the eigen values of the matrix */
|
|
xC = MTH_M_xMul( stScaleMatrix.stCol_0.xX , stScaleMatrix.stCol_1.xY);
|
|
xD = MTH_M_xMul( stScaleMatrix.stCol_2.xY , stScaleMatrix.stCol_1.xZ);
|
|
xE = MTH_M_xMul( stScaleMatrix.stCol_1.xX , stScaleMatrix.stCol_0.xY);
|
|
xF = MTH_M_xMul( stScaleMatrix.stCol_2.xX , stScaleMatrix.stCol_0.xZ);
|
|
xP = MTH_M_xNeg( MTH_M_xAdd3( stScaleMatrix.stCol_0.xX , stScaleMatrix.stCol_1.xY , stScaleMatrix.stCol_2.xZ ) );
|
|
xP2= MTH_M_xSqr( xP );
|
|
/*xQ=xC+(stScaleMatrix.stCol_0.xX+stScaleMatrix.stCol_1.xY)*stScaleMatrix.stCol_2.xZ-xD-xE-xF;*/
|
|
xQ = MTH_M_xMul( MTH_M_xAdd( stScaleMatrix.stCol_0.xX , stScaleMatrix.stCol_1.xY ) , stScaleMatrix.stCol_2.xZ );
|
|
xQ = MTH_M_xAdd( xQ , xC );
|
|
xQ = MTH_M_xSub( xQ , MTH_M_xAdd3( xD , xE , xF ) );
|
|
/* xR=(xE-xC)*stScaleMatrix.stCol_2.xZ+xD*stScaleMatrix.stCol_0.xX-*/
|
|
/* 2*(stScaleMatrix.stCol_1.xX*stScaleMatrix.stCol_2.xY*stScaleMatrix.stCol_0.xZ)+*/
|
|
/* xF*stScaleMatrix.stCol_1.xY;*/
|
|
xR = MTH_M_xMul( MTH_M_xSub( xE , xC ) , stScaleMatrix.stCol_2.xZ );
|
|
xR = MTH_M_xAdd( xR , MTH_M_xMul( xD , stScaleMatrix.stCol_0.xX ) );
|
|
xR = MTH_M_xSub( xR , MTH_M_xMul( MTH_C_2 , MTH_M_xMul3( stScaleMatrix.stCol_1.xX , stScaleMatrix.stCol_2.xY , stScaleMatrix.stCol_0.xZ ) ) );
|
|
xR = MTH_M_xAdd( xR , MTH_M_xMul( xF , stScaleMatrix.stCol_1.xY ) );
|
|
/*xA=xQ-xP2*MTH_C_Inv3;*/
|
|
xA = MTH_M_xSub( xQ , MTH_M_xMul( xP2 , MTH_C_Inv3 ) );
|
|
/*xB=xP*xP2*C_2DIV27-(xP*xQ)*MTH_C_Inv3+xR;*/
|
|
xB = MTH_M_xNeg( MTH_M_xMul3( xP , xQ , MTH_C_Inv3 ) );
|
|
xB = MTH_M_xAdd3( MTH_M_xMul3( xP , xP2 , C_2DIV27 ) , xB , xR );
|
|
/*xM=2*MTH_M_xSqrt(-xA*MTH_C_Inv3);*/
|
|
xM = MTH_M_xNeg( MTH_M_xMul( xA , MTH_C_Inv3 ) );
|
|
xM = MTH_M_xMul( MTH_C_2 , MTH_M_xSqrt( xM ) );
|
|
|
|
/*
|
|
xCT=(3*xB)/(xA*xM);
|
|
if(xCT>1.0)
|
|
xCT=1.0;
|
|
if(xCT<(-1.0))
|
|
xCT=-1.0;
|
|
*/
|
|
xCT = MTH_M_xDiv( MTH_M_xMul( MTH_C_3 , xB ) , MTH_M_xMul( xA , xM ) );
|
|
if ( MTH_M_bGreater( xCT , MTH_C_ONE ) )
|
|
xCT = MTH_C_ONE;
|
|
if ( MTH_M_bLess( xCT , MTH_C_MinusONE ) )
|
|
xCT = MTH_C_MinusONE;
|
|
|
|
|
|
/*xTeta=MTH_C_Inv3*MTH_M_xACos(xCT);*/
|
|
xTeta = MTH_M_xMul( MTH_C_Inv3 , MTH_M_xACos( xCT ) );
|
|
xCosTeta = MTH_M_xCos( xTeta );
|
|
xSinTeta = MTH_M_xSin( xTeta );
|
|
|
|
/*xL1=xM*xCosTeta-(xP*MTH_C_Inv3);*/
|
|
xL1 = MTH_M_xMulSubMul( xM , xCosTeta , xP , MTH_C_Inv3 );
|
|
/*xL2=xM*((xCosTeta+MTH_C_Sqrt3*xSinTeta)*MTH_C_Inv2)-(xP*MTH_C_Inv3);*/
|
|
xL2 = MTH_M_xMul( MTH_M_xAdd( xCosTeta , MTH_M_xMul( MTH_C_Sqrt3 , xSinTeta ) ) , MTH_C_Inv2 );
|
|
xL2 = MTH_M_xMulSubMul( xM , xL2 , xP , MTH_C_Inv3 );
|
|
/*xL3=xM*((xCosTeta-MTH_C_Sqrt3*xSinTeta)*MTH_C_Inv2)-(xP*MTH_C_Inv3);*/
|
|
xL3 = xL2; /* ?????????*/
|
|
|
|
xRet = MTH_M_xAbs(xL1);
|
|
xRet = MTH_M_xMax(xRet, MTH_M_xAbs(xL2));
|
|
xRet = MTH_M_xMax(xRet, MTH_M_xAbs(xL3));
|
|
}
|
|
else
|
|
{
|
|
/* We suppose we have a diagonal matrix */
|
|
xRet = MTH_M_xAbs(stScaleMatrix.stCol_0.xX);
|
|
xRet = MTH_M_xMax(xRet, MTH_M_xAbs(stScaleMatrix.stCol_1.xY));
|
|
xRet = MTH_M_xMax(xRet, MTH_M_xAbs(stScaleMatrix.stCol_2.xZ));
|
|
}
|
|
return xRet;
|
|
#endif
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vPrintUsedStaticMemory
|
|
// Display the information about the memory used
|
|
----------------------------------------------------------------------------//
|
|
// Methods :
|
|
----------------------------------------------------------------------------//
|
|
// Input :
|
|
// Output :
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : Aug 97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
|
|
#ifndef RETAIL
|
|
void POS_fn_vPrintUsedStaticMemory()
|
|
{
|
|
Mmg_M_PrintUsedStaticMemoryInModule(Pos);
|
|
}
|
|
#endif /* RETAIL */
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vQuatToMat
|
|
// Takes 4 reals transform them into a rotation matrix
|
|
----------------------------------------------------------------------------//
|
|
// Methods : See Advanced animation & rendering techniques p363
|
|
----------------------------------------------------------------------------//
|
|
// Input : _xX : Axis X
|
|
// _xY : Axis Y
|
|
// _xZ : Axis Z
|
|
// _xW : cos of the angle
|
|
// Output : _p_stMatrix : pointer to the MTH matrix
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : Jul 97 Author: Alain Robin
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
void POS_fn_vQuatToMat(MTH3D_tdstMatrix* _p_stMatrix, MTH_tdxReal _xX,
|
|
MTH_tdxReal _xY,
|
|
MTH_tdxReal _xZ,
|
|
MTH_tdxReal _xW)
|
|
{
|
|
/* it's now conform to MTH specs */
|
|
MTH_tdxReal xXs,xYs,xZs,xWx,xWy,xWz,xXx,xXy,xXz,xYy,xYz,xZz;
|
|
|
|
/* it seems that all quaternions have norm 1 !!!!! */
|
|
xXs = MTH_M_xAdd(_xX,_xX);
|
|
xYs = MTH_M_xAdd(_xY,_xY);
|
|
xZs = MTH_M_xAdd(_xZ,_xZ);
|
|
|
|
xWx = MTH_M_xMul(_xW,xXs);
|
|
xWy = MTH_M_xMul(_xW,xYs);
|
|
xWz = MTH_M_xMul(_xW,xZs);
|
|
xXx = MTH_M_xMul(_xX,xXs);
|
|
xXy = MTH_M_xMul(_xX,xYs);
|
|
xXz = MTH_M_xMul(_xX,xZs);
|
|
xYy = MTH_M_xMul(_xY,xYs);
|
|
xYz = MTH_M_xMul(_xY,xZs);
|
|
xZz = MTH_M_xMul(_xZ,xZs);
|
|
|
|
(_p_stMatrix->stCol_0).xX =MTH_M_xSub(MTH_C_ONE,MTH_M_xAdd(xYy,xZz));
|
|
(_p_stMatrix->stCol_1).xX =MTH_M_xAdd(xXy,xWz);
|
|
(_p_stMatrix->stCol_2).xX =MTH_M_xSub(xXz,xWy);
|
|
|
|
(_p_stMatrix->stCol_0).xY =MTH_M_xSub(xXy,xWz);
|
|
(_p_stMatrix->stCol_1).xY =MTH_M_xSub(MTH_C_ONE,MTH_M_xAdd(xXx,xZz));
|
|
(_p_stMatrix->stCol_2).xY =MTH_M_xAdd(xYz,xWx);
|
|
|
|
(_p_stMatrix->stCol_0).xZ =MTH_M_xAdd(xXz,xWy);
|
|
(_p_stMatrix->stCol_1).xZ =MTH_M_xSub(xYz,xWx);
|
|
(_p_stMatrix->stCol_2).xZ =MTH_M_xSub(MTH_C_ONE,MTH_M_xAdd(xXx,xYy));
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------
|
|
// Description : POS_fn_vUpdateMatrixType
|
|
// Recompute the real type of the matrix
|
|
----------------------------------------------------------------------------//
|
|
// Input : _hMatrix : Handle to the matrix
|
|
// Output :
|
|
----------------------------------------------------------------------------//
|
|
// Creation date : Jul 98 Author: Marc Trabucato
|
|
----------------------------------------------------------------------------//
|
|
// Modifications :
|
|
// Modification date : Modification author :
|
|
----------------------------------------------------------------------------*/
|
|
|
|
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
|
|
void POS_fn_vUpdateMatrixType(POS_tdxHandleToPosition _hMatrix)
|
|
{
|
|
/* update type*/
|
|
if (_hMatrix->ulType != POS_C_ulIdentityMatrixFlag )
|
|
{
|
|
if (!MTH3D_M_bMatchMatrix(&(_hMatrix->stRotationMatrix),&(_hMatrix->stTransformMatrix),POS_M_xEpsilonEqual))
|
|
{ /* transform matrix differs from rotation matrix*/
|
|
_hMatrix->ulType = POS_C_ulCompleteMatrixFlag;
|
|
}
|
|
else if (!POS_M_bIsMatrixIdentity(&(_hMatrix->stRotationMatrix)))
|
|
{ /* rotation matrix is not identity*/
|
|
_hMatrix->ulType = POS_C_ulRotationMatrixFlag;
|
|
}
|
|
else if (!POS_M_bIsVectorNull(&(_hMatrix)->stTranslationVector))
|
|
{ /* translation vector is not null*/
|
|
_hMatrix->ulType = POS_C_ulTranslationMatrixFlag;
|
|
}
|
|
else
|
|
{
|
|
_hMatrix->ulType = POS_C_ulIdentityMatrixFlag;
|
|
}
|
|
}
|
|
|
|
/* clean values*/
|
|
switch(_hMatrix->ulType)
|
|
{
|
|
case POS_C_ulIdentityMatrixFlag:
|
|
MTH3D_M_vNullVector(&(_hMatrix)->stTranslationVector);
|
|
/* no break here !!!*/
|
|
case POS_C_ulTranslationMatrixFlag:
|
|
MTH3D_M_vSetIdentityMatrix(&(_hMatrix->stRotationMatrix));
|
|
/* no break here !!!*/
|
|
case POS_C_ulRotationMatrixFlag:
|
|
MTH3D_M_vCopyMatrix(&(_hMatrix->stTransformMatrix),&(_hMatrix->stRotationMatrix));
|
|
return;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
|
|
|
|
|
|
/*----------------------------------------------------------------------------
|
|
----------------------------------------------------------------------------*/
|
|
#ifdef __cplusplus
|
|
} /*extern "C"*/
|
|
#endif
|