/*
 *=======================================================================================
 * Name        : MecTools.c
 * Author      : J Thénoz               Date : 01/05/98
 * Description : Mechanic tools funstions
 *=======================================================================================
 */
 
 
#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/MECTOOLS.h"
 
 
#include "PCS.h" /* Physical Collide Set */
#include "SCT.h" /* SeCTor*/
#include "FIL.h" /* SeCTor*/
#include "PRT.h" /* PaRTicules*/
#include "GAM.h" /* GAMe*/



/*
 * Normal
 */
 
/* normal of a wall ? */
ACP_tdxBool MEC_fn_bWallNormal (struct DNM_stDynamics *p_stDynamic, MTH3D_tdstVector* p_stNormal)
{
	MTH_tdxReal xDotProduct;
	MTH3D_tdstVector stVerticalVector;
 
	MTH3D_M_vSetVectorElements (&stVerticalVector, MTH_C_ZERO, MTH_C_ZERO, MTH_C_ONE);
	xDotProduct = MTH3D_M_xDotProductVector ( p_stNormal, &stVerticalVector );
	if ( (xDotProduct>MEC_fn_xWallLimit(p_stDynamic)) && (xDotProduct<MEC_fn_xGroundLimit(p_stDynamic)) )
		return TRUE;
	else return FALSE;
}
 
/* normal of a ground ? */
ACP_tdxBool MEC_fn_bGroundNormal (struct DNM_stDynamics *p_stDynamic, MTH3D_tdstVector* p_stNormal)
{
	MTH_tdxReal xDotProduct;
	MTH3D_tdstVector stVerticalVector;
 
	MTH3D_M_vSetVectorElements (&stVerticalVector, MTH_C_ZERO, MTH_C_ZERO, MTH_C_ONE);
	xDotProduct = MTH3D_M_xDotProductVector ( p_stNormal, &stVerticalVector );
	if (xDotProduct>=MEC_fn_xGroundLimit(p_stDynamic))
		return TRUE;
	else return FALSE;
}
 
 
/*
 * Obstacle
 */
 
 
/* this obstacle is a ceil ? */
ACP_tdxBool MEC_fn_bCeilObstacle (struct DNM_stDynamics *p_stDynamic, DNM_tdstMecObstacle *p_stObstacle)
{
	MTH_tdxReal xDotProduct;
	MTH3D_tdstVector stVerticalVector;
	MTH3D_tdstVector *p_stNormalVector;
 
	MTH3D_M_vSetVectorElements (&stVerticalVector, MTH_C_ZERO, MTH_C_ZERO, MTH_C_ONE);
	p_stNormalVector = DNM_M_p_stObstacleGetNorm (p_stObstacle);
 
	xDotProduct = MTH3D_M_xDotProductVector ( p_stNormalVector, &stVerticalVector );
	if (xDotProduct<MEC_fn_xWallLimit(p_stDynamic))
		return TRUE;
	else return FALSE;
}
 
 

#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
ACP_tdxBool MEC_fn_bIsStatic (long lObstacle, DNM_tdstMecObstacle *p_stObstacle)
{
	long lCounter;
	DNM_tdstMecObstacle *p_stTmpObstacle;
 
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stTmpObstacle = p_stObstacle+lCounter;
		if (MEC_fn_bIsObstacleValid(p_stTmpObstacle) && MTH_M_bLess(DNM_M_xObstacleGetRate (p_stObstacle+lCounter),MTH_C_ZERO)) return TRUE;
	}
	return FALSE;
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */

 
 
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
void MEC_fn_vAddObstacleList (long *p_lObstacle, DNM_tdstMecObstacle *p_stObstacle, long lAddObstacle, DNM_tdstMecObstacle *p_stAddObstacle)
{
	long lObstacle=0;
	DNM_tdstMecObstacle a_stObstacle [2*COL_C_xMaxNumberOfCollisions];
	long lCounter1, lCounter2;
	DNM_tdstMecObstacle *p_stObstacle1, *p_stObstacle2;
 
	lCounter2 = 0;
	p_stObstacle2 = p_stAddObstacle;
 
	for (lCounter1=0; lCounter1<*p_lObstacle; lCounter1++)
	{
		p_stObstacle1 = p_stObstacle+lCounter1;
 
		if (MEC_fn_bIsObstacleValid(p_stObstacle1))
		{
			while ( MTH_M_bGreater(DNM_M_xObstacleGetRate(p_stObstacle1),DNM_M_xObstacleGetRate(p_stObstacle2)) && lCounter2<lAddObstacle )
			{
				a_stObstacle[lObstacle] = *p_stObstacle2;
				lObstacle++;
				do lCounter2++;
				while ( !MEC_fn_bIsObstacleValid(p_stAddObstacle+lCounter2) && lCounter2<lAddObstacle );
				p_stObstacle2 = p_stAddObstacle+lCounter2;
			}
		
			a_stObstacle[lObstacle] = *p_stObstacle1;
			lObstacle++;
		}
	}
 
	for (;lCounter2<lAddObstacle; lCounter2++)
	{
		if (MEC_fn_bIsObstacleValid(p_stAddObstacle+lCounter2))
		{
			a_stObstacle[lObstacle] = p_stAddObstacle[lCounter2];
			lObstacle++;
		}
	}
 
	/* return result */
	if (lObstacle>COL_C_xMaxNumberOfCollisions) lObstacle = COL_C_xMaxNumberOfCollisions;
	memcpy ( p_stObstacle, a_stObstacle, lObstacle*sizeof(DNM_tdstMecObstacle) );
	*p_lObstacle = lObstacle;
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */
 
ACP_tdxBool MEC_fn_bCanCrashActor (DNM_tdstMecObstacle* p_stCollideObstacle)
{
	HIE_tdxHandleToSuperObject hSupObj;
 
	hSupObj = DNM_M_p_stObstacleGetObject (p_stCollideObstacle);
	if (!HIE_fn_bIsSuperObjectValid(hSupObj)) return FALSE;
	hSupObj = MEC_fn_hGetFatherActor (hSupObj);
	return (fn_ulStandardGameGetCustomBitsSO(hSupObj) & C_ulDNM_NO_CRASH_CUSTOM_BIT) != C_ulDNM_NO_CRASH_CUSTOM_BIT;
}
 
 
 
 
/*
 * Collision 
 */
 
 
/* need to call collide ? */
/* yes only if we move */
BOOL MEC_fn_bDynamicsNeedCollide (struct DNM_stDynamics *p_stDynamic)
{
	POS_tdxHandleToPosition hCurrentMatrix, hPreviousMatrix;
	MTH3D_tdstVector *p_stCurrentPosition, *p_stPreviousPosition;
	MTH3D_tdstVector *p_stCurrentI, *p_stCurrentJ, *p_stCurrentK;
	MTH3D_tdstVector *p_stPreviousI, *p_stPreviousJ, *p_stPreviousK;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	p_stPreviousPosition = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
	POS_fn_vGetRotationVector ( hCurrentMatrix, &p_stCurrentI, &p_stCurrentJ, &p_stCurrentK );
	POS_fn_vGetRotationVector ( hPreviousMatrix, &p_stPreviousI, &p_stPreviousJ, &p_stPreviousK );
 
 
	return
		!MTH3D_M_bEqualVector (p_stCurrentPosition,p_stPreviousPosition)
	||	!MTH3D_M_bEqualVector (p_stCurrentI,p_stPreviousI)
	||	!MTH3D_M_bEqualVector (p_stCurrentJ,p_stPreviousJ)
	||	!MTH3D_M_bEqualVector (p_stCurrentK,p_stPreviousK);
}
 
 
 
MTH_tdxReal MEC_fn_xGetSlide (struct DNM_stDynamics *p_stDynamic,DNM_tdstMecObstacle *p_stObstacle)
{
	MTH_tdxReal xSlide;
	GMT_tdxHandleToGameMaterial			 hGameMaterial;
	DNM_tdxHandleToMecMatCharacteristics hMatCharacteristics;
 
	hGameMaterial = hMatCharacteristics = NULL;
 
	if (!p_stObstacle) return MTH_C_ZERO;
 
	hGameMaterial = DNM_M_hObstacleGetCollidedMaterial (p_stObstacle);
	if (hGameMaterial!=NULL) hMatCharacteristics=GMT_fn_hGetMechanicsMaterial(hGameMaterial);
 
	xSlide = MTH_C_ZERO;
 
	/* type */
	if (hGameMaterial && MEC_fn_bTypeOfGameMaterial (hGameMaterial,C_xDNM_SLIDE_TYPE_GAME_MATERIAL)) xSlide = MTH_C_ONE;
 
	/* game material */
	if (hMatCharacteristics) xSlide = DNM_M_xObstacleGetSlide (p_stObstacle);
 
	/* collision control */
	if (DNM_M_bDynamicsIsCollisionControl(p_stDynamic)) xSlide = DNM_M_xDynamicsGetSlide (p_stDynamic);
 
	return xSlide;
}
 
MTH_tdxReal MEC_fn_xGetRebound (struct DNM_stDynamics *p_stDynamic,DNM_tdstMecObstacle *p_stObstacle)
{
	MTH_tdxReal xRebound;
	GMT_tdxHandleToGameMaterial			 hGameMaterial;
	DNM_tdxHandleToMecMatCharacteristics hMatCharacteristics;
 
	hGameMaterial = hMatCharacteristics = NULL;
 
	if (!p_stObstacle) return MTH_C_ZERO;
 
	hGameMaterial = DNM_M_hObstacleGetCollidedMaterial (p_stObstacle);
	if (hGameMaterial!=NULL) hMatCharacteristics=GMT_fn_hGetMechanicsMaterial(hGameMaterial);
 
	xRebound = MTH_C_ZERO;
 
	/* type */
	if (hGameMaterial && MEC_fn_bTypeOfGameMaterial (hGameMaterial,C_xDNM_REBOUND_TYPE_GAME_MATERIAL)) xRebound = MTH_C_ONE;
 
	/* game material */
	if (hMatCharacteristics) xRebound = DNM_M_xObstacleGetRebound (p_stObstacle);
 
	/* collsion control */
	if (DNM_M_bDynamicsIsCollisionControl(p_stDynamic)) xRebound = DNM_M_xDynamicsGetRebound (p_stDynamic);
 
	return xRebound;
}
 
 
void MEC_fn_vComputeCollisionTranslation ( long lObstacle, DNM_tdstMecObstacle *p_stObstacle, MTH3D_tdstVector* p_stMove )
{
	long lCounter;
	MTH_tdxReal xRate/*, xNorm*/;
	DNM_tdstMecObstacle* p_stTmpObstacle;
	MTH3D_tdstVector stVector;
 
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stTmpObstacle = p_stObstacle+lCounter;
		if (MEC_fn_bIsObstacleValid(p_stTmpObstacle))
		{
			xRate = DNM_M_xObstacleGetRate(p_stTmpObstacle);
			if (MTH_M_bGreater(xRate,MTH_C_ONE))
			{
				DNM_M_xObstacleSetRate (p_stTmpObstacle,MTH_C_ONE);

				/* adjust contact point */
				MTH3D_M_vNormalizeVector (&stVector,DNM_M_pObstacleGetZoneMove(p_stTmpObstacle));
				MTH3D_M_vMulScalarVector (&stVector,DNM_M_xObstacleGetZoneRadius(p_stTmpObstacle),&stVector);
				MTH3D_M_vAddVector (&stVector,&stVector,DNM_M_pObstacleGetZonePosition(p_stTmpObstacle));
				DNM_M_ObstacleSetContact (p_stTmpObstacle,&stVector);
			}
			else if (MTH_M_bGreaterEqual(xRate,MTH_C_ZERO))
				MTH3D_M_vMulScalarVector ( DNM_M_xObstacleGetTranslation (p_stTmpObstacle), MTH_M_xSub(MTH_C_ONE,DNM_M_xObstacleGetRate(p_stTmpObstacle)),p_stMove );	
		}
	}
}
 
