/*
 *=======================================================================================
 * Name        : MecOpt.c
 * Author      : J Thénoz               Date : 01/05/98
 * Description : Mechanic options
 *=======================================================================================
 */
 
 
 
#include "MEC/MECInc.h"
#include "MEC/DnmDynam.h"
#include "MEC/ParsData.h"
#include "MEC/DNMSurSt.h"
#include "MEC/mbase.h"
#include "Extheade.h"
#include "COL.h"
#include "MEC/MecDef.h"
#include "MEC/MECOpt.h"
#include "MEC/MECTools.h"
 
 
 
 
/* private*/
/* Inertia */
void MEC_fn_vInertiaCoeff 
(
	struct DNM_stDynamics *p_stDynamic,
	MTH_tdxReal xDT,
	MTH_tdxReal xInertiaX,
	MTH_tdxReal xInertiaY,
	MTH_tdxReal xInertiaZ,
	POS_tdxHandleToPosition hMatrix,
	POS_tdxHandleToPosition hInvMatrix
)
{
	MTH3D_tdstVector stTranslation, stPreviousTranslation, stMove;
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stCurrentPosition, *p_stPreviousPosition;
	MTH3D_tdstVector *p_stSpeed;
 
 
	MTH_tdxReal Ux, Uy, Uz;
	MTH_tdxReal Vx, Vy, Vz;
 
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
 
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	p_stPreviousPosition = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
	MTH3D_M_vSubVector (&stTranslation, p_stCurrentPosition, p_stPreviousPosition);
 
	MEC_fn_vInvertMatrix (p_stDynamic,hInvMatrix,hMatrix);
 
	p_stSpeed = DNM_M_pDynamicsGetPreviousSpeed ( p_stDynamic );
	MTH3D_M_vMulScalarVector (&stPreviousTranslation,xDT,p_stSpeed);
	/* sub translation part without inertia */
	if (!DNM_M_bDynamicsIsMechanicChange(p_stDynamic))
		MTH3D_M_vSubVector (&stPreviousTranslation, &stPreviousTranslation, DNM_M_pDynamicsGetInertiaTranslation(p_stDynamic));
 
	POS_fn_vRotateVector ( &stTranslation, hInvMatrix, &stTranslation );
	POS_fn_vRotateVector ( &stPreviousTranslation, hInvMatrix, &stPreviousTranslation );
 
	MTH3D_M_vGetVectorElements ( &Ux, &Uy, &Uz, &stTranslation );
	MTH3D_M_vGetVectorElements ( &Vx, &Vy, &Vz, &stPreviousTranslation );
 
	Vx = MTH_M_xAdd ( MTH_M_xMul(xInertiaX,MTH_M_xSub(Vx,Ux)),Ux);
	Vy = MTH_M_xAdd ( MTH_M_xMul(xInertiaY,MTH_M_xSub(Vy,Uy)),Uy);
	Vz = MTH_M_xAdd ( MTH_M_xMul(xInertiaZ,MTH_M_xSub(Vz,Uz)),Uz);
	MTH3D_M_vSetVectorElements ( &stMove, Vx, Vy, Vz );
 
	POS_fn_vRotateVector ( &stMove, hMatrix, &stMove );
	MEC_fn_vRoundVector (&stMove,C_xDNM_REAL_PRECISION);
	MTH3D_M_vAddVector ( p_stCurrentPosition, p_stPreviousPosition, &stMove );
}
 
 
/* Gravity */
void MEC_fn_vGravitySpeed
(
	struct DNM_stDynamics *p_stDynamic,
	MTH_tdxReal xDT
)
{
	MTH_tdxReal xGravity = DNM_M_xDynamicsGetGravityFactor(p_stDynamic);
	MTH3D_tdstVector *p_stPreviousTranslation, *p_stCurrentTranslation;
	MTH3D_tdstVector *p_stSpeed;
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH_tdxReal xZ;
 
	/* if gravity option is on and there's an obstacle and no slide -> no gravity */
	if (DNM_M_bDynamicsIsNoGravity(p_stDynamic)) return; 
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
	
	p_stPreviousTranslation = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
	p_stCurrentTranslation = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	
	p_stSpeed = DNM_M_pDynamicsGetPreviousSpeed ( p_stDynamic );
 
	xGravity = MTH_M_xMul ( xGravity, MTH_C_MinusONE );
	xGravity = MTH_M_xMul ( xGravity, xDT );
	xGravity = MTH_M_xAdd ( xGravity, MTH3D_M_xGetZofVector(p_stSpeed) );
	xGravity = MTH_M_xMul ( xGravity, xDT );
 
/*	
	if (DNM_M_bDynamicsIsAdvancedSize(p_stDynamic))
	{
		xGravity = MTH_M_xSub ( xGravity, MTH3D_M_xGetZofVector(DNM_M_pDynamicsGetInertiaTranslation(p_stDynamic)) );
	} 
*/
 
	xZ = MTH_M_xAdd (MTH3D_M_xGetZofVector(p_stPreviousTranslation),xGravity);
	MTH3D_M_vSetZofVector ( p_stCurrentTranslation, xZ );
}
 
 
/* inertia */
void MEC_fn_vInertia 
(
	struct DNM_stDynamics *p_stDynamic,
	MTH_tdxReal xDT,
	POS_tdxHandleToPosition hMatrix,
	POS_tdxHandleToPosition hInvMatrix
)
{
	if (DNM_M_bDynamicsIsInertia(p_stDynamic))
	{
		MTH_tdxReal xInertiaX = MEC_fn_xConvertScale(DNM_M_xDynamicsGetInertiaFactorX(p_stDynamic));
		MTH_tdxReal xInertiaY = MEC_fn_xConvertScale(DNM_M_xDynamicsGetInertiaFactorY(p_stDynamic));
		MTH_tdxReal xInertiaZ = MEC_fn_xConvertScale(DNM_M_xDynamicsGetInertiaFactorZ(p_stDynamic));
		MEC_fn_vInertiaCoeff (p_stDynamic,xDT,xInertiaX,xInertiaY,xInertiaZ,hMatrix,hInvMatrix);
	}
	/* we are walking on a sliding ground -> slide by inertia */
	if (DNM_M_bDynamicsIsOnGround (p_stDynamic))
	{
		if (DNM_M_bDynamicsIsSlidingGround(p_stDynamic))
		{
 
			MTH_tdxReal xSlide = DNM_M_xDynamicsGetPreviousSlide (p_stDynamic);
			MTH_tdxReal xSildeFactorX, xSildeFactorY, xSildeFactorZ;
		
			xSlide = MEC_fn_xConvertScale (xSlide);
 
			xSildeFactorX = MTH_M_xMul ( xSlide, MEC_fn_xConvertScale (DNM_M_xDynamicsGetSlideFactorX(p_stDynamic)) );
			xSildeFactorY = MTH_M_xMul ( xSlide, MEC_fn_xConvertScale (DNM_M_xDynamicsGetSlideFactorY(p_stDynamic)) );
			xSildeFactorZ = MTH_M_xMul ( xSlide, MEC_fn_xConvertScale (DNM_M_xDynamicsGetSlideFactorZ(p_stDynamic)) );
	
			MEC_fn_vInertiaCoeff (p_stDynamic,xDT,xSildeFactorX,xSildeFactorY,xSildeFactorZ,hMatrix,hInvMatrix);
		}
	}
}
 
/* Tilt */
void MEC_fn_vAddTiltSpeed
(
	struct DNM_stDynamics *p_stDynamic,   
	MTH_tdxReal xDt
)
{
	MTH3D_tdstVector *p_stPreviousI, *p_stPreviousJ, *p_stPreviousK;
	MTH3D_tdstVector *p_stCurrentI, *p_stCurrentJ, *p_stCurrentK;
	MTH3D_tdstVector stInitialTranslation, stVector;
	MTH3D_tdstVector stOriginTranslation;
	MTH3D_tdstVector *p_stCurrentTranslation;
	MTH3D_tdstVector *p_stPreviousTranslation;
	MTH_tdxReal xDotProd, xAngle;
	MTH_tdxReal xTiltAngle, xInitTiltAngle;
	MTH_tdxReal xInertia;
 
	POS_tdxHandleToPosition hCurrentMatrix; 
	POS_tdxHandleToPosition hPreviousMatrix;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
 
	xInertia = DNM_M_xDynamicsGetTiltInertia ( p_stDynamic );
 
	POS_fn_vGetRotationVector ( hPreviousMatrix, &p_stPreviousI, &p_stPreviousJ, &p_stPreviousK);
	POS_fn_vGetRotationVector ( hCurrentMatrix, &p_stCurrentI, &p_stCurrentJ, &p_stCurrentK);
 
	xDotProd = MTH3D_M_xDotProductVector ( p_stPreviousI, p_stCurrentJ );
	xAngle = MTH_M_xMul ( MTH_M_xMul ( xDotProd, DNM_M_xDynamicsGetTiltIntensity(p_stDynamic)) , MTH3D_M_xNormVector(DNM_M_pDynamicsGetPreviousSpeed(p_stDynamic)) );
	xAngle = MTH_M_xDiv ( xAngle, MTH_M_xDoubleToReal(10) );
 
	xTiltAngle = DNM_M_xDynamicsGetTiltAngle(p_stDynamic);
	xInitTiltAngle = xTiltAngle;
 
	xTiltAngle = MTH_M_xAdd ( MTH_M_xMul(xTiltAngle,xInertia), MTH_M_xMul(xAngle,MTH_M_xSub(MTH_C_ONE,xInertia)) );
	xAngle = MTH_M_xSub ( xTiltAngle, xInitTiltAngle );
	
	if (MTH_M_bDifferent ( xAngle, MTH_C_ZERO ) )
	{
		DNM_M_vDynamicsSetTiltAngle ( p_stDynamic, xTiltAngle );
 
		MTH3D_M_vSetBaseJVector ( &stVector );
 
		MTH3D_M_vMulScalarVector ( &stOriginTranslation, DNM_M_xDynamicsGetTiltOrigin(p_stDynamic), p_stCurrentK );
 
		p_stCurrentTranslation = POS_fn_p_stGetTranslationVector (hCurrentMatrix);
		p_stPreviousTranslation = POS_fn_p_stGetTranslationVector (hPreviousMatrix);
		MTH3D_M_vCopyVector (&stInitialTranslation,p_stCurrentTranslation);
 
		MTH3D_M_vCopyVector (p_stCurrentTranslation,&stOriginTranslation);
		POS_fn_vRotatePositionAroundAxis (hCurrentMatrix, &stVector, xAngle);
		MTH3D_M_vSubVector (&stVector, p_stCurrentTranslation, &stOriginTranslation);
 
		MTH3D_M_vAddVector (p_stCurrentTranslation, &stInitialTranslation, &stVector);
	}
}
 