void MEC_fn_vDeleteWallEdge (long lObstacle, DNM_tdstMecObstacle *p_stObstacle, DNM_tdstDynamics *p_stDynamic)
{
	long lCounter;
	long lCounter1;
	DNM_tdstMecObstacle *p_stTmpObstacle;
	DNM_tdstMecObstacle *p_stTmpObstacle1;
	MTH3D_tdstVector stVector;
	ACP_tdxBool bRemoveEdge = TRUE;
 
	/* delete wall */
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stTmpObstacle = p_stObstacle + lCounter;
		
		if (MEC_fn_bIsObstacleValid(p_stTmpObstacle))
		{
			if (MEC_fn_bWallObstacle(p_stDynamic,p_stTmpObstacle))
			{
				if ( (DNM_M_xObstacleGetCollidedEntity(p_stTmpObstacle)==COL_C_xLowEdgeEntity) )
				{
					if (!MEC_fn_bIsDoubleEdge(p_stTmpObstacle))
					{
						for (lCounter1=0; lCounter1<lObstacle; lCounter1++)
						{
							if (lCounter1!=lCounter)
							{
								p_stTmpObstacle1 = p_stObstacle + lCounter1;
								if (MEC_fn_bIsObstacleValid(p_stTmpObstacle1))
								{
									if (MEC_fn_bIsEdge(p_stTmpObstacle1) && MEC_fn_bWallObstacle(p_stDynamic,p_stTmpObstacle1))
									{
										MTH3D_M_vSubVector (&stVector,DNM_M_p_stObstacleGetContact(p_stTmpObstacle),DNM_M_p_stObstacleGetContact(p_stTmpObstacle1));		
//										if (MTH3D_M_bIsNullVector(&stVector))
										if (MEC_fn_bIsNullVector(&stVector))
											bRemoveEdge = FALSE;
									}
								}
							}
						}
 
						if (bRemoveEdge)
							MEC_fn_vInvalidateObstacle (p_stTmpObstacle);
					}
				}
			}
		}
	}
}
 