void MEC_fn_vCancelTilt ( struct DNM_stDynamics *p_stDynamic )
{
	MTH3D_tdstVector stInitialTranslation, stVector;
	MTH3D_tdstVector *p_stCurrentI, *p_stCurrentJ, *p_stCurrentK;
	MTH3D_tdstVector stOriginTranslation;
	MTH3D_tdstVector *p_stTranslation;
	POS_tdxHandleToPosition hCurrentMatrix; 
	MTH_tdxReal xAngle;
 
	xAngle = DNM_M_xDynamicsGetTiltAngle(p_stDynamic);
	if (MTH_M_bEqual(xAngle,MTH_C_ZERO)) return;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	POS_fn_vGetRotationVector ( hCurrentMatrix, &p_stCurrentI, &p_stCurrentJ, &p_stCurrentK);
	p_stTranslation = POS_fn_p_stGetTranslationVector (hCurrentMatrix);
	MTH3D_M_vSetBaseJVector ( &stVector );
 
	MTH3D_M_vMulScalarVector ( &stOriginTranslation, DNM_M_xDynamicsGetTiltOrigin(p_stDynamic), p_stCurrentK );
	MTH3D_M_vCopyVector (&stInitialTranslation,p_stTranslation);
	MTH3D_M_vSubVector (p_stTranslation, p_stTranslation, &stOriginTranslation);
	POS_fn_vRotatePositionAroundAxis (hCurrentMatrix, &stVector, MTH_M_xNeg(xAngle));
	POS_fn_vSetTranslationVector (hCurrentMatrix,&stInitialTranslation);
 
	DNM_M_vDynamicsSetTiltAngle (p_stDynamic,MTH_C_ZERO);
}
 
 
 