void MEC_fn_vDeleteUnderGroundWall (long lObstacle, DNM_tdstMecObstacle *p_stObstacle, DNM_tdstDynamics *p_stDynamic, DNM_tdstMecObstacle *p_stGroundObstacle )
{
	long lCounter;
	DNM_tdstMecObstacle *p_stTmpObstacle, *p_stGroundEdge;
	MTH3D_tdstVector *p_stNormal;
	MTH3D_tdstVector stContact, stVector;
	MTH_tdxReal xDotProd;
 
	if (!p_stGroundObstacle) return;

	p_stGroundEdge = NULL;
	for (lCounter=0; lCounter<lObstacle && !p_stGroundEdge; lCounter++)
	{
		p_stTmpObstacle = p_stObstacle+lCounter;
 
		if (MEC_fn_bIsObstacleValid(p_stTmpObstacle))
		{
			if 
			( 
				MEC_fn_bGroundObstacle (p_stDynamic,p_stTmpObstacle) 
			&&	MEC_fn_bIsEdge (p_stTmpObstacle) 
			&&	MTH_M_bLessEqual(MTH3D_M_xGetZofVector(DNM_M_p_stObstacleGetContact(p_stTmpObstacle)),MTH3D_M_xGetZofVector(DNM_M_p_stObstacleGetContact(p_stGroundObstacle)))
			)
			p_stGroundEdge = p_stTmpObstacle;
		}
	}


	p_stNormal = DNM_M_p_stObstacleGetNorm (p_stGroundObstacle);
	MTH3D_M_vCopyVector ( &stContact,DNM_M_p_stObstacleGetContact (p_stGroundEdge ? p_stGroundEdge : p_stGroundObstacle) );
	
	/* search walls */
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stTmpObstacle = p_stObstacle+lCounter;
 
		if (MEC_fn_bIsObstacleValid(p_stTmpObstacle))
		{
 
			if (MEC_fn_bWallObstacle(p_stDynamic,p_stTmpObstacle))
			{
				MTH3D_M_vSubVector ( &stVector, DNM_M_p_stObstacleGetContact(p_stTmpObstacle), &stContact);	
				xDotProd = MTH3D_M_xDotProductVector (&stVector,p_stNormal);

//				if ( MTH_M_bLess(xDotProd,MTH_M_xDoubleToReal(1e-5)) && !MTH3D_M_bIsNullVector(&stVector) )
				if ( MTH_M_bLessEqualZero ( xDotProd ) )
					MEC_fn_vInvalidateObstacle(p_stTmpObstacle);
			}
		}
	}
}
 
 
DNM_tdstMecObstacle* MEC_fn_pSelectGround (struct DNM_stDynamics *p_stDynamic,long lObstacle, DNM_tdstMecObstacle *p_stObstacle,ACP_tdxBool *bFirstGround)
{
	long lCounter, lGround;
	DNM_tdstMecObstacle *p_stTmpObstacle;
	MTH3D_tdstVector stVector;
	DNM_tdstMecObstacle *p_stGroundObstacle = NULL;
	MTH_tdxReal xGroundDeltaRate, xDeltaRate;

	*bFirstGround = TRUE;

	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stTmpObstacle = p_stObstacle+lCounter;
		
		if (MEC_fn_bIsObstacleValid (p_stTmpObstacle))
		{
			if (MEC_fn_bGroundObstacle(p_stDynamic,p_stTmpObstacle))
			{
				if (!p_stGroundObstacle)
				{
					p_stGroundObstacle = p_stTmpObstacle;
					lGround = lCounter;
					if (DNM_M_bDynamicsIsMechanicChange(p_stDynamic) && !MEC_fn_bIsCollide(p_stGroundObstacle))
						return p_stGroundObstacle;
					xGroundDeltaRate = MTH_M_xAbs ( MTH_M_xSub ( DNM_M_xObstacleGetRate(p_stGroundObstacle), MTH_M_xDoubleToReal(0.5) ) );
				}
				else
				{
					MTH3D_M_vSubVector ( &stVector, DNM_M_p_stObstacleGetContact(p_stGroundObstacle), DNM_M_p_stObstacleGetContact(p_stTmpObstacle) );	
					if (MTH_M_bGreater(MTH3D_M_xGetZofVector(&stVector),C_xDNM_ON_GROUND_STEP))
					{
						xDeltaRate = MTH_M_xAbs ( MTH_M_xSub ( DNM_M_xObstacleGetRate(p_stTmpObstacle), MTH_M_xDoubleToReal(0.5) ) );
						if ( MTH_M_bLess (xDeltaRate,xGroundDeltaRate) )
						{
							p_stGroundObstacle = p_stTmpObstacle;
							xGroundDeltaRate = xDeltaRate;
							*bFirstGround = FALSE;
							lGround = lCounter;
						}
					}
				}
			}
		}
	}

	return p_stGroundObstacle;
}



ACP_tdxBool MEC_fn_bDetectStep ( struct DNM_stDynamics *p_stDynamic, long lObstacle, DNM_tdstMecObstacle *p_stObstacle, DNM_tdstMecObstacle *p_stGroundObstacle, MTH3D_tdstVector* p_stMoveDirection )
{
	long lCounter;
	ACP_tdxBool bStep = FALSE;
	ACP_tdxBool bStepCandidate = FALSE;
	ACP_tdxBool bFindGround = FALSE;
	MTH_tdxReal xDotProd;
	MTH3D_tdstVector stVector;
	DNM_tdstMecObstacle *p_stTmpObstacle;

	if (!MEC_fn_bIsEdge(p_stGroundObstacle))
		return FALSE;

	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stTmpObstacle = p_stObstacle+lCounter;

		if (MEC_fn_bIsObstacleValid (p_stTmpObstacle))
		{
			if (p_stTmpObstacle==p_stGroundObstacle)
				bFindGround = TRUE;
			
			else if (MEC_fn_bWallObstacle(p_stDynamic,p_stTmpObstacle))
			{
				MTH3D_M_vSubVector ( &stVector, DNM_M_p_stObstacleGetContact(p_stGroundObstacle), DNM_M_p_stObstacleGetContact(p_stTmpObstacle) );	
				if (MTH3D_M_bIsNullVector(&stVector))
				{
					xDotProd = MTH3D_M_xDotProductVector (DNM_M_p_stObstacleGetNorm(p_stTmpObstacle),p_stMoveDirection);
					if (MTH_M_bLessZero(xDotProd))
						bStepCandidate=TRUE;
				}
			}
			else if (MEC_fn_bGroundObstacle(p_stDynamic,p_stTmpObstacle))
			{
				MTH3D_M_vSubVector ( &stVector, DNM_M_p_stObstacleGetContact(p_stGroundObstacle), DNM_M_p_stObstacleGetContact(p_stTmpObstacle) );	
				if (MTH_M_bLess(MTH3D_M_xGetZofVector(&stVector),C_xDNM_ON_GROUND_STEP) && !MTH3D_M_bIsNullVector(&stVector))
				{
					if (bStepCandidate && bFindGround)
					{
						bStep = TRUE;
						bStepCandidate = FALSE;
					}
				}
			}
		}
	}
	return bStep;
}




/*
 * Speed
 */ 
 
/* compute absolute speed */
void MEC_fn_vComputeSpeed (struct DNM_stDynamics *p_stDynamic, MTH_tdxReal xDT)
{
	MTH3D_tdstVector stSpeed;
	
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stCurrentTranslation, *p_stPreviousTranslation;
 
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
 
	p_stCurrentTranslation = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	p_stPreviousTranslation = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
 
	MTH3D_M_vSubVector (&stSpeed,p_stCurrentTranslation,p_stPreviousTranslation);
	MTH3D_M_vDivScalarVector (&stSpeed, &stSpeed, xDT);
	DNM_M_vDynamicsSetPreviousSpeed (p_stDynamic, &stSpeed);
}
 
 
/*
 * Matrix
 */
 