void MEC_fn_vImposeSpeed
(
	struct DNM_stDynamics *p_stDynamic,
	MTH_tdxReal xDT,
	POS_tdxHandleToPosition hMatrix,
	POS_tdxHandleToPosition hInvMatrix
)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
	MTH3D_tdstVector stTranslation;
	MTH3D_tdstVector* pstImposeSpeed;
	MTH3D_tdstVector stImposeMove;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
 
	pstImposeSpeed = DNM_M_pDynamicsGetImposeSpeed ( p_stDynamic );
	MTH3D_M_vMulScalarVector ( &stImposeMove, xDT, pstImposeSpeed );
 
	p_stPreviousPosition = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	MTH3D_M_vSubVector ( &stTranslation, p_stCurrentPosition, p_stPreviousPosition);
 
 
	if (DNM_M_bDynamicsIsRelativeImposeSpeed(p_stDynamic) && !MTH3D_M_bIsNullVector(&stTranslation))
	{
		MEC_fn_vInvertMatrix (p_stDynamic,hInvMatrix,hMatrix);
		POS_fn_vRotateVector ( &stTranslation, hInvMatrix, &stTranslation );
	}
 
	if (DNM_M_bDynamicsIsImposeSpeedX(p_stDynamic)) MTH3D_M_vSetXofVector (&stTranslation,MTH3D_M_xGetXofVector(&stImposeMove));
	if (DNM_M_bDynamicsIsImposeSpeedY(p_stDynamic)) MTH3D_M_vSetYofVector (&stTranslation,MTH3D_M_xGetYofVector(&stImposeMove));
	if (DNM_M_bDynamicsIsImposeSpeedZ(p_stDynamic)) MTH3D_M_vSetZofVector (&stTranslation,MTH3D_M_xGetZofVector(&stImposeMove));
 
	if (DNM_M_bDynamicsIsRelativeImposeSpeed(p_stDynamic) && !MTH3D_M_bIsNullVector(&stTranslation)) POS_fn_vRotateVector ( &stTranslation, hMatrix, &stTranslation );
 
	MTH3D_M_vAddVector ( p_stCurrentPosition, p_stPreviousPosition, &stTranslation );
}
 
/*add speed*/
void MEC_fn_vAddSpeed 
(
	struct DNM_stDynamics *p_stDynamic,
	MTH_tdxReal xDT,
	POS_tdxHandleToPosition hMatrix,
	POS_tdxHandleToPosition hInvMatrix
)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
	MTH3D_tdstVector stTranslation;
	MTH3D_tdstVector* pstAddSpeed;
	MTH3D_tdstVector stAddMove;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix =  DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
 
	pstAddSpeed = DNM_M_pDynamicsGetAddSpeed ( p_stDynamic );
	MTH3D_M_vMulScalarVector ( &stAddMove, xDT, pstAddSpeed );
 
	p_stPreviousPosition = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	MTH3D_M_vSubVector ( &stTranslation, p_stCurrentPosition, p_stPreviousPosition);
 
 
	if (DNM_M_bDynamicsIsRelativeAddSpeed(p_stDynamic) && !MTH3D_M_bIsNullVector(&stTranslation))
	{
		MEC_fn_vInvertMatrix (p_stDynamic,hInvMatrix,hMatrix);
		POS_fn_vRotateVector ( &stTranslation, hInvMatrix, &stTranslation );
	}
 
	if (DNM_M_bDynamicsIsAddSpeedX(p_stDynamic)) MTH3D_M_vSetXofVector (&stTranslation,MTH_M_xAdd(MTH3D_M_xGetXofVector(&stTranslation),MTH3D_M_xGetXofVector(&stAddMove)));
	if (DNM_M_bDynamicsIsAddSpeedY(p_stDynamic)) MTH3D_M_vSetYofVector (&stTranslation,MTH_M_xAdd(MTH3D_M_xGetYofVector(&stTranslation),MTH3D_M_xGetYofVector(&stAddMove)));
	if (DNM_M_bDynamicsIsAddSpeedZ(p_stDynamic)) MTH3D_M_vSetZofVector (&stTranslation,MTH_M_xAdd(MTH3D_M_xGetZofVector(&stTranslation),MTH3D_M_xGetZofVector(&stAddMove)));
 
	if (DNM_M_bDynamicsIsRelativeAddSpeed(p_stDynamic) && !MTH3D_M_bIsNullVector(&stTranslation)) POS_fn_vRotateVector ( &stTranslation, hMatrix, &stTranslation );
 
	MTH3D_M_vAddVector ( p_stCurrentPosition, p_stPreviousPosition, &stTranslation );
}
 
/* propose speed*/
void MEC_fn_vProposeSpeed
(
	struct DNM_stDynamics *p_stDynamic,
	MTH_tdxReal xDT,
	POS_tdxHandleToPosition hMatrix,
	POS_tdxHandleToPosition hInvMatrix
)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
	MTH3D_tdstVector stTranslation;
	MTH3D_tdstVector* pstProposeSpeed;
	MTH3D_tdstVector stProposeMove;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
 
	pstProposeSpeed = DNM_M_pDynamicsGetProposeSpeed ( p_stDynamic );
	MTH3D_M_vMulScalarVector ( &stProposeMove, xDT, pstProposeSpeed );
 
	p_stPreviousPosition = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	MTH3D_M_vSubVector ( &stTranslation, p_stCurrentPosition, p_stPreviousPosition);
	
	if (DNM_M_bDynamicsIsRelativeProposeSpeed(p_stDynamic) && !MTH3D_M_bIsNullVector(&stTranslation))
	{
		MEC_fn_vInvertMatrix (p_stDynamic,hInvMatrix,hMatrix);
		POS_fn_vRotateVector ( &stTranslation, hInvMatrix, &stTranslation );
	}
 
	if (DNM_M_bDynamicsIsProposeSpeedX(p_stDynamic)) MTH3D_M_vSetXofVector (&stTranslation,MTH3D_M_xGetXofVector(&stProposeMove));
	if (DNM_M_bDynamicsIsProposeSpeedY(p_stDynamic)) MTH3D_M_vSetYofVector (&stTranslation,MTH3D_M_xGetYofVector(&stProposeMove));
	if (DNM_M_bDynamicsIsProposeSpeedZ(p_stDynamic)) MTH3D_M_vSetZofVector (&stTranslation,MTH3D_M_xGetZofVector(&stProposeMove));
 
	if (DNM_M_bDynamicsIsRelativeProposeSpeed(p_stDynamic) && !MTH3D_M_bIsNullVector(&stTranslation)) POS_fn_vRotateVector ( &stTranslation, hMatrix, &stTranslation );
 
	MTH3D_M_vAddVector ( p_stCurrentPosition, p_stPreviousPosition, &stTranslation );
}
 
/* stream speed*/
void MEC_fn_vAddStream (	
	struct DNM_stDynamics *p_stDynamic,
	MTH_tdxReal xDT,
	ACP_tdxBool bSlide
)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stCurrentPosition;
	MTH3D_tdstVector stTranslation;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
 
	MEC_fn_vComputeStream ( &stTranslation, p_stDynamic, xDT, bSlide);
 
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	MTH3D_M_vAddVector ( p_stCurrentPosition, p_stCurrentPosition, &stTranslation );
}
 
/* impose rotation matrix*/
void MEC_fn_vImposeRotation	( struct DNM_stDynamics *p_stDynamic )
{
	MTH3D_tdstMatrix* p_stImposeRotationMatrix;
	POS_tdxHandleToPosition hCurrentMatrix;
	MTH3D_tdstVector stI, stJ, stK;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	p_stImposeRotationMatrix = DNM_M_p_stDynamicsGetImposeRotationMatrix (p_stDynamic);
 
	/* rotation */
	MTH3D_M_vGetVectorsInMatrix ( &stI, &stJ, &stK, p_stImposeRotationMatrix );
	POS_fn_vSetRotationMatrix ( hCurrentMatrix, &stI, &stJ, &stK );
}
 
void MEC_fn_vAdditionnalTranslation ( struct DNM_stDynamics *p_stDynamic )
{
	POS_tdxHandleToPosition hCurrentMatrix;
	MTH3D_tdstVector *p_stTranslation;
	MTH3D_tdstVector* p_stAdditionnalTranslation;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	p_stAdditionnalTranslation = DNM_M_pDynamicsGetAddTranslation (p_stDynamic);
	
	p_stTranslation = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	MTH3D_M_vAddVector ( p_stTranslation, p_stTranslation, p_stAdditionnalTranslation);
}
 
 
 