/* Init matrix*/
void MEC_fn_vInitMatrix (DNM_tdstDynamics *p_stDynamic)
{
	POS_tdxHandleToPosition hCurrentMatrix, hPreviousMatrix, hAbsolutePreviousMatrix;
	POS_tdxHandleToPosition hPrevPreviousMatrix;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);

	/*--- Complex ---*/
	if (DNM_M_bDynamicsIsComplexSize(p_stDynamic))
	{
		hAbsolutePreviousMatrix = DNM_M_p_stDynamicsGetAbsolutePreviousMatrix (p_stDynamic);
		hPrevPreviousMatrix	= DNM_M_p_stDynamicsGetPrevPreviousMatrix (p_stDynamic);
		POS_fn_vCopyMatrix ( hPrevPreviousMatrix, hPreviousMatrix );
		POS_fn_vCopyMatrix ( hAbsolutePreviousMatrix, hCurrentMatrix );
	}

	/*--- Base ---*/
	POS_fn_vCopyMatrix ( hPreviousMatrix, hCurrentMatrix );
}
 
 
/* correct scale */
void MEC_fn_vCorrectMatrix (struct DNM_stDynamics *p_stDynamic, POS_tdxHandleToPosition hMatrix)
{
	MTH3D_tdstVector stI, stJ, stK;
	POS_fn_vNormalizeMatrix (hMatrix);
	MTH3D_M_vSetVectorElements ( &stI, DNM_M_p_stDynamicsGetScaleX(p_stDynamic), MTH_C_ZERO, MTH_C_ZERO );
	MTH3D_M_vSetVectorElements ( &stJ, MTH_C_ZERO, DNM_M_p_stDynamicsGetScaleY(p_stDynamic), MTH_C_ZERO );
	MTH3D_M_vSetVectorElements ( &stK, MTH_C_ZERO, MTH_C_ZERO, DNM_M_p_stDynamicsGetScaleZ(p_stDynamic) );
	POS_fn_vSetScaleMatrix (hMatrix, &stI, &stJ, &stK);
	return;	
}
 
ACP_tdxBool MEC_fn_bIsEqualMatrix (POS_tdxHandleToPosition hMatrix1, POS_tdxHandleToPosition hMatrix2)
{
	MTH3D_tdstVector *p_stTranslation1, *p_stTranslation2;
 
	p_stTranslation1 = POS_fn_p_stGetTranslationVector ( hMatrix1 );
	p_stTranslation2 = POS_fn_p_stGetTranslationVector ( hMatrix2 );
 
	return MTH3D_M_bEqualVector (p_stTranslation1,p_stTranslation2) && MEC_fn_bIsEqualTransformMatrix (hMatrix1,hMatrix2);
}
 
 
ACP_tdxBool MEC_fn_bIsEqualRotationMatrix (POS_tdxHandleToPosition hMatrix1, POS_tdxHandleToPosition hMatrix2)
{
	MTH3D_tdstVector *p_stI1, *p_stJ1, *p_stK1;
	MTH3D_tdstVector *p_stI2, *p_stJ2, *p_stK2;
 
	POS_fn_vGetRotationVector ( hMatrix1, &p_stI1, &p_stJ1, &p_stK1 );
	POS_fn_vGetRotationVector ( hMatrix2, &p_stI2, &p_stJ2, &p_stK2 );
 
	return MTH3D_M_bEqualVector (p_stI1,p_stI2) && MTH3D_M_bEqualVector (p_stJ1,p_stJ2) && MTH3D_M_bEqualVector (p_stK1,p_stK2);
}
 
ACP_tdxBool MEC_fn_bIsEqualTransformMatrix (POS_tdxHandleToPosition hMatrix1, POS_tdxHandleToPosition hMatrix2)
{
	MTH3D_tdstVector stI1, stJ1, stK1;
	MTH3D_tdstVector stI2, stJ2, stK2;
 
	POS_fn_vGetTransformMatrix ( hMatrix1, &stI1, &stJ1, &stK1 );
	POS_fn_vGetTransformMatrix ( hMatrix2, &stI2, &stJ2, &stK2 );
 
	return MTH3D_M_bEqualVector (&stI1,&stI2) && MTH3D_M_bEqualVector (&stJ1,&stJ2) && MTH3D_M_bEqualVector (&stK1,&stK2);
}
 
void MEC_fn_vTranslateMatrixOn (struct DNM_stDynamics *p_stDynamic,MTH3D_tdstVector* p_stMatrixTranslation)
{
	POS_tdxHandleToPosition hCurrentMatrix, hPreviousMatrix;
	MTH3D_tdstVector *p_stCurrentVector, *p_stPreviousVector;
	long lx, ly, lz;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
 
	p_stCurrentVector = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	p_stPreviousVector = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
 
	lx = MTH_M_xRealToLong(MTH3D_M_xGetXofVector(p_stPreviousVector));
	ly = MTH_M_xRealToLong(MTH3D_M_xGetYofVector(p_stPreviousVector));
	lz = MTH_M_xRealToLong(MTH3D_M_xGetZofVector(p_stPreviousVector));
 
	MTH3D_M_vSetXofVector ( p_stMatrixTranslation, MTH_M_xLongToReal(lx) );
	MTH3D_M_vSetYofVector ( p_stMatrixTranslation, MTH_M_xLongToReal(ly) );
	MTH3D_M_vSetZofVector ( p_stMatrixTranslation, MTH_M_xLongToReal(lz) );
 
	MTH3D_M_vSubVector ( p_stCurrentVector, p_stCurrentVector, p_stMatrixTranslation );
	MTH3D_M_vSubVector ( p_stPreviousVector, p_stPreviousVector, p_stMatrixTranslation );
}
 
void MEC_fn_vTranslateMatrixOff (struct DNM_stDynamics *p_stDynamic,MTH3D_tdstVector* p_stMatrixTranslation)
{
	POS_tdxHandleToPosition hCurrentMatrix;
	MTH3D_tdstVector *p_stCurrentVector;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
 
	p_stCurrentVector = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	MTH3D_M_vAddVector ( p_stCurrentVector, p_stCurrentVector, p_stMatrixTranslation );
}
 
 
void MEC_fn_vUpdateMatrix ( struct DNM_stDynamics *p_stDynamic, POS_tdxHandleToPosition _hMatrix )
{
	MTH3D_tdstVector stI, stJ, stK;
	MTH3D_M_vSetVectorElements ( &stI, DNM_M_p_stDynamicsGetScaleX(p_stDynamic), MTH_C_ZERO, MTH_C_ZERO );
	MTH3D_M_vSetVectorElements ( &stJ, MTH_C_ZERO, DNM_M_p_stDynamicsGetScaleY(p_stDynamic), MTH_C_ZERO );
	MTH3D_M_vSetVectorElements ( &stK, MTH_C_ZERO, MTH_C_ZERO, DNM_M_p_stDynamicsGetScaleZ(p_stDynamic) );
 
	POS_fn_vSetScaleMatrix ( _hMatrix, &stI, &stJ, &stK );
}
 
 
 
/*
 * Adjust
 */	
 