void MEC_fn_vPositionLimit ( struct DNM_stDynamics *p_stDynamic, MTH3D_tdstVector* p_stSafePreviousPosition )
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
 
	if (!DNM_M_bDynamicsIsLimit(p_stDynamic)) return;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
	p_stPreviousPosition = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
 
 
	if (DNM_M_bDynamicsIsLimitX(p_stDynamic))
	{
		MTH_tdxReal	xXMin, xXMax, xX;
		xXMin = MTH_M_xMin ( MTH3D_M_xGetXofVector (p_stPreviousPosition), MTH3D_M_xGetXofVector (p_stCurrentPosition) );
		xXMax = MTH_M_xMax ( MTH3D_M_xGetXofVector (p_stPreviousPosition), MTH3D_M_xGetXofVector (p_stCurrentPosition) );
		xX = MTH3D_M_xGetXofVector ( DNM_M_pDynamicsGetLimit(p_stDynamic) );
	
		if ( MTH_M_bLessEqual(xXMin,xX)  && MTH_M_bLess(xX,xXMax) )
		{
			MTH3D_M_vSetXofVector (p_stCurrentPosition,MTH3D_M_xGetXofVector(p_stSafePreviousPosition));
		}
	}
 
	if (DNM_M_bDynamicsIsLimitY(p_stDynamic))
	{
		MTH_tdxReal	xYMin, xYMax, xY;
		xYMin = MTH_M_xMin ( MTH3D_M_xGetYofVector (p_stPreviousPosition), MTH3D_M_xGetYofVector (p_stCurrentPosition) );
		xYMax = MTH_M_xMax ( MTH3D_M_xGetYofVector (p_stPreviousPosition), MTH3D_M_xGetYofVector (p_stCurrentPosition) );
		xY = MTH3D_M_xGetYofVector ( DNM_M_pDynamicsGetLimit(p_stDynamic) );
	
		if ( MTH_M_bLessEqual (xYMin,xY)  && MTH_M_bLess (xY,xYMax) )
		{
			MTH3D_M_vSetYofVector (p_stCurrentPosition,MTH3D_M_xGetYofVector(p_stSafePreviousPosition));
		}
	}
 
	if (DNM_M_bDynamicsIsLimitZ(p_stDynamic))
	{
		MTH_tdxReal	xZMin, xZMax, xZ;
		xZMin = MTH_M_xMin ( MTH3D_M_xGetZofVector (p_stPreviousPosition), MTH3D_M_xGetZofVector (p_stCurrentPosition) );
		xZMax = MTH_M_xMax ( MTH3D_M_xGetZofVector (p_stPreviousPosition), MTH3D_M_xGetZofVector (p_stCurrentPosition) );
		xZ = MTH3D_M_xGetZofVector ( DNM_M_pDynamicsGetLimit(p_stDynamic) );
	
		if ( MTH_M_bLessEqual(xZMin,xZ)  && MTH_M_bLess(xZ,xZMax) )
		{
			MTH3D_M_vSetZofVector (p_stCurrentPosition,MTH3D_M_xGetZofVector(p_stSafePreviousPosition));
		}
	}
}
 
 
/* speed limit */
void MEC_fn_vSpeedLimit
(
	struct DNM_stDynamics *p_stDynamic,
	MTH_tdxReal xDT,
	POS_tdxHandleToPosition hMatrix,
	POS_tdxHandleToPosition hInvMatrix
)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
	MTH3D_tdstVector stMove;
	MTH3D_tdstVector stCorrectMove;
	MTH3D_tdstVector stMaxMove;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
 
	/* global axis */
	p_stPreviousPosition = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	MTH3D_M_vSubVector ( &stMove, p_stCurrentPosition, p_stPreviousPosition );
 
	/* local axis */
	MEC_fn_vInvertMatrix (p_stDynamic,hInvMatrix,hMatrix);
	POS_fn_vRotateVector ( &stMove, hInvMatrix, &stMove );
 
	MTH3D_M_vCopyVector ( &stMaxMove, DNM_M_pDynamicsGetMaxSpeed(p_stDynamic) );
	MTH3D_M_vMulScalarVector ( &stMaxMove, xDT, &stMaxMove );
	MTH3D_M_vCopyVector ( &stCorrectMove, &stMove );
 
	if (MTH_M_bGreater(MTH_M_xAbs(MTH3D_M_xGetXofVector(&stMove)),MTH3D_M_xGetXofVector(&stMaxMove)))
		MTH3D_M_vSetXofVector ( &stCorrectMove, MTH_M_xMul ( MTH3D_M_xGetXofVector(&stMaxMove), MTH_M_xSign(MTH3D_M_xGetXofVector(&stMove))) );
	
	if (MTH_M_bGreater(MTH_M_xAbs(MTH3D_M_xGetYofVector(&stMove)),MTH3D_M_xGetYofVector(&stMaxMove)))
		MTH3D_M_vSetYofVector ( &stCorrectMove, MTH_M_xMul ( MTH3D_M_xGetYofVector(&stMaxMove), MTH_M_xSign(MTH3D_M_xGetYofVector(&stMove))) );

	if (MTH_M_bGreater(MTH_M_xAbs(MTH3D_M_xGetZofVector(&stMove)),MTH3D_M_xGetZofVector(&stMaxMove)))
		MTH3D_M_vSetZofVector ( &stCorrectMove, MTH_M_xMul ( MTH3D_M_xGetZofVector(&stMaxMove), MTH_M_xSign(MTH3D_M_xGetZofVector(&stMove))) );

	/* global axis */
	POS_fn_vRotateVector ( &stCorrectMove, hMatrix, &stCorrectMove );
 
	MTH3D_M_vAddVector ( p_stCurrentPosition, p_stPreviousPosition, &stCorrectMove );
}
 
void MEC_fn_vAdjustScaleData 
(
	struct DNM_stDynamics *p_stDynamic,
	POS_tdxHandleToPosition hMatrix,
	POS_tdxHandleToPosition hInvMatrix
)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
	MTH3D_tdstVector stTranslation;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
 
	/* invert matrix */
	MEC_fn_vInvertMatrix (p_stDynamic,hInvMatrix,hMatrix);
 
	/* animation */
	p_stPreviousPosition = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	MTH3D_M_vSubVector ( &stTranslation, p_stCurrentPosition, p_stPreviousPosition);
	
	POS_fn_vRotateVector ( &stTranslation, hInvMatrix, &stTranslation );
	MEC_fn_vAdjustScaleVector ( p_stDynamic, &stTranslation );
	POS_fn_vRotateVector ( &stTranslation, hMatrix, &stTranslation );
 
	MTH3D_M_vAddVector ( p_stCurrentPosition, p_stPreviousPosition, &stTranslation );
 
	/* impose */
	MTH3D_M_vCopyVector ( &stTranslation, DNM_M_pDynamicsGetImposeSpeed(p_stDynamic) );
	if (!DNM_M_bDynamicsIsRelativeImposeSpeed(p_stDynamic)) POS_fn_vRotateVector ( &stTranslation, hInvMatrix, &stTranslation );
	MEC_fn_vAdjustScaleVector ( p_stDynamic, &stTranslation );
	if (!DNM_M_bDynamicsIsRelativeImposeSpeed(p_stDynamic)) POS_fn_vRotateVector ( &stTranslation, hMatrix, &stTranslation );
	DNM_M_vDynamicsSetImposeSpeed (p_stDynamic,&stTranslation)
	
	/* propose */
	MTH3D_M_vCopyVector ( &stTranslation, DNM_M_pDynamicsGetProposeSpeed(p_stDynamic) );
	if (!DNM_M_bDynamicsIsRelativeProposeSpeed(p_stDynamic)) POS_fn_vRotateVector ( &stTranslation, hInvMatrix, &stTranslation );
	MEC_fn_vAdjustScaleVector ( p_stDynamic, &stTranslation );
	if (!DNM_M_bDynamicsIsRelativeImposeSpeed(p_stDynamic)) POS_fn_vRotateVector ( &stTranslation, hMatrix, &stTranslation );
	DNM_M_vDynamicsSetProposeSpeed (p_stDynamic,&stTranslation)
 
	/* add */
	MTH3D_M_vCopyVector ( &stTranslation, DNM_M_pDynamicsGetAddSpeed(p_stDynamic) );
	if (!DNM_M_bDynamicsIsRelativeAddSpeed(p_stDynamic)) POS_fn_vRotateVector ( &stTranslation, hInvMatrix, &stTranslation );
	MEC_fn_vAdjustScaleVector ( p_stDynamic, &stTranslation );
	if (!DNM_M_bDynamicsIsRelativeImposeSpeed(p_stDynamic)) POS_fn_vRotateVector ( &stTranslation, hMatrix, &stTranslation );
	DNM_M_vDynamicsSetAddSpeed (p_stDynamic,&stTranslation)
}