#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 07/06/99 */
void MEC_fn_vAdjustStaticVector (struct DNM_stDynamics *p_stDynamic,MTH3D_tdstVector *p_stVector)
{
	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);
 
	p_stPreviousPosition = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
 
	MTH3D_M_vSubVector ( &stTranslation, p_stVector, p_stCurrentPosition );
	POS_fn_vSetTranslationVector ( hCurrentMatrix, p_stVector );
	
	MTH3D_M_vAddVector ( p_stPreviousPosition, p_stPreviousPosition, &stTranslation );
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 07/06/99 */

 
 
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
void MEC_fn_vAdjustStaticMove (struct DNM_stDynamics *p_stDynamic,MTH3D_tdstVector *p_stMove)
{
	POS_tdxHandleToPosition hCurrentMatrix;
	MTH3D_tdstVector *p_stCurrentPosition;
	MTH3D_tdstVector stVector;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
 
	MTH3D_M_vAddVector (&stVector,p_stCurrentPosition,p_stMove);
	MEC_fn_vAdjustStaticVector (p_stDynamic,&stVector);
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */
 
 
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
void MEC_fn_vAdjustStaticMatrix (struct DNM_stDynamics *p_stDynamic,POS_tdxHandleToPosition hMatrix)
{
	POS_tdxHandleToPosition hCurrentMatrix;
	MTH3D_tdstVector *p_stTranslation;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
 
	/* translation */
	p_stTranslation = POS_fn_p_stGetTranslationVector ( hMatrix );
	MEC_fn_vAdjustStaticVector (p_stDynamic,p_stTranslation);
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
 
 
void MEC_fn_vAdjustScaleVector (struct DNM_stDynamics *p_stDynamic,MTH3D_tdstVector* p_stVector)
{
	MTH3D_M_vSetXofVector ( p_stVector, MTH_M_xMul(MTH3D_M_xGetXofVector (p_stVector),DNM_M_p_stDynamicsGetScaleX(p_stDynamic)) );
	MTH3D_M_vSetYofVector ( p_stVector, MTH_M_xMul(MTH3D_M_xGetYofVector (p_stVector),DNM_M_p_stDynamicsGetScaleY(p_stDynamic)) );
	MTH3D_M_vSetZofVector ( p_stVector, MTH_M_xMul(MTH3D_M_xGetZofVector (p_stVector),DNM_M_p_stDynamicsGetScaleZ(p_stDynamic)) );
}
 
void MEC_fn_vAdjustSpeed (struct DNM_stDynamics* p_stDynamic, MTH_tdxReal xDT)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
	MTH3D_tdstVector stTranslation;
	MTH_tdxReal xDotProd;
 
	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 );
	MTH3D_M_vSubVector ( &stTranslation, p_stCurrentPosition, p_stPreviousPosition);
	if (!MEC_fn_bIsNullVector(&stTranslation)) MTH3D_M_vNormalizeVector (&stTranslation,&stTranslation)
	else MTH3D_M_vNullVector (&stTranslation);
 
	xDotProd = MTH3D_M_xDotProductVector (&stTranslation,DNM_M_pDynamicsGetPreviousSpeed(p_stDynamic));
	MTH3D_M_vMulScalarVector (&stTranslation, MTH_M_xMul(xDotProd,xDT), &stTranslation);
 
	MTH3D_M_vAddVector (p_stCurrentPosition,p_stPreviousPosition,&stTranslation);
}
 
 
 
/*
 * Mth
 */
 
MTH_tdxReal MEC_fn_vRoundReal (MTH_tdxReal xReal, MTH_tdxReal xEpsilon)
{
	long lValue;
	MTH_tdxReal xValue;
 
	xValue = MTH_M_xDiv (xReal, xEpsilon);
	lValue = MTH_M_xRealToLong (xValue);
	xReal = MTH_M_xLongToReal (lValue);
	return MTH_M_xMul (xReal, xEpsilon);
}
 
void MEC_fn_vRoundVector (MTH3D_tdstVector* p_stVector, MTH_tdxReal xEpsilon)
{
	MTH3D_M_vSetXofVector ( p_stVector, MEC_fn_vRoundReal ( MTH3D_M_xGetXofVector(p_stVector), xEpsilon ) );
	MTH3D_M_vSetYofVector ( p_stVector, MEC_fn_vRoundReal ( MTH3D_M_xGetYofVector(p_stVector), xEpsilon ) );
	MTH3D_M_vSetZofVector ( p_stVector, MEC_fn_vRoundReal ( MTH3D_M_xGetZofVector(p_stVector), xEpsilon ) );
}
 
 
/* Convert inertia, slide and rebound factor*/
MTH_tdxReal MEC_fn_xConvertScale ( MTH_tdxReal xValue )
{
	/* 1 - (1-x)^4   0<x<1 */
	/* x             x>=1 */
	MTH_tdxReal xTmpValue;
 
	if (xValue>=MTH_C_ONE) return xValue;
	if (xValue>MTH_C_ZERO)
	{
		xTmpValue = MTH_M_xSub ( MTH_C_ONE, xValue );
		xTmpValue = MTH_M_xMul ( xTmpValue, xTmpValue );
		xTmpValue = MTH_M_xMul ( xTmpValue, xTmpValue );
		xTmpValue = MTH_M_xSub ( MTH_C_ONE, xTmpValue );
	}
	else
		xTmpValue=MTH_C_ZERO;
	return 	xTmpValue;
}
 
 
 
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
ACP_tdxBool MEC_fn_bVectorParallel ( MTH3D_tdstVector* p_stVector1, MTH3D_tdstVector* p_stVector2 )
{
	MTH_tdxReal	xDotProd;
	MTH_tdxReal xProdNorm;
 
 
	xDotProd = MTH3D_M_xDotProductVector (p_stVector1,p_stVector2);
	xProdNorm = MTH_M_xMul ( MTH3D_M_xNormVector(p_stVector1), MTH3D_M_xNormVector(p_stVector2) );
 
	return MTH_M_bLess ( MTH_M_xAbs(MTH_M_xSub(xDotProd,xProdNorm)), MTH_M_xDoubleToReal(0.01) );
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
 

 
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
void MEC_fn_vCheckMinVectorNorm ( MTH3D_tdstVector* p_stVector, MTH_tdxReal xLimit )
{
	MTH_tdxReal xNorm;
	xNorm = MTH3D_M_xNormVector (p_stVector);

	if (MTH_M_bLess(xNorm,xLimit))
	{
		MTH3D_M_vNullVector (p_stVector);	
	}
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */



 
/*
 * BackFace
 */
 
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
void MEC_fn_vDeleteBackFace ( long lObstacle, DNM_tdstMecObstacle *p_stObstacle )
{
	long lCounter;
	MTH_tdxReal xDotProd;
 
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		DNM_tdstMecObstacle *p_stCurrentObstacle = p_stObstacle + lCounter;
		
		if ( MEC_fn_bIsObstacleValid (p_stCurrentObstacle) &&!MEC_fn_bIsWater (p_stCurrentObstacle) )
		{
			xDotProd = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stCurrentObstacle),DNM_M_pObstacleGetZoneMove(p_stCurrentObstacle) );
				
/*			if ( MTH_M_bGreaterEqual(xDotProd, MTH_C_ZERO ) )*/
			if ( MTH_M_bGreater(xDotProd, MTH_M_xNeg(C_xDNM_MTH_EPSILON)) )
				MEC_fn_vInvalidateObstacle (p_stCurrentObstacle);
 
		}
 
	}
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */

 
 
void MEC_fn_vDeleteOnGroundBackFace ( long lObstacle, DNM_tdstMecObstacle *p_stObstacle, struct DNM_stDynamics* p_stDynamic, MTH3D_tdstVector* p_stVector, DNM_tdstMecObstacle *p_stGroundObstacle )
{
	MTH3D_tdstVector stVector, stAdjustVector;
	long lCounter;
/* XB 05/05/99 */
/* 	ACP_tdxBool bWallOk = FALSE; */
/* End XB 05/05/99 */
	MTH_tdxReal xDotProd;
	DNM_tdstMecObstacle *p_stCurrentObstacle;
 
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stCurrentObstacle = p_stObstacle + lCounter;
		
		if (MEC_fn_bIsObstacleValid(p_stCurrentObstacle))
		{
			if (MEC_fn_bWallObstacle(p_stDynamic,p_stCurrentObstacle))
			{
				xDotProd = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stCurrentObstacle),p_stVector );
				if ( MTH_M_bLessEqual(xDotProd,MTH_C_ZERO))
				{
					MTH3D_M_vSubVector (&stVector,DNM_M_p_stObstacleGetContact(p_stCurrentObstacle),POS_fn_p_stGetTranslationVector(DNM_M_p_stDynamicsGetPreviousMatrix(p_stDynamic)));
					MTH3D_M_vMulScalarVector ( &stAdjustVector, DNM_M_xObstacleGetZoneRadius(p_stCurrentObstacle), DNM_M_pDynamicsGetGroundNormal(p_stDynamic) );
					MTH3D_M_vSubVector ( &stVector, &stVector, &stAdjustVector );

					xDotProd = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stCurrentObstacle),&stVector );
				
					if ( MTH_M_bGreater(xDotProd,MTH_C_ZERO) )
						MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
				}
				else MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
			}
			else if (!MEC_fn_bGroundObstacle(p_stDynamic,p_stCurrentObstacle))
			{
				if (p_stGroundObstacle)
				{
					MTH3D_M_vSubVector (&stVector,DNM_M_p_stObstacleGetContact(p_stCurrentObstacle),DNM_M_p_stObstacleGetContact(p_stGroundObstacle));
					xDotProd = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stGroundObstacle),&stVector );
					if (MTH_M_bLess(xDotProd,C_ulDNM_RAYMAN_ZDM_RADIUS))
						MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
				}
			}
		}
	}
}
 
void MEC_fn_vDeleteGiBackFace ( long lObstacle, DNM_tdstMecObstacle *p_stObstacle, struct DNM_stDynamics* p_stDynamic, MTH3D_tdstVector* p_stVector )
{
	long lCounter;
	MTH_tdxReal xDotProd;
	MTH3D_tdstVector stVerticalVector;
 
	MTH3D_M_vSetBaseKVector (&stVerticalVector);
 
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		DNM_tdstMecObstacle *p_stCurrentObstacle = p_stObstacle + lCounter;
 
		if (MEC_fn_bIsObstacleValid(p_stCurrentObstacle))
		{
		
			if ( MEC_fn_bTypeOfGameMaterial(DNM_M_hObstacleGetCollidedMaterial (p_stCurrentObstacle),C_xDNM_GI_TYPE_GAME_MATERIAL) && !MEC_fn_bTypeOfGameMaterial(DNM_M_hObstacleGetCollidedMaterial (p_stCurrentObstacle),C_xDNM_NO_COLLISION_TYPE_GAME_MATERIAL)  )
			{	
				xDotProd = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stCurrentObstacle),&stVerticalVector );
		
				if ( MTH_M_bGreater(xDotProd,MTH_C_ZERO) )
					MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
			}
			else if (MEC_fn_bCeilObstacle(p_stDynamic,p_stCurrentObstacle))
			{
				MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
			}
			else if (MEC_fn_bWallObstacle(p_stDynamic,p_stCurrentObstacle))
			{
				xDotProd = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stCurrentObstacle), p_stVector );
				if ( MTH_M_bGreater(xDotProd,MTH_C_ZERO) )
					MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
			}
		}
	}
}
 
void MEC_fn_vDeleteClimbBackFace ( long lObstacle, DNM_tdstMecObstacle *p_stObstacle, struct DNM_stDynamics* p_stDynamic, MTH3D_tdstVector* p_stVector )
{
	long lCounter;
	MTH_tdxReal xDotProd;
 
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		DNM_tdstMecObstacle *p_stCurrentObstacle = p_stObstacle + lCounter;
 
		if (MEC_fn_bIsObstacleValid(p_stCurrentObstacle))
		{
			POS_tdxHandleToPosition hCurrentMatrix;
			MTH3D_tdstVector *p_stX, *p_stY, *p_stZ;
 
			hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
			POS_fn_vGetRotationVector ( hCurrentMatrix, &p_stX, &p_stY, &p_stZ );
 
			xDotProd = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stCurrentObstacle), p_stY );
 
			if ( MEC_fn_bTypeOfGameMaterial(DNM_M_hObstacleGetCollidedMaterial (p_stCurrentObstacle),C_xDNM_CLIMB_TYPE_GAME_MATERIAL) && !MEC_fn_bTypeOfGameMaterial(DNM_M_hObstacleGetCollidedMaterial (p_stCurrentObstacle),C_xDNM_NO_COLLISION_TYPE_GAME_MATERIAL) )
			{	
				if ( MTH_M_bLess(xDotProd,MTH_M_xDoubleToReal(-0.707)) )
					MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
			}
			else
			{
				if ( MTH_M_bLess(xDotProd,MTH_M_xDoubleToReal(-0.95)) )
					MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
				else
				{
					xDotProd = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stCurrentObstacle),p_stVector );
					
					if ( MTH_M_bGreater(xDotProd,C_xDNM_MTH_EPSILON) )
						MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
				}
			}
 
		}
	}
}
 
 
 
void MEC_fn_vDeleteBaseBackFace ( long lObstacle, DNM_tdstMecObstacle *p_stObstacle, struct DNM_stDynamics* p_stDynamic )
{
	MTH3D_tdstVector  stVector;
	long lCounter;
	MTH_tdxReal xDotProd1, xDotProd2;
 
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		DNM_tdstMecObstacle *p_stCurrentObstacle = p_stObstacle + lCounter;
		
		if ( MEC_fn_bIsObstacleValid(p_stCurrentObstacle) && !MEC_fn_bIsWater(p_stCurrentObstacle) )
		{
			if (!MEC_fn_bIsNullVector(DNM_M_pObstacleGetZoneMove(p_stCurrentObstacle)))
				MTH3D_M_vNormalizeVector (&stVector,DNM_M_pObstacleGetZoneMove(p_stCurrentObstacle))
			else
				MTH3D_M_vNullVector (&stVector);
			xDotProd2 = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stCurrentObstacle),&stVector );
		
			if ( MTH_M_bGreater(xDotProd2,MTH_M_xDoubleToReal(0.707)) )			/* 45° */
				MEC_fn_vInvalidateObstacle (p_stCurrentObstacle);
			else if (!DNM_M_bDynamicsIsHanging(p_stDynamic))
			{
//				if (MTH_M_bLess( MTH3D_M_xNormVector(DNM_M_pObstacleGetZoneMove(p_stCurrentObstacle)), DNM_M_xObstacleGetZoneRadius(p_stCurrentObstacle)) || MTH_M_bLess ( MTH_M_xAbs(xDotProd2), MTH_M_xDoubleToReal(0.17365) ) ) /* 10° */
				{
					if (MTH_M_bEqualZero(DNM_M_xObstacleGetZoneRadius(p_stCurrentObstacle)))
						MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
					else
					{
						MTH3D_M_vSubVector ( &stVector, DNM_M_p_stObstacleGetContact(p_stCurrentObstacle), DNM_M_pObstacleGetZonePosition(p_stCurrentObstacle) ); 
						MTH3D_M_vAddVector ( &stVector, &stVector, DNM_M_pObstacleGetZoneMove(p_stCurrentObstacle) ); 
						xDotProd1 = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stCurrentObstacle),&stVector );

						if (MTH_M_bGreater(xDotProd1,MTH_C_ZERO))
							MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
					}
				} 
//				else
//				{
//					if ( MTH_M_bGreaterZero(xDotProd2) )
//						MEC_fn_vInvalidateObstacle (p_stCurrentObstacle);
//				}
			}
		}
	}
}
 
 
/*
 * No collided faces
 */
void  MEC_fn_vDeleteNoCollidedGroundFace ( long lObstacle, DNM_tdstMecObstacle *p_stObstacle, struct DNM_stDynamics* p_stDynamic )
{
	long lCounter;
	DNM_tdstMecObstacle *p_stCurrentObstacle;
 
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stCurrentObstacle = p_stObstacle + lCounter;
		
		if (MEC_fn_bIsObstacleValid(p_stCurrentObstacle))
		{
			if (MEC_fn_bGroundObstacle(p_stDynamic,p_stCurrentObstacle))
			{
				if (MEC_fn_bTypeOfGameMaterial (DNM_M_hObstacleGetMyMaterial(p_stCurrentObstacle),C_xDNM_NO_COLLISION_WITH_GROUND))
					MEC_fn_vInvalidateObstacle(p_stCurrentObstacle);
			}
		}
	}
}
 
 
void MEC_fn_vDeleteHangingObstacle ( long lObstacle, DNM_tdstMecObstacle *p_stObstacle, struct DNM_stDynamics* p_stDynamic )
{
	long lCounter;
	MTH3D_tdstVector  stVector;
	DNM_tdstMecObstacle *p_stTmpObstacle;
	DNM_tdstMecObstacle *p_stGroundObstacle = NULL;
	MTH_tdxReal xZ = MTH_C_InfinitPlus;
	MTH_tdxReal xHandZPosition;

//	if (MEC_fn_bIsPlatform(p_stDynamic))
//	{
//		for (lCounter=0; lCounter<lObstacle; lCounter++)
//			MEC_fn_vInvalidateObstacle(p_stObstacle + lCounter);
//		return;
//	}

	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stTmpObstacle = p_stObstacle + lCounter;
		if (MEC_fn_bIsObstacleValid(p_stTmpObstacle)&&MEC_fn_bGroundObstacle(p_stDynamic,p_stTmpObstacle))
		{
			if ( MTH_M_bLess ( MTH3D_M_xGetZofVector(DNM_M_p_stObstacleGetContact(p_stTmpObstacle)), xZ ) )
			{
				p_stGroundObstacle = p_stTmpObstacle;
				xZ = MTH3D_M_xGetZofVector(DNM_M_p_stObstacleGetContact(p_stTmpObstacle));
			}
		}
	}

	if (MEC_fn_bIsPlatform(p_stDynamic))
		xHandZPosition = MTH_M_xAdd ( DNM_M_xDynamicsGetHangingLimit(p_stDynamic), MTH3D_M_xGetZofVector(POS_fn_p_stGetTranslationVector(HIE_fn_hGetSuperObjectGlobalMatrix(DNM_M_hDynamicsGetPlatformSO(p_stDynamic)))) );
	else
		xHandZPosition = DNM_M_xDynamicsGetHangingLimit(p_stDynamic);
	
	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stTmpObstacle = p_stObstacle + lCounter;
		if (MEC_fn_bIsObstacleValid(p_stTmpObstacle))
		{
			if ( !MEC_fn_bGroundObstacle(p_stDynamic,p_stTmpObstacle) && MTH_M_bLessEqual(DNM_M_xObstacleGetRate(p_stTmpObstacle),MTH_C_ONE) && !MEC_fn_bIsEdge(p_stTmpObstacle) )
			{
				if (p_stGroundObstacle)			
				{
					MTH3D_M_vSubVector ( &stVector, DNM_M_p_stObstacleGetContact(p_stTmpObstacle), DNM_M_p_stObstacleGetContact(p_stGroundObstacle) );
					if (MTH_M_bLess ( MTH3D_M_xDotProductVector(DNM_M_p_stObstacleGetNorm(p_stGroundObstacle),&stVector), MTH_M_xDoubleToReal(0.005) ))
						MEC_fn_vInvalidateObstacle(p_stTmpObstacle);
				}
				else
				{
					if (MTH_M_bLessEqual(MTH3D_M_xGetZofVector(DNM_M_p_stObstacleGetContact(p_stTmpObstacle)),/*DNM_M_xDynamicsGetHangingLimit(p_stDynamic)*/ xHandZPosition ))
						MEC_fn_vInvalidateObstacle(p_stTmpObstacle);
				}
			}
			else
				MEC_fn_vInvalidateObstacle(p_stTmpObstacle);
		}
	}
}


void MEC_fn_vDeleteObstacleBehind ( long lObstacle, DNM_tdstMecObstacle *a_stObstacle, DNM_tdstMecObstacle *p_stObstacle )
{
	long lCounter;
	MTH3D_tdstVector  stVector;
	DNM_tdstMecObstacle *p_stTmpObstacle;
	MTH_tdxReal xDotProd;

	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stTmpObstacle = a_stObstacle + lCounter;
		if (MEC_fn_bIsObstacleValid(p_stTmpObstacle)&&(p_stTmpObstacle!=p_stObstacle))
		{
			MTH3D_M_vSubVector ( &stVector, DNM_M_p_stObstacleGetContact(p_stTmpObstacle), DNM_M_p_stObstacleGetContact(p_stObstacle) );
			xDotProd = MTH3D_M_xDotProductVector ( DNM_M_p_stObstacleGetNorm(p_stObstacle), &stVector );
			if (MTH_M_bLess(xDotProd,MTH_M_xDoubleToReal(-1e-3)))
				MEC_fn_vInvalidateObstacle(p_stTmpObstacle);
		}
	}
}


void MEC_fn_vDeleteNullTranslationObstacle ( long lObstacle, DNM_tdstMecObstacle *a_stObstacle )
{
	long lCounter;
	DNM_tdstMecObstacle *p_stTmpObstacle;

	for (lCounter=0; lCounter<lObstacle; lCounter++)
	{
		p_stTmpObstacle = a_stObstacle + lCounter;
		if ( MEC_fn_bIsObstacleValid(p_stTmpObstacle) && MEC_fn_bIsNullVector(DNM_M_xObstacleGetTranslation(p_stTmpObstacle)) )
			MEC_fn_vInvalidateObstacle(p_stTmpObstacle);
	}
}




/*
 * Hierarchy
 */
 
/* Get father actor*/
HIE_tdxHandleToSuperObject MEC_fn_hGetFatherActor (HIE_tdxHandleToSuperObject hSupObj)
{
	HIE_tdxHandleToSuperObject hActor = hSupObj;
 
	while (HIE_fn_ulGetSuperObjectType(hActor)!=HIE_C_ulActor)
	{
		hActor = HIE_fn_hGetSuperObjectFather (hActor);
		if (!HIE_fn_bIsSuperObjectValid (hActor)) return hActor;
	}
	return hActor;
}
 
 
 
/*
 * Frame
 */
 
 
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
void MEC_fn_vComputeInitialFrameTranslation ( MTH3D_tdstVector *p_stVector, struct DNM_stDynamics *p_stDynamic, long lFrame)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
 
	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 );
	MTH3D_M_vSubVector ( p_stVector, p_stCurrentPosition, p_stPreviousPosition );
	MTH3D_M_vDivScalarVector ( p_stVector, p_stVector, MTH_M_xDoubleToReal(lFrame) );
 
	POS_fn_vSetTranslationVector ( hCurrentMatrix, p_stPreviousPosition ); 
} 
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */
 
 
void MEC_fn_vSetInitialFrameTranslation ( struct DNM_stDynamics *p_stDynamic, MTH3D_tdstVector* p_stVector)
{
	POS_tdxHandleToPosition hCurrentMatrix;
	MTH3D_tdstVector *p_stCurrentPosition;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
 
	p_stCurrentPosition = POS_fn_p_stGetTranslationVector ( hCurrentMatrix );
	MTH3D_M_vAddVector (p_stCurrentPosition,p_stCurrentPosition,p_stVector);
}
 
 
void MEC_fn_vComputeStream ( MTH3D_tdstVector *p_stStream, struct DNM_stDynamics *p_stDynamic, 	MTH_tdxReal xDT, ACP_tdxBool bSlide)
{
	MTH_tdxReal xMultFactor;
 
	xMultFactor = MTH_M_xMul (xDT,DNM_M_xDynamicsGetStreamFactor(p_stDynamic));
	MTH3D_M_vMulScalarVector ( p_stStream, xMultFactor, DNM_M_pDynamicsGetStreamSpeed ( p_stDynamic ) );
	if (bSlide) MTH3D_M_vMulScalarVector ( p_stStream, DNM_M_xDynamicsGetPreviousSlide(p_stDynamic), p_stStream );
}
 
void MEC_fn_vComputeInertiaTranslation ( struct DNM_stDynamics *p_stDynamic, MTH_tdxReal xDT )
{
	MTH3D_tdstVector stVector;
	ACP_tdxBool bSlide;
 
	/* stream */
	bSlide = (DNM_M_xDynamicsGetStreamPriority(p_stDynamic)!=1);
	MEC_fn_vComputeStream ( &stVector, p_stDynamic, xDT, bSlide);
 
	/* additionnal rotation */
	MTH3D_M_vAddVector ( &stVector, &stVector, DNM_M_pDynamicsGetAddTranslation (p_stDynamic) );
 
	DNM_M_vDynamicsSetInertiaTranslation ( p_stDynamic, &stVector );
}
 
 
 
/*
 * Actor
 */

#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
ACP_tdxBool MEC_fn_bIsMultiCollision (HIE_tdxHandleToSuperObject hSupObj)
{
	/* Rayman */
	return (fn_ulStandardGameGetCustomBitsSO(hSupObj) & C_ulDNM_RAYMAN_CUSTOM_BIT) == C_ulDNM_RAYMAN_CUSTOM_BIT;
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */
 
 
/*#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC   */
ACP_tdxBool MEC_fn_bMainActor (HIE_tdxHandleToSuperObject hSupObj)
{
	/* Rayman */
	return (fn_ulStandardGameGetCustomBitsSO(hSupObj) & C_ulDNM_RAYMAN_CUSTOM_BIT) == C_ulDNM_RAYMAN_CUSTOM_BIT;
}
/*#endif /* _FIRE_DEADCODE_U64_    /* Added by RUC   */
 
 
 
 
/*
 * Optimization
 */
 
 
ACP_tdxBool MEC_fn_bNoMove (struct DNM_stDynamics *p_stDynamic)
{
	if ( !MTH3D_M_bIsNullVector(DNM_M_pDynamicsGetPreviousSpeed(p_stDynamic)) ) return FALSE;
	if ( DNM_M_bDynamicsIsGravity(p_stDynamic) ) return FALSE;
	if ( DNM_M_bDynamicsIsAnimation(p_stDynamic) ) return FALSE;
	if ( DNM_M_bDynamicsIsImposeSpeed(p_stDynamic) ) return FALSE;
	if ( DNM_M_bDynamicsIsImposeRotation(p_stDynamic) ) return FALSE;
	if ( DNM_M_bDynamicsIsProposeSpeed(p_stDynamic) ) return FALSE;
 
	if (DNM_M_bDynamicsIsAdvancedSize(p_stDynamic))
	{
		if ( DNM_M_bDynamicsIsInertia(p_stDynamic) ) return FALSE;
		if ( DNM_M_bDynamicsIsImposeRotation(p_stDynamic) ) return FALSE;
		if ( DNM_M_bDynamicsIsStream(p_stDynamic) ) return FALSE;
		if ( DNM_M_bDynamicsIsAddSpeed(p_stDynamic) ) return FALSE;
	}
	
	if (DNM_M_bDynamicsIsComplexSize(p_stDynamic))
	{
		if ( MEC_fn_bIsPlatform(p_stDynamic) ) return FALSE;
	}
 
	return TRUE;
}
 
 
 
/*
 * Safe position
 */

#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
void MEC_fn_vAdjustSafeTranslation (struct DNM_stDynamics *p_stDynamic)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector* p_stTranslation;
 
	hCurrentMatrix = DNM_M_p_stDynamicsGetCurrentMatrix (p_stDynamic);
	hPreviousMatrix = DNM_M_p_stDynamicsGetPreviousMatrix (p_stDynamic);
 
	if (DNM_M_bDynamicsIsSafeValid(p_stDynamic))
	{
		p_stTranslation =  DNM_M_pDynamicsGetSafeTranslation ( p_stDynamic );
		POS_fn_vSetTranslationVector ( hPreviousMatrix, p_stTranslation );
		POS_fn_vSetTranslationVector ( hCurrentMatrix, p_stTranslation );
	}
	else
	{
		p_stTranslation = POS_fn_p_stGetTranslationVector ( hPreviousMatrix );
		POS_fn_vSetTranslationVector ( hCurrentMatrix, p_stTranslation );
	}
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */

 

#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC 04/06/99 */
void MEC_fn_vRegisterSafeTranslation (struct DNM_stDynamics *p_stDynamic, POS_tdxHandleToPosition hMatrix)
{
	MTH3D_tdstVector *p_stTranslation;
 
	p_stTranslation = POS_fn_p_stGetTranslationVector ( hMatrix );
 
	DNM_M_vDynamicsSetSafeValid ( p_stDynamic, TRUE );
	DNM_M_vDynamicsSetSafeTranslation ( p_stDynamic, p_stTranslation );
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC 04/06/99 */



void MEC_fn_vSlowMode (struct DNM_stDynamics *p_stDynamic, long lMode)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
	MTH3D_tdstVector *p_stSpeed;
	MTH3D_tdstVector stVector;
	MTH_tdxReal xInvMode;

	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 );
	xInvMode = MTH_M_xInv (MTH_M_xDoubleToReal(lMode));

	/* adjust position */
	MTH3D_M_vSubVector ( &stVector, p_stCurrentPosition, p_stPreviousPosition );
	MTH3D_M_vMulScalarVector ( &stVector, xInvMode , &stVector );	
	MTH3D_M_vAddVector ( p_stCurrentPosition, p_stPreviousPosition, &stVector );
	
	/* adjust speed */
	p_stSpeed = DNM_M_pDynamicsGetPreviousSpeed (p_stDynamic);
	MTH3D_M_vMulScalarVector ( p_stSpeed, xInvMode , p_stSpeed );	
}


void MEC_fn_vNormalMode (struct DNM_stDynamics *p_stDynamic, long lMode)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
	MTH3D_tdstVector *p_stSpeed;
	MTH3D_tdstVector stVector;

	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 );

	/* adjust position */
	MTH3D_M_vSubVector ( &stVector, p_stCurrentPosition, p_stPreviousPosition );
	MTH3D_M_vMulScalarVector ( &stVector, MTH_M_xDoubleToReal(lMode) , &stVector );	
	MTH3D_M_vSubVector ( p_stPreviousPosition, p_stCurrentPosition, &stVector );
	
	/* adjust speed */
	p_stSpeed = DNM_M_pDynamicsGetPreviousSpeed (p_stDynamic);
	MTH3D_M_vMulScalarVector ( p_stSpeed, MTH_M_xDoubleToReal(lMode) , p_stSpeed );	
}

void MEC_fn_vRepeatMove (struct DNM_stDynamics *p_stDynamic)
{
	POS_tdxHandleToPosition hPreviousMatrix, hCurrentMatrix;
	MTH3D_tdstVector *p_stPreviousPosition, *p_stCurrentPosition;
	MTH3D_tdstVector stVector;

	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 );

	MTH3D_M_vSubVector ( &stVector, p_stCurrentPosition, p_stPreviousPosition );
	MTH3D_M_vCopyVector ( p_stPreviousPosition, p_stCurrentPosition );
	MTH3D_M_vAddVector ( p_stCurrentPosition, p_stCurrentPosition, &stVector );
}