/*
 *=======================================================================================
 * Name        : DNMCame.c
 * Author      : CB / YLG / VL               Date : there's a long time ago
 * Description : Mechanic function (parsing, obstacle, update) for camera
 *=======================================================================================
 */
#include "cpa_expt.h"
#include "ACP_Base.h"
 
 
#include "MEC/ParsData.h"
#include "MEC/DNMMTH.h"
#include "MEC/MecInc.h"
#include "MEC/Mcamic.h"
#include "MEC/DNMObsta.h"
#include "POS.h"
#include "MEC/DNMDynam.h"
 
#include "MEC/MECTools.h"
 
 
#include "Extheade.h"
#include "MTH.h"
 
#include "Spo.h"
#include "COL.h"
 
#include "MEC/DNMCame.h"
#include "MEC/DNMSurSt.h"
#include "SAFe.h"
 
#include "PCS.h" /* Physical Collide Set */
#include "SCT.h" /* SeCTor*/
#include "FIL.h" /* SeCTor*/
#include "PRT.h" /* PaRTicules*/
#include "GAM.h" /* GAMe*/
 

extern DNM_tdstMecObstacle *g_a_stObstacle;
//extern DNM_tdstMecObstacle g_a_stObstacle [COL_C_xMaxNumberOfCollisions];
 
 /*
 **************************************************************************************************
 **************************************************************************************************
	TOOLS
 **************************************************************************************************
 **************************************************************************************************
 */
/*
 *=================================================================================================
 *	Compute the horizontal angle between two vectors
 *=================================================================================================
 */
MTH_tdxReal CAM_fn_xComputeHorizontalAngle
(
	MTH3D_tdstVector	*_p_stOrgVector,
	MTH3D_tdstVector	*_p_stDestVector,
	MTH3D_tdstVector	*_p_stAxisZ
)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	MTH3D_tdstVector	stVect1, stVect2, stAxisZ;
	MTH_tdstRotation	stRotation;
	MTH_tdxReal			xAngle;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	MTH3D_M_vCopyVector(&stVect1,_p_stOrgVector);
	MTH3D_M_vCopyVector(&stVect2,_p_stDestVector);
	MTH3D_M_vCopyVector(&stAxisZ,_p_stAxisZ);
	/*
	 *	Projection of two vectors onto the horizontal plane 
	 */
/*FB170798*/
	/*MTH_p_stVectorComputeProjection(&stVect1, &stVect1, &stAxisZ);*/
	MTH_fn_vVectorPlaneProjection(&stVect1,&stVect1,&stAxisZ,&stAxisZ);
	
/*END FB*/
	if (!MEC_fn_bIsNullVector(&stVect1))
	{	MTH3D_M_vNormalizeVector(&stVect1, &stVect1);}
	else
	{	return(MTH_C_ZERO);}
/*FB170798*/
	/*MTH_p_stVectorComputeProjection(&stVect2, &stVect2, &stAxisZ);*/
	MTH_fn_vVectorPlaneProjection(&stVect2,&stVect2,&stAxisZ,&stAxisZ);
/*END FB*/
	if (!MEC_fn_bIsNullVector(&stVect2))
	{	MTH3D_M_vNormalizeVector(&stVect2, &stVect2);}
	else
	{	return(MTH_C_ZERO);}
 
	/*
	 *	Compute the horizontal rotation between the two projected vectors 	
	 */
	MTH_p_stRotationComputeRotationWith2Vectors(&stRotation, &stVect1, &stVect2);
	xAngle = MTH_M_xRotationGetAngle(&stRotation);
	
	if(MTH_M_bLessZero(MTH3D_M_xDotProductVector(MTH_M_p_stRotationGetAxis(&stRotation), &stAxisZ)))
		xAngle = MTH_M_xNeg(xAngle);
 
	return (xAngle);
}
 
/*
 *=================================================================================================
 *	Compute the vertical angle between two vectors
 *=================================================================================================
 */
MTH_tdxReal CAM_fn_xComputeVerticalAngle
(
	MTH3D_tdstVector	*_p_stOrgVector,
	MTH3D_tdstVector	*_p_stDestVector,
	MTH3D_tdstVector	*_p_stAxisZ
)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	MTH3D_tdstVector	stVector, stAxisZ, stProj;
	MTH_tdxReal			xOrgAngle, xDestAngle;
	MTH_tdstRotation	stRotation;
	MTH_tdxReal			xAngle;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	MTH3D_M_vCopyVector(&stAxisZ,_p_stAxisZ);
 
	/* 
	 * Vertical angle of org vector.
	 */
	MTH3D_M_vCopyVector(&stVector,_p_stOrgVector);
 
	if(MTH_M_bIsNullWithEpsilon(MTH3D_M_xDotProductVector(&stVector, &stAxisZ), MTH_M_xFloatToReal(0.001f)))
		xOrgAngle = MTH_C_ZERO;
	else
	{
/*FB170798*/
		/*MTH_p_stVectorComputeProjection(&stProj, &stVector, &stAxisZ);*/
		MTH_fn_vVectorPlaneProjection(&stProj,&stVector,&stAxisZ,&stAxisZ);
		/*MTH3D_M_vNormalizeVector(&stVector, &stVector);*/
/*END FB*/
 
 
		if (!MEC_fn_bIsNullVector(&stProj))
		{
/*FB170798*/
			MTH3D_M_vNormalizeVector(&stVector, &stVector);
/*END FB*/
			MTH3D_M_vNormalizeVector(&stProj, &stProj);
 
			MTH_p_stRotationComputeRotationWith2Vectors(&stRotation, &stVector, &stProj);
			xOrgAngle = MTH_M_xRotationGetAngle(&stRotation);
 
			/* Sens */
			MTH_p_stRotationComputeRotationWith2Vectors(&stRotation, &stVector, &stAxisZ);
			xAngle = MTH_M_xRotationGetAngle(&stRotation);
			if (MTH_M_bGreater(xAngle, MTH_C_PiBy2))
				xOrgAngle = MTH_M_xNeg(xOrgAngle);
		}
		else
		{
			xOrgAngle = MTH_C_ZERO;
		}
	}
 
	/* 
	 * Vertical angle of dest vector.
	 */
	MTH3D_M_vCopyVector(&stVector,_p_stDestVector);
 
	if(MTH_M_bEqualZero(MTH3D_M_xDotProductVector(&stVector, &stAxisZ)))
		xDestAngle = MTH_C_ZERO;
	else
	{
/*FB170798*/
		/*MTH_p_stVectorComputeProjection(&stProj, &stVector, &stAxisZ);*/
		MTH_fn_vVectorPlaneProjection(&stProj,&stVector,&stAxisZ,&stAxisZ);
/*END FB*/
		if (!MEC_fn_bIsNullVector(&stProj))
		{
			MTH3D_M_vNormalizeVector(&stVector, &stVector);
			MTH3D_M_vNormalizeVector(&stProj, &stProj);
	
			MTH_p_stRotationComputeRotationWith2Vectors(&stRotation, &stVector, &stProj);
			xDestAngle = MTH_M_xRotationGetAngle(&stRotation);
 
			/* Sens */
			MTH_p_stRotationComputeRotationWith2Vectors(&stRotation, &stVector, &stAxisZ);
			xAngle = MTH_M_xRotationGetAngle(&stRotation);
			if (MTH_M_bGreater(xAngle, MTH_C_PiBy2))
				xDestAngle = MTH_M_xNeg(xDestAngle);
		}
		else
		{
			xDestAngle = MTH_C_ZERO;
		}
	}
 
	xAngle = MTH_M_xSub(xDestAngle, xOrgAngle);
	return (xAngle);
}
 
 
/*
 **************************************************************************************************
 **************************************************************************************************
 *	General parsing function for position and angle.
 **************************************************************************************************
 **************************************************************************************************
 */
 
/*
 *=================================================================================================
 *=================================================================================================
 */
void DNM_fn_vDynamicsTakeCareOfLastMove
(
	MTH3D_tdstVector	*_p_stShiftingResult,
	MTH3D_tdstVector	*_p_stLastShifting,
	MTH_tdxReal			_xCutAngleFactor
)
{
	MTH3D_tdstVector	stTempVector, stTempVector1;
	MTH_tdstRotation	stRotation;
 
/*FB170798*/
	register MTH_tdxReal			xNormShiftingResult;
	register MTH_tdxReal			xNormLastShiftingResult;
/*END FB*/
 
	if(_xCutAngleFactor == MTH_C_ONE)
		return;
 
/*FB170798*/
	xNormShiftingResult = MTH3D_M_xNormVector(_p_stShiftingResult);
	xNormLastShiftingResult = MTH3D_M_xNormVector(_p_stLastShifting);
/*END FB*/
 
	if
	(
/*FB170798*/
/*		(MTH_M_bGreater(MTH3D_M_xNormVector(_p_stShiftingResult), MTH_M_xFloatToReal(0.1f)))
	&&	(MTH_M_bGreater(MTH3D_M_xNormVector(_p_stLastShifting), MTH_M_xFloatToReal(0.1f)))
*/
		(MTH_M_bGreater(xNormShiftingResult, MTH_M_xFloatToReal(0.1f)))
	&&	(MTH_M_bGreater(xNormLastShiftingResult, MTH_M_xFloatToReal(0.1f)))
 
/*END FB*/
	)
	{
/*FB170798*/
/*		MTH3D_M_vNormalizeVector(&stTempVector, _p_stShiftingResult);
		MTH3D_M_vNormalizeVector(&stTempVector1, _p_stLastShifting);*/
		MTH3D_M_vMulScalarVector(&stTempVector,MTH_M_xInv(xNormShiftingResult),_p_stShiftingResult);
		MTH3D_M_vMulScalarVector(&stTempVector1,MTH_M_xInv(xNormLastShiftingResult),_p_stLastShifting);
/*END FB*/
		MTH_p_stRotationComputeRotationWith2Vectors(&stRotation, &stTempVector1, &stTempVector);
 
		stRotation.m_xAngle = MTH_M_xDiv(stRotation.m_xAngle, _xCutAngleFactor);
		MTH_p_stRotationMulVector(&stRotation, &stTempVector1, &stTempVector);
		MTH3D_M_vNormalizeVector(&stTempVector, &stTempVector);
/*FB170798*/
		/*MTH3D_M_vMulScalarVector(_p_stShiftingResult, MTH3D_M_xNormVector(_p_stShiftingResult), &stTempVector);*/
		MTH3D_M_vMulScalarVector(_p_stShiftingResult, xNormShiftingResult, &stTempVector);
/*END FB*/
	}
}
 
/*
 *=================================================================================================
 *	Parsing of a vector move.
 *=================================================================================================
 */
void DNM_fn_vDynamicsCameraPositionParsing
(
	MTH_tdxReal				_xSpeed,
	MTH_tdxReal				_xIncSpeed,
	MTH_tdxReal				_xDecSpeed,
	MTH_tdxReal				_xLastNorm,
	char					_cIgnoreParsing,
	char					_cIgnoreInertia,
	MTH3D_tdstVector		*_p_stActualPos,
	MTH3D_tdstVector		*_p_stShiftingSrc,
	MTH3D_tdstVector		*_p_stDestPos,
	MTH3D_tdstVector		*_p_stShiftingResult
)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	MTH_tdxReal		xNorm, xNormSrc, xTmpNorm;
	MTH_tdxReal		xFactAccel, xInt;
	MTH_tdxReal		xNormWanted;
	ACP_tdxBool		bNormWantedIsModified = FALSE;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
	/*
	 * First test.
	 *------------
	 */
 
	/*
	 * No parsing or very short move. Destination move is the same than request one.
	 */
 
/*FB170798*/
	xNormWanted = xNormSrc = MTH3D_M_xNormVector(_p_stShiftingSrc);
 
	/*if((_cIgnoreParsing) || (MTH_M_bLessEqual(MTH3D_M_xNormVector(_p_stShiftingSrc),MTH_M_xFloatToReal(0.001f))))*/
	if((_cIgnoreParsing) || (MTH_M_bLessEqual(xNormSrc,MTH_M_xFloatToReal(0.001f))))
/*END FB*/
	{
/*FB170798*/
		/*MTH3D_M_vCopyVector(_p_stDestPos, _p_stActualPos);*/
		/*MTH3D_M_vAddVector(_p_stDestPos, _p_stDestPos, _p_stShiftingSrc);*/
		MTH3D_M_vAddVector(_p_stDestPos, _p_stActualPos, _p_stShiftingSrc);
/*END FB*/
		MTH3D_M_vCopyVector(_p_stShiftingResult, _p_stShiftingSrc);
		return;
	}
 
	/*
	 * No speed, no move.
	 */
	if (MTH_M_bEqualZero(_xSpeed))
	{
		MTH3D_M_vCopyVector(_p_stDestPos, _p_stActualPos);
		MTH3D_M_vNullVector(_p_stShiftingSrc);
		return;
	}
 
	/*
	 *	Serv position without keeping distance between computed point and target.
	 *	Norm of source vertex is just norm to max linear speed.
	 *---------------------------------------------------------------------------
	 */
	MTH3D_M_vCopyVector(_p_stShiftingResult, _p_stShiftingSrc);
/*FB170798*/
	/*xNormWanted = MTH3D_M_xNormVector(_p_stShiftingResult);*/
	/*if (MTH_M_bGreater(xNormWanted, _xSpeed))*/
	if (MTH_M_bGreater(xNormSrc, _xSpeed))
/*END FB*/
	
	{
		xNormWanted = _xSpeed;	
		bNormWantedIsModified = TRUE;
	}
	
	/*
	 *	Inertia.
	 *---------------------------------------------------------------------------
	 */
	if(!_cIgnoreInertia)
	{
		/*
		 *	To slowly decrease speed.
		 */
		xNorm = xNormSrc;/*FB170798 = MTH3D_M_xNormVector(_p_stShiftingSrc); END FB*/
		if (!MTH_M_bIsNull(_xDecSpeed))
		{
			xInt = MTH_M_xMul(_xSpeed, _xDecSpeed);
			if (MTH_M_bLess(xNorm,xInt))
			{
				xNorm = MTH_M_xDiv(xNorm, _xDecSpeed);
				if (MTH_M_bGreater(xNorm,MTH_M_xFloatToReal(0.001f)) && MTH_M_bLess(xNorm,xNormWanted))
				{
					bNormWantedIsModified = TRUE;
					xNormWanted = xNorm;
				}
			}
		}
 
		/*
		 *	To slowly increase speed.
		 */
		if (!MTH_M_bIsNull(_xIncSpeed))
		{
			xFactAccel = MTH_M_xDiv(_xSpeed, _xIncSpeed);
 
			if (MTH_M_bGreater(xNormWanted,(xTmpNorm = MTH_M_xAdd(_xLastNorm, xFactAccel))))
			{
				bNormWantedIsModified = TRUE;
				xNormWanted = xTmpNorm;
			}
		}
	}
 
	if(bNormWantedIsModified)
	{
/*FB170798*/
		/*MTH3D_M_vNormalizeVector(_p_stShiftingResult, _p_stShiftingResult);*/
		/*MTH3D_M_vMulScalarVector(_p_stShiftingResult, xNormWanted, _p_stShiftingResult);*/
		MTH3D_M_vMulScalarVector(_p_stShiftingResult,MTH_M_xDiv(xNormWanted,xNormSrc),_p_stShiftingResult);
/*END FB*/
 
	}
 
	/*
	 *	Update dest position.	
	 */
/*FB170798*/
	/*MTH3D_M_vCopyVector(_p_stDestPos, _p_stActualPos);*/
	/*MTH3D_M_vAddVector(_p_stDestPos, _p_stDestPos, _p_stShiftingResult);*/
	MTH3D_M_vAddVector(_p_stDestPos, _p_stActualPos, _p_stShiftingResult);
/*END FB*/
}
 
/*
 *=================================================================================================
 *	Parsing of an angle.
 *=================================================================================================
 */
MTH_tdxReal DNM_fn_xDynamicsCameraAngleParsing
(
	MTH_tdxReal				_xSpeed,
	MTH_tdxReal				_xIncSpeed,
	MTH_tdxReal				_xDecSpeed,
	MTH_tdxReal				_xRequestAngle,
	MTH_tdxReal				_xLastAngle,
	char					_cIgnoreParsing,
	char					_cIgnoreInertia
)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	MTH_tdxReal		xRetAngle;
	MTH_tdxReal		xInt, xAngle, xFactAccel;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
	/*
	 *	No parsing. Dest angle is requested one.
	 */
	if((_cIgnoreParsing) || ( MTH_M_bLess(_xRequestAngle, (MTH_M_xFloatToReal(0.001f)))))
	{
		return _xRequestAngle;
	}
 
	/*
	 *	Serve angle with speed.
	 *---------------------------------------------------------------------------------------------
	 */
	xRetAngle = _xRequestAngle;
 
	if (MTH_M_bGreater(xRetAngle, _xSpeed))
		xRetAngle = _xSpeed;
 
	/*
	 *	Inertia.
	 *---------------------------------------------------------------------------------------------
	 */
	if(!_cIgnoreInertia)
	{
		xAngle = _xRequestAngle;
		xInt = MTH_M_xMul(_xSpeed, _xDecSpeed);
		if (MTH_M_bLess(xAngle, xInt))
		{
			xAngle = MTH_M_xDiv(xAngle, _xDecSpeed);
			if (MTH_M_bGreater(xRetAngle, xAngle))
				xRetAngle = xAngle;
		}
 
		xFactAccel = MTH_M_xDiv(_xSpeed, _xIncSpeed);
		if (MTH_M_bGreater(xRetAngle,MTH_M_xAdd(_xLastAngle, xFactAccel)))
		{
			xRetAngle = MTH_M_xAdd(_xLastAngle, xFactAccel);
		}
	}
 
	return xRetAngle;
}
 
 
/*
 **************************************************************************************************
 **************************************************************************************************
 *	Parsing.
 **************************************************************************************************
 **************************************************************************************************
 */
 
 
/*
 *=================================================================================================
 *=================================================================================================
 MAIN FUNCTION
 *=================================================================================================
 *=================================================================================================
 */
DNM_tdstDynamics *DNM_p_stDynamicsCameraParsing
(
	DNM_tdstDynamics				*_p_stDynamics,
	HIE_tdxHandleToSuperObject		_h_SupObj,
	DNM_tdstParsingDatas			*_p_stExternData,
	DNM_tdxHandleToMecIdentityCard	_h_MecIdCard,	
	MTH_tdxReal						_xDT
)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	tdstMACDPID					*p_stDNMMACDPID;  
	DNM_tdstReport				*p_stReport;
	MTH3D_tdstVector			stPreviousPos;
	MTH3D_tdstVector			stShiftingSrc, stShiftingResult;
 
    MTH3D_tdstVector			stTempVector, stYVector;
	POS_tdstCompletePosition	*p_stGlobalMatrix;
	MTH_tdxReal					xSpeed, xAngle, xNorm;
	MTH3D_tdstVector			stTempVector1;
	MTH3D_tdstVector			stTempVector2;
	MTH_tdstRotation			stRotation;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* Variables temporaires*/
	MTH3D_tdstVector			stDNMTempVector;
 
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
#ifdef DNM_DEBUG
    if(_p_stDynamics == NULL)
        return NULL;
    if(_h_SupObj == NULL)
        return NULL;
    if(_p_stExternData == NULL)
        return NULL;
    if(!DNM_fn_bIsIdentityCardValid(_h_MecIdCard))
        return NULL;
#endif
 
    /* 
	 *	Get mechanic report and IA requests
	 */
	p_stReport = DNM_M_p_stDynamicsGetReport(_p_stDynamics);
	p_stDNMMACDPID = DNM_M_p_stDynamicsGetExternalDatas(_p_stDynamics);
 
	/*
	 *	Test if the request comes from AI
	 */
/*	if(_eRequest == LRM_eRequest_NoAction)*/
/*	{*/
/*		DNM_M_eDynamicsSetDynamicalState(_p_stDynamics, DNM_NoMove);*/
/*		return _p_stDynamics;*/
/*	}*/
/*	else*/
 
/* comment by jt 071098	*/
/*		DNM_M_eDynamicsSetDynamicalState(_p_stDynamics, DNM_Move);*/
 
	/* 
	 *	MOVE : Linear.
	 *---------------------------------------------------------------------------------------------
	 */
	p_stGlobalMatrix = HIE_fn_hGetSuperObjectGlobalMatrix(_h_SupObj);
	POS_fn_vGetTranslationVector(p_stGlobalMatrix, &stPreviousPos);
 
	/*
	 *	Get requested shifting pos.
	 */	
	MTH3D_M_vCopyVector(&stShiftingSrc, M_p_stMACDPIDGetMovePos(p_stDNMMACDPID));
 
	/* 
	 *	Get speed 
	 */
	xSpeed = M_xMACDPIDGetLinearSpeed(p_stDNMMACDPID);
	xSpeed = MTH_M_xMul(xSpeed, _xDT);
 
	/*
	 *	Parse.
	 */
	MTH3D_M_vCopyVector(&stTempVector, DNM_M_p_stReportGetLastMove(p_stReport));
	MTH3D_M_vCopyVector(&stTempVector1, DNM_M_p_stCPDGetPosition(_p_stExternData));
 
	DNM_fn_vDynamicsCameraPositionParsing
	(
		xSpeed,
		M_xMACDPIDGetLinearIncreaseSpeed(p_stDNMMACDPID),
		M_xMACDPIDGetLinearDecreaseSpeed(p_stDNMMACDPID),
 
		MTH3D_M_xNormVector(&stTempVector),
		(char) (M_uwMACDPIDGetFlags(p_stDNMMACDPID) & DNM_CAM_C_NoLinearParsing),
		(char) (M_uwMACDPIDGetFlags(p_stDNMMACDPID) & DNM_CAM_C_NoLinearInertia),
		&stPreviousPos,
		&stShiftingSrc,
		&stTempVector1,
		&stShiftingResult
	);
	MTH3D_M_vCopyVector(DNM_M_p_stCPDGetPosition(_p_stExternData), &stTempVector1);
 
	/* 
	 *	MOVE : Angular.
	 *---------------------------------------------------------------------------------------------
	 */
	if(!(M_uwMACDPIDGetFlags(p_stDNMMACDPID) & DNM_CAM_C_NoAngularParsing))
	{
		/* 
		 *	Get speed 
		 */
		xSpeed = M_xMACDPIDGetAngularSpeed(p_stDNMMACDPID);
		xSpeed = MTH_M_xMul(xSpeed, _xDT);
 
		/*
		 *	Get actual angle (reference point, wanted pos) and (reference point, actual pos)
		 */
		MTH3D_M_vSubVector(&stTempVector, M_p_stMACDPIDGetPosReferencePoint(p_stDNMMACDPID), &stPreviousPos);
		if (!MEC_fn_bIsNullVector(&stTempVector))
		{
			MTH3D_M_vNormalizeVector(&stTempVector, &stTempVector);
		}
 
		MTH3D_M_vCopyVector(&stTempVector1, M_p_stMACDPIDGetPosReferencePoint(p_stDNMMACDPID));
		MTH3D_M_vCopyVector(&stTempVector2, DNM_M_p_stCPDGetPosition(_p_stExternData));
 
		MTH3D_M_vSubVector(&stTempVector1, &stTempVector1, &stTempVector2);
		xNorm = MTH3D_M_xNormVector(&stTempVector1);
		if (!MTH_M_bEqualZero(xNorm))
		{
/*FB170798*/
			/*MTH3D_M_vNormalizeVector(&stTempVector1, &stTempVector1);*/
			MTH3D_M_vMulScalarVector(&stTempVector1,MTH_M_xInv(xNorm),&stTempVector1);
/*END FB*/
		}
 
		MTH_p_stRotationComputeRotationWith2Vectors(&stRotation, &stTempVector, &stTempVector1);
		xAngle = MTH_M_xRotationGetAngle(&stRotation);
 
		/*
		 *	Parse.
		 */
		DNM_M_xCPDGetLastAngleMoveCam(_p_stExternData) =
			DNM_fn_xDynamicsCameraAngleParsing
			(
				xSpeed,
				M_xMACDPIDGetAngularIncreaseSpeed(p_stDNMMACDPID),
				M_xMACDPIDGetAngularDecreaseSpeed(p_stDNMMACDPID),
				xAngle,
				DNM_M_xCPDGetLastAngleMoveCam(_p_stExternData),
				(char) (M_uwMACDPIDGetFlags(p_stDNMMACDPID) & DNM_CAM_C_NoAngularParsing),
				(char) (M_uwMACDPIDGetFlags(p_stDNMMACDPID) & DNM_CAM_C_NoAngularInertia)
			);
 
		/*
		 *	Compute new wanted position of camera.
		 */
		if(DNM_M_xCPDGetLastAngleMoveCam(_p_stExternData))
		{
			stRotation.m_xAngle = DNM_M_xCPDGetLastAngleMoveCam(_p_stExternData);
			MTH_p_stRotationMulVector(&stRotation, &stTempVector, &stTempVector);
		}
 
		MTH3D_M_vMulScalarVector(&stTempVector, xNorm, &stTempVector);
		MTH3D_M_vNegVector(&stTempVector, &stTempVector);
 
/*FB170798*/
		/*MTH3D_M_vAddVector(&stTempVector, &stTempVector, M_p_stMACDPIDGetPosReferencePoint(p_stDNMMACDPID));
		MTH3D_M_vCopyVector(&stDNMTempVector, &stTempVector);
		MTH3D_M_vCopyVector(DNM_M_p_stCPDGetPosition(_p_stExternData), &stDNMTempVector);*/
		MTH3D_M_vAddVector(DNM_M_p_stCPDGetPosition(_p_stExternData), &stTempVector, M_p_stMACDPIDGetPosReferencePoint(p_stDNMMACDPID));
/*END FB*/
	}
 
	/* Take care of last move to avoid dir change too fast */
	if
	(
		(!(M_uwMACDPIDGetFlags(p_stDNMMACDPID) & DNM_CAM_C_NoLinearParsing))
	||	(!(M_uwMACDPIDGetFlags(p_stDNMMACDPID) & DNM_CAM_C_NoAngularParsing))
	)
	{
		MTH3D_M_vSubVector(&stTempVector, DNM_M_p_stCPDGetPosition(_p_stExternData), &stPreviousPos);
		DNM_fn_vDynamicsTakeCareOfLastMove(&stTempVector, DNM_M_p_stReportGetLastMove(p_stReport), M_xMACDPIDGetCutAngleFactorPos(p_stDNMMACDPID));
		MTH3D_M_vAddVector(DNM_M_p_stCPDGetPosition(_p_stExternData), &stTempVector, &stPreviousPos);
	}
 
	/* 
	 * TARGET
	 *---------------------------------------------------------------------------------------------
	 */
 
	/*
	 *	In case of a fixed orientation, we compute a target above camera.
	 */
	if(M_uwMACDPIDGetFlags(p_stDNMMACDPID) & DNM_CAM_C_FixedOrientation)
	{
		/* Compute target with actual camera rotation matrix */
		POS_fn_vGetRotationMatrix(p_stGlobalMatrix, &stTempVector, &stYVector, &stTempVector);
		MTH3D_M_vNegVector(&stYVector, &stYVector);
		POS_fn_vGetTranslationVector(p_stGlobalMatrix, &stTempVector);
/*FB170798*/
		/*MTH3D_M_vAddVector(&stYVector, &stYVector, &stTempVector);
		MTH3D_M_vCopyVector(DNM_M_p_stCPDGetTarget(_p_stExternData), &stYVector);*/
		MTH3D_M_vAddVector(DNM_M_p_stCPDGetTarget(_p_stExternData), &stYVector, &stTempVector);
/*END FB*/
		/* Target was considered not to move */
		MTH3D_M_vNullVector(&stShiftingResult);
		MTH3D_M_vCopyVector(&stDNMTempVector, &stShiftingResult);
		DNM_M_vReportSetLastTgtMove(p_stReport, &stDNMTempVector);
	}
 
	/*
	 *	Else we really take care of target passed to meca.
	 */
	else
	{
		/*
		 *	Get previous target.
		 */
		MTH3D_M_vCopyVector(&stPreviousPos, DNM_M_p_stCPDGetTarget(_p_stExternData));
 
		/*
		 *	Get requested shifting pos.
		 */	
/*FB170798		*/
		/*MTH3D_M_vCopyVector(&stShiftingSrc, M_p_stMACDPIDGetTarget(p_stDNMMACDPID));
		MTH3D_M_vSubVector(&stShiftingSrc, &stShiftingSrc, &stPreviousPos);*/
		MTH3D_M_vSubVector(&stShiftingSrc, M_p_stMACDPIDGetTarget(p_stDNMMACDPID), &stPreviousPos);
/*END FB*/
 
		/* 
		 *	Get speed 
		 */
		xSpeed = M_xMACDPIDGetTargetSpeed(p_stDNMMACDPID);
		xSpeed = MTH_M_xMul(xSpeed, _xDT);
 
		/*
		 *	Parse.
		 */
		MTH3D_M_vCopyVector(&stTempVector, DNM_M_p_stReportGetLastTgtMove(p_stReport));
		MTH3D_M_vCopyVector(&stTempVector1, DNM_M_p_stCPDGetTarget(_p_stExternData));
		DNM_fn_vDynamicsCameraPositionParsing
		(
			xSpeed,
			M_xMACDPIDGetTargetIncreaseSpeed(p_stDNMMACDPID),
			M_xMACDPIDGetTargetDecreaseSpeed(p_stDNMMACDPID),
			MTH3D_M_xNormVector(&stTempVector),
			(char) (M_uwMACDPIDGetFlags(p_stDNMMACDPID) & DNM_CAM_C_NoTargetParsing),
			(char) (M_uwMACDPIDGetFlags(p_stDNMMACDPID) & DNM_CAM_C_NoTargetInertia),
			&stPreviousPos,
			&stShiftingSrc,
			&stTempVector1,
			&stShiftingResult
		);
 
		/* Take care of last move to avoid dir change too fast */
		DNM_fn_vDynamicsTakeCareOfLastMove(&stShiftingResult, DNM_M_p_stReportGetLastTgtMove(p_stReport), M_xMACDPIDGetCutAngleFactorTgt(p_stDNMMACDPID));
		MTH3D_M_vAddVector(DNM_M_p_stCPDGetTarget(_p_stExternData), &stShiftingResult, &stPreviousPos);
		DNM_M_vReportSetLastTgtMove(p_stReport, &stShiftingResult);
	}
 
	return _p_stDynamics;
}
 
  
/*
 **************************************************************************************************
 **************************************************************************************************
 *	Obstacle module for camera.
 *	Test if a collision occured. If yes, replace the camera to avoid the conflict...
 **************************************************************************************************
 **************************************************************************************************
 */
 
/*
 *=================================================================================================
 *	Camera collision
 *=================================================================================================
 */
/*DNM_tdstMecObstacle	a_stResultObstacle [COL_C_xMaxNumberOfCollisions];*/
long CAM_lHaveITakeSThgIntoTheMug
(
	DNM_tdstMecObstacle			*_p_stResultObstacle,  
	HIE_tdxHandleToSuperObject	_p_stSupObj,
	POS_tdstCompletePosition	*_p_stStartPosition,
	POS_tdstCompletePosition	*_p_stEndPosition
)
{
	long							lCounter;
	long							lCollision;
	DNM_tdstMecObstacle				*p_stObstacle;
	DNM_tdstMecObstacle				*p_stReturnObstacle;
	GMT_tdxHandleToGameMaterial		hGameMat;
	GMT_tdxHandleToCollideMaterial	hCollMat;
	GMT_tdxMask						xCollMask;
	ACP_tdxBool						bWater;
	char							bLessDetected = 0;
	MTH3D_tdstVector				stStartVec, stTempVec;
 
	POS_fn_vGetTranslationVector(_p_stStartPosition, &stStartVec);
	POS_fn_vGetTranslationVector(_p_stEndPosition, &stTempVec);
 
	lCollision = COL_lHaveITakeSThgIntoTheMug ( g_a_stObstacle, _p_stSupObj, _p_stStartPosition, _p_stEndPosition );

	for (lCounter=0; lCounter<lCollision; lCounter++)
	{
		bWater = FALSE;
		
		p_stObstacle = g_a_stObstacle+lCounter;
 
		/* water */
		hGameMat = DNM_M_hObstacleGetCollidedMaterial(p_stObstacle);
		if (GMT_fn_b_ulIsValid(hGameMat))
		{
			hCollMat = GMT_fn_hGetCollideMaterial(hGameMat);
			if (hCollMat != (GMT_tdxHandleToCollideMaterial)GMT_C_InvalidCollideMaterial)
			{
				xCollMask = GMT_fn_hGetCollideMaterialIdentifier(hCollMat);
				bWater = ((xCollMask & 0xC000) != 0);
			}
		}

		if (!bWater)
		{
			MTH3D_M_vSubVector(&stTempVec, DNM_M_p_stObstacleGetContact(p_stObstacle), &stStartVec);
			if(MTH_M_bLessEqual(MTH3D_M_xDotProductVector(&stTempVec, DNM_M_p_stObstacleGetNorm(p_stObstacle)), MTH_C_ZERO))
			{
				if(MTH_M_bGreaterEqual(DNM_M_xObstacleGetRate(p_stObstacle),MTH_C_ZERO))
				{
					if 
					(
//						MTH_M_bGreaterEqual(MTH_M_xAbs(MTH3D_M_xDotProductVector(DNM_M_pObstacleGetZoneMove(p_stObstacle), DNM_M_p_stObstacleGetNorm(p_stObstacle))), MTH_M_xDoubleToReal(0.005) )
//					||	MTH_M_bGreater(DNM_M_xObstacleGetRate(p_stObstacle),MTH_M_xDoubleToReal(0.1))	
						MTH_M_bLessEqual(MTH3D_M_xDotProductVector(DNM_M_pObstacleGetZoneMove(p_stObstacle), DNM_M_p_stObstacleGetNorm(p_stObstacle)),MTH_M_xDoubleToReal(-0.005))
					)
					{
						*_p_stResultObstacle = *p_stObstacle;
						return 1;
					}
				}
				else
				{
					bLessDetected = 1;
					p_stReturnObstacle = p_stObstacle;
				}
			}
		}
	}
 
	if(bLessDetected)
	{
		*_p_stResultObstacle = *p_stReturnObstacle;
		return 1;
	}
	return 0;
}
 
/*
 *=================================================================================================
 *	Test if vector is NULL with an epsilon.
 *=================================================================================================
 */
ACP_tdxBool CAM_fn_bVectorIsNull(MTH3D_tdstVector *_p_stVector, MTH_tdxReal _xEpsilon)
{
	register MTH_tdxReal xX,xY,xZ,xVal;
 
	/* compute vector norm */
	xX = MTH_M_xAbs(MTH3D_M_xGetXofVector(_p_stVector));
	xY = MTH_M_xAbs(MTH3D_M_xGetYofVector(_p_stVector));	
	xZ = MTH_M_xAbs(MTH3D_M_xGetZofVector(_p_stVector));	
 
	xVal = MTH_M_xAdd(xX,MTH_M_xAdd(xY,xZ));
 
	/* test and return result */
	return (MTH_M_bIsNullWithEpsilon(xVal,_xEpsilon));
}
 
/*
 *=================================================================================================
 *	Determin if two vectors are equal with an epsilon.
 *=================================================================================================
 */
char MTH_bCompareVectors(MTH3D_tdstVector *_p_stFirst, MTH3D_tdstVector *_p_stSecond, MTH_tdxReal _xEpsilon)
{
/*FB170798*/
	MTH3D_tdstVector	stTempPoint;
 
	MTH3D_M_vCopyVector(&stTempPoint, _p_stFirst);
	MTH3D_M_vSubVector(&stTempPoint, &stTempPoint, _p_stSecond);
	return CAM_fn_bVectorIsNull(&stTempPoint, _xEpsilon);
/* ce n'est pas tout a fait la meme chose
	return (
			MTH_M_bEqualWithEpsilon(MTH3D_M_xGetXofVector(_p_stFirst),MTH3D_M_xGetXofVector(_p_stSecond),_xEpsilon)
			&& MTH_M_bEqualWithEpsilon(MTH3D_M_xGetYofVector(_p_stFirst),MTH3D_M_xGetYofVector(_p_stSecond),_xEpsilon)
			&& MTH_M_bEqualWithEpsilon(MTH3D_M_xGetZofVector(_p_stFirst),MTH3D_M_xGetZofVector(_p_stSecond),_xEpsilon)
			);
*/
/*END FB*/
 
}
 
/*
 *=================================================================================================
 *	Determin if normal has already been put in array.
 *=================================================================================================
 */
char CAM_fn_cNormalHere(MTH3D_tdstVector *_p_stArray, MTH3D_tdstVector *_p_stVector, unsigned int uiNum)
{
	unsigned int	uiIndex;
 
	for(uiIndex = 0; uiIndex < uiNum; uiIndex++)
	{
		if(MTH3D_M_bEqualVector(_p_stArray + uiIndex, _p_stVector))
			return 1;
	}
	return 0;
}
 
/*
 *=================================================================================================
 *	Main obstacle function.
 *=================================================================================================
 */
#define MTH_C_PiBy6                MTH_M_xFloatToReal(0.5235987755983f)
#define	MYH_C_PiMinusPiBy6		   MTH_M_xSub(MTH_C_Pi, MTH_C_PiBy6)	
 
/*extern fn_vSectInfoInitForSingleCharacter(HIE_tdxHandleToSuperObject, MTH3D_tdstVector *);*/
struct DNM_stMecObstacle *DNM_p_stObstacleCameraObstacle
(
	struct DNM_stMecObstacle		*_p_stObstacle,
	struct DNM_stDynamics		*_p_stDynamics,
	DNM_tdstParsingDatas		*_p_stExternData,
	HIE_tdxHandleToSuperObject	_hSupObj,
	MTH3D_tdstVector			*_p_stWantedPosition,
	MTH3D_tdstVector			*_p_stNewPosition,
	MTH_tdxReal					_xDT
)
{ 
	/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
#define CAM_C_MaxDNMNormals	5
#define CAM_M_RestoreLastPos()\
	{\
	MTH3D_M_vCopyVector(_p_stNewPosition, &stInitialOrgPoint);\
	MTH3D_M_vCopyVector(&stOrgPoint, &stInitialOrgPoint);\
	POS_fn_vSetTranslationVector(HIE_fn_hGetSuperObjectMatrix(_hSupObj), _p_stNewPosition);\
}
	/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
	DNM_tdstReport				*p_stReport;
	tdstMACDPID					*p_stMACDPID;
	
	MTH_tdstRotation			stRotation;
	
	long						lNbCollisions;
	MTH_tdxReal					xAngle, xNorm;
	MTH3D_tdstVector			a_stNormals[CAM_C_MaxDNMNormals];
	MTH3D_tdstVector			a_stCross[CAM_C_MaxDNMNormals];
	unsigned int				uiNumNormals = 0, uiNumCross = 0;
	char						cWantSecond, cNormalHere;
	
	MTH3D_tdstVector			a_stVectors[11];
	MTH3D_tdstVector			*p_stObstacleNorm = a_stVectors;
	MTH3D_tdstVector			*p_stProjectionVector = a_stVectors + 1;
	MTH3D_tdstVector			*p_stTempVector = a_stVectors + 2;
	MTH3D_tdstVector			*p_stTempVector1 = a_stVectors + 3;
	MTH3D_tdstVector			*p_stComputedCurrentPosition = a_stVectors + 4;
	MTH3D_tdstVector			*p_stComputedWantedPosition = a_stVectors + 5;
	MTH3D_tdstVector			*p_stLastMoveVector = a_stVectors + 6;
	MTH3D_tdstVector			*p_stCurrentPosition = a_stVectors + 7;
	MTH3D_tdstVector			*p_stImpactVector = a_stVectors + 8;
	MTH3D_tdstVector			*p_stProjectedVector = a_stVectors + 9;
	MTH3D_tdstVector			*p_stLastComputedCurrentPosition = a_stVectors + 10;
	
	POS_tdstCompletePosition	stBeginMatrix;
	POS_tdstCompletePosition	stEndMatrix;
	/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
	/* Variables temporaires*/
	MTH3D_tdstVector			stDNMTempVector;
	/*FB170798*/
	MTH_tdxReal					xNormTempVector;
	/*END FB*/
	
	/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	
#ifdef DNM_DEBUG 
    if(_p_stDynamics == NULL)
        return NULL;
#endif
	
		/* 
		*	Get mechanic report and AI requests.
	*/
	p_stReport = DNM_M_p_stDynamicsGetReport(_p_stDynamics);
	p_stMACDPID = DNM_M_p_stDynamicsGetExternalDatas(_p_stDynamics);
	
    /* 
	*	Get the actual position of the camera.
	*/
    POS_fn_vGetTranslationVector(HIE_fn_hGetSuperObjectGlobalMatrix(_hSupObj), p_stCurrentPosition); 
    
	/*
	*	First init computed positions.
	*/
	MTH3D_M_vCopyVector(p_stComputedCurrentPosition, p_stCurrentPosition);
	MTH3D_M_vCopyVector(p_stComputedWantedPosition, _p_stWantedPosition);
	
	/*
	*	The last real move position.
	*/
	MTH3D_M_vCopyVector(p_stLastMoveVector, DNM_M_p_stReportGetLastMove(p_stReport));
	
	if (!MEC_fn_bIsNullVector(p_stLastMoveVector))
		MTH3D_M_vNormalizeVector(p_stLastMoveVector, p_stLastMoveVector);
	
		/*
		*	Make the loop to test all the collisions to find a new good position.
		*	If we can't find it, camera doesn't move.
	*/
	DNM_M_bReportSetCollisionFlagToFalse(p_stReport);
	DNM_M_ulReportSetSurfaceState(p_stReport, 0);
	cWantSecond = 0;
	
	/*
	*	Flags, no collision : Set DNM wanted position to initial wanted one and exit.
	*	We force sector to be recompute.
	*/
	if (M_uwMACDPIDGetFlags(p_stMACDPID) & DNM_CAM_C_NoObstacle)
	{
		MTH3D_M_vCopyVector(p_stComputedCurrentPosition, _p_stWantedPosition);
	}
	else
	{
		POS_fn_vSetIdentityMatrix(&stBeginMatrix);
		POS_fn_vSetIdentityMatrix(&stEndMatrix);

/* ANNECY MT - 28/07/99 { */
		/* Flags, No Collision When Not Moving */
		if ( ! MTH_bCompareVectors(p_stComputedCurrentPosition, p_stComputedWantedPosition, MTH_M_xFloatToReal(0.02f)) || 
			! (M_uwMACDPIDGetFlags(p_stMACDPID) & DNM_CAM_C_NoCollisionWhenNotMoving) )
/* END ANNECY MT } */
		{
			do
			{
			/* 
			*	Get a collision.
			*	If there's no collision (or no more), we exit cause current position of camera is the
			*	good one.
				*/
				POS_fn_vSetTranslationVector(&stBeginMatrix, p_stComputedCurrentPosition);
				POS_fn_vSetTranslationVector(&stEndMatrix, p_stComputedWantedPosition);
				
				lNbCollisions =  CAM_lHaveITakeSThgIntoTheMug
					(
					_p_stObstacle,
					_hSupObj,            
					&stBeginMatrix,  
					&stEndMatrix
					);
				
					/*
					* No collision : Exit (p_stDNMComputedWantedPosition correct)
				*/
				if(lNbCollisions <= 0)
				{
					MTH3D_M_vCopyVector(p_stComputedCurrentPosition, p_stComputedWantedPosition);
					break;
				}
				
				/*
				*	COLLISION DETECTED
				*-----------------------------------------------------------------------------------------
				*/
				
				/* 
				*	Get the norm of the obstacle.
				*/
				DNM_M_ObstacleSetNorm(&p_stReport->m_stObstacle, DNM_M_p_stObstacleGetNorm(_p_stObstacle));
				MTH3D_M_vCopyVector(p_stObstacleNorm, DNM_M_p_stObstacleGetNorm(_p_stObstacle));
				
				/* 
				*	If this is the first collision, we set bIsWall to indicate if obstacle is almost opposite
				*	to camera move.
				*/
				
				MTH3D_M_vSubVector(p_stTempVector, _p_stWantedPosition, p_stCurrentPosition);
				xNormTempVector = MTH3D_M_xNormVector(p_stTempVector);
				
				if(!(DNM_M_bReportCollision(p_stReport)) && ! MTH_M_bIsNull(xNormTempVector))
				{
					DNM_M_bReportSetWallFlagToFalse(p_stReport);
					
					/*xAngle = MTH_M_xDiv(MTH3D_M_xDotProductVector(p_stTempVector, p_stObstacleNorm), MTH3D_M_xNormVector(p_stTempVector));*/
					xAngle = MTH_M_xDiv(MTH3D_M_xDotProductVector(p_stTempVector, p_stObstacleNorm), xNormTempVector);
					if (MTH_M_bLess(xAngle,MTH_M_xFloatToReal(-0.7f))) 
					{
					/*
					*	We must have camera move that goes in the direction of the reference point.
						*/
						MTH3D_M_vSubVector(p_stTempVector1, M_p_stMACDPIDGetPosReferencePoint(p_stMACDPID), p_stCurrentPosition);
						/*FB170798*/
						/*MTH3D_M_vNormalizeVector(p_stTempVector, p_stTempVector);*/
						MTH3D_M_vMulScalarVector(p_stTempVector,MTH_M_xInv(xNormTempVector),p_stTempVector);
						/*END FB*/
						MTH3D_M_vNormalizeVector(p_stTempVector1, p_stTempVector1);
						xAngle = MTH3D_M_xDotProductVector(p_stTempVector, p_stTempVector1);
						if (MTH_M_bGreater(xAngle,MTH_M_xFloatToReal(0.7f))) 
							DNM_M_bReportSetWallFlagToTrue(p_stReport);
					}
				}
				
				/* Set flag */
				DNM_M_bReportSetCollisionFlagToTrue(p_stReport);
				DNM_M_ulReportSetSurfaceState(p_stReport, C_WOT_ulWall);
				
				/*!!!!!!!!!!!!!!!!!!*/
				/* Special case, -1 */
				/*!!!!!!!!!!!!!!!!!!*/
				if(MTH_M_bLess(DNM_M_xObstacleGetRate(_p_stObstacle),MTH_C_ZERO))
				{
					DNM_M_bReportSetCollisionFlagToTrue(p_stReport);
					xAngle = MTH3D_M_xNormVector(&_p_stObstacle->m_stTranslation);
					/*FB170798*/
					/*MTH3D_M_vNormalizeVector(&stDNMTempVector, &_p_stObstacle->m_stTranslation);*/
					MTH3D_M_vMulScalarVector(&stDNMTempVector,MTH_M_xInv(xAngle),&_p_stObstacle->m_stTranslation);
					/*END FB*/
					xAngle = MTH_M_xAdd(xAngle, MTH_M_xFloatToReal(0.02f));
					MTH3D_M_vMulScalarVector(&stDNMTempVector, xAngle, &stDNMTempVector);
					MTH3D_M_vAddVector(p_stComputedCurrentPosition, &stDNMTempVector,p_stComputedWantedPosition);
					break;
				}
				
				/*
				*	Get the last position of camera before collision.
				*/
				MTH3D_M_vSubVector(p_stImpactVector, p_stComputedWantedPosition, p_stComputedCurrentPosition);
				MTH3D_M_vMulScalarVector(p_stImpactVector, DNM_M_xObstacleGetRate(_p_stObstacle), p_stImpactVector);
				MTH3D_M_vAddVector(p_stImpactVector, p_stImpactVector, p_stComputedCurrentPosition);
				
				/*
				*	Projection if face has not been hit once.
				*/
				cNormalHere = CAM_fn_cNormalHere(a_stNormals, p_stObstacleNorm, uiNumNormals);
				if(!cNormalHere)
				{
					/* Too much normal !!! => Restore current position to camera */
					if(uiNumNormals == CAM_C_MaxDNMNormals)
					{
						MTH3D_M_vCopyVector(p_stComputedCurrentPosition, p_stCurrentPosition);
						break;
					}
					
					/* Add normal in array */
					MTH3D_M_vCopyVector(&a_stNormals[uiNumNormals], p_stObstacleNorm);
					uiNumNormals++;
					
					/* 
					* Compute impact vector.
					*/
					MTH3D_M_vSubVector(p_stProjectionVector, p_stImpactVector, DNM_M_p_stObstacleGetContact(_p_stObstacle));
					MTH3D_M_vNormalizeVector(p_stProjectionVector, p_stProjectionVector);
					
					/*
					* Choose between normal and impact vector. If difference between the two vectors is small,
					* we took normal.
					*/
					MTH_p_stRotationComputeRotationWith2Vectors(&stRotation, p_stProjectionVector, p_stObstacleNorm);
					xAngle = MTH_M_xRotationGetAngle(&stRotation);
					
					if (MTH_M_bLess(xAngle, MTH_C_PiBy6))
					{
						MTH3D_M_vCopyVector(p_stProjectionVector, p_stObstacleNorm);
					}
					
					/*
					* Go to a given distance of face.
					*/
					/*FB170798*/
					/*MTH3D_M_vCopyVector(p_stTempVector, p_stImpactVector);*/
					/*MTH3D_M_vSubVector(p_stTempVector, p_stTempVector, p_stComputedCurrentPosition);*/
					MTH3D_M_vSubVector(p_stTempVector, p_stImpactVector, p_stComputedCurrentPosition);
					/*END FFB*/
					if(!MEC_fn_bIsNullVector(p_stTempVector))
					{
						/*FB170798*/
						xNormTempVector = MTH3D_M_xNormVector(p_stTempVector);
						/*xNorm =  MTH_M_xSub(MTH3D_M_xNormVector(p_stTempVector), MTH_M_xFloatToReal(0.02f));*/
						xNorm =  MTH_M_xSub(xNormTempVector, MTH_M_xFloatToReal(0.02f));
						/*END FB			*/
						if (MTH_M_bLessZero(xNorm))
						{
							MTH3D_M_vNullVector(p_stTempVector);
						}
						else
						{				
							/*FB170798						*/
							/*MTH3D_M_vNormalizeVector(p_stTempVector, p_stTempVector);
							MTH3D_M_vMulScalarVector(p_stTempVector, xNorm, p_stTempVector);*/
							MTH3D_M_vMulScalarVector(p_stTempVector, MTH_M_xDiv(xNorm,xNormTempVector), p_stTempVector);
							/*END FB*/
						}
						MTH3D_M_vCopyVector(p_stLastComputedCurrentPosition, p_stComputedCurrentPosition);
						MTH3D_M_vAddVector(p_stComputedCurrentPosition, p_stComputedCurrentPosition, p_stTempVector);
					}
					
					/*
					*	Get the move vector between the current computed position and the
					*	initial wanted one.
					*/
					MTH3D_M_vSubVector(p_stProjectedVector, _p_stWantedPosition, p_stComputedCurrentPosition);
					
					/*
					*	Project the newly computed move vector to the plan of the collision vector.
					*/
					/*FB170798*/
					/*MTH_p_stVectorComputeProjection(p_stProjectedVector, p_stProjectedVector, p_stProjectionVector);*/
					MTH_fn_vVectorPlaneProjection(p_stProjectedVector, p_stProjectedVector,p_stProjectionVector,p_stProjectionVector);
					/*END FB*/
					
					/*
					* Check for the angle if last move vector is not NULL.
					* If angle between this projected vector and the last move vector is too small, we
					* don't move.
					*/
					if((!MEC_fn_bIsNullVector(p_stLastMoveVector)) && (!MEC_fn_bIsNullVector(p_stProjectedVector)))
					{
						MTH3D_M_vNormalizeVector(p_stTempVector, p_stProjectedVector);
						MTH_p_stRotationComputeRotationWith2Vectors(&stRotation, p_stTempVector, p_stLastMoveVector);
						xAngle = MTH_M_xRotationGetAngle(&stRotation);
						if (MTH_M_bGreater(xAngle,MYH_C_PiMinusPiBy6))
							cWantSecond = 1;
					}
				}
				
				/*
				* The last thing.
				*/
				if((cWantSecond) || (cNormalHere))
				{
					if
						(
						(uiNumNormals <= 1)
						||	(MTH3D_M_bEqualVector(p_stObstacleNorm, &a_stNormals[uiNumNormals - 1]))
						||	(uiNumCross == CAM_C_MaxDNMNormals)
						)
					{
						DNM_M_bReportSetWallFlagToTrue(p_stReport);
						break;
					}
					
					/*
					* Compute move vector.
					*/
					MTH3D_M_vCopyVector(p_stProjectedVector, _p_stWantedPosition);
					MTH3D_M_vSubVector(p_stProjectedVector, p_stProjectedVector, p_stComputedCurrentPosition);
					
					/*
					* Compute direction (cross product).
					*/
					MTH3D_M_vCrossProductVector(p_stTempVector, &a_stNormals[uiNumNormals - 1], p_stObstacleNorm);
					MTH3D_M_vNormalizeVector(p_stTempVector, p_stTempVector);
					
					/*
					* If cross product has already been computed, exit.
					*/
					if(CAM_fn_cNormalHere(a_stCross, p_stTempVector, uiNumCross))
						break;
					
						/*
						* Add cross product in array.
					*/
					MTH3D_M_vCopyVector(&a_stCross[uiNumCross], p_stTempVector);
					uiNumCross++;
					
					/*	
					* Compute the direction to move.
					*/
					/*FB170798*/
					/*MTH_p_stVectorComputeProjection(p_stTempVector1, p_stProjectedVector, p_stTempVector);*/
					MTH_fn_vVectorPlaneProjection(p_stTempVector1, p_stProjectedVector,p_stTempVector,p_stTempVector);
					/*END FB*/
					MTH3D_M_vSubVector(p_stProjectedVector, p_stProjectedVector, p_stTempVector1);
				}
				
				if (MTH3D_M_bIsNullVector(p_stProjectedVector))
				{
					MTH3D_M_vNullVector(p_stLastMoveVector);
				}
				else
				{
				/*
				*	Make the new position.
					*/
					MTH3D_M_vAddVector(p_stComputedWantedPosition, p_stComputedCurrentPosition, p_stProjectedVector);
					
					/*
					* Make the new last move vector.
					*/
					MTH3D_M_vNormalizeVector(p_stLastMoveVector, p_stProjectedVector);
				}
				
/* ANNECY MT - 28/07/99 { */
				/* No move if a too small request */
				if(MTH_bCompareVectors(p_stComputedCurrentPosition, p_stComputedWantedPosition, MTH_M_xFloatToReal(0.02f)))
					break;
/* END ANNECY MT } */
				
			} while(1);
		}
	}
	
	/*
	*	New move vector.
	*/
	MTH3D_M_vSubVector(p_stTempVector, p_stComputedCurrentPosition, p_stCurrentPosition);
	
	/*
	*	New last move vector.
	*/
	/*FB170798*/
	/*MTH3D_M_vCopyVector(&stDNMTempVector, p_stTempVector);
	DNM_M_vReportSetLastMove(p_stReport, &stDNMTempVector);*/
	DNM_M_vReportSetLastMove(p_stReport, p_stTempVector);
	/*END FB*/
	
	/*
	*	New angle between current pos and previous one (with reference position point).
	*/
	xAngle = MTH_C_ZERO;
	MTH3D_M_vSubVector(p_stTempVector, M_p_stMACDPIDGetPosReferencePoint(p_stMACDPID), p_stCurrentPosition);
	if(!MEC_fn_bIsNullVector(p_stTempVector))
	{
		MTH3D_M_vNormalizeVector(p_stTempVector, p_stTempVector);
		
		MTH3D_M_vSubVector(p_stTempVector1, M_p_stMACDPIDGetPosReferencePoint(p_stMACDPID), p_stComputedCurrentPosition);
		if(!MEC_fn_bIsNullVector(p_stTempVector1))
		{
			MTH3D_M_vNormalizeVector(p_stTempVector1, p_stTempVector1);
			MTH_p_stRotationComputeRotationWith2Vectors(&stRotation, p_stTempVector, p_stTempVector1);
			xAngle = MTH_M_xRotationGetAngle(&stRotation);
		}
	}
	DNM_M_xCPDGetLastAngleMoveCam(_p_stExternData) = xAngle;
	
	/*
	*	Set _p_stNewPosition to computed one.
	*	Set local matrix to this position.
	*/
	MTH3D_M_vCopyVector(_p_stNewPosition, p_stComputedCurrentPosition);
	
	return _p_stObstacle;
}
 
 
/*
 **************************************************************************************************
 **************************************************************************************************
 *	Update camera matrix.
 **************************************************************************************************
 **************************************************************************************************
 */
 
 
/*
 *=================================================================================================
 * Compute camera system axis
 *=================================================================================================
 */
void CAM_fn_vComputeCameraSystemAxis
(
	struct DNM_stDynamics		*_p_stDynamics,
	HIE_tdxHandleToSuperObject	_h_SupObj,
	MTH3D_tdstVector			*_p_stNewPosition,
	MTH3D_tdstVector			*_p_stNewTarget,
	MTH3D_tdstVector			*_p_stAxisX,
	MTH3D_tdstVector			*_p_stAxisY,
	MTH3D_tdstVector			*_p_stAxisZ
)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	MTH3D_tdstVector	stXVector, stYVector;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
	MTH3D_M_vSubVector(_p_stAxisY, _p_stNewPosition, _p_stNewTarget);
	SAF_M_AssertWithMsg(!MEC_fn_bIsNullVector(_p_stAxisY), "Position = Target impossible !");
 
/*FB170798*/
	MTH3D_M_vCopyVector(&stYVector,_p_stAxisY);
/*END FB*/
	if (!MEC_fn_bIsNullVector(_p_stAxisY))
		MTH3D_M_vNormalizeVector(_p_stAxisY, _p_stAxisY);
 
	/* If too close from Z axis, take last camera Z axis as reference */
/*FB170798*/
	/*MTH3D_M_vSubVector(&stYVector, _p_stNewPosition, _p_stNewTarget);*/
	/*MTH_p_stVectorComputeProjection(&stYVector, &stYVector, _p_stAxisZ);*/
	MTH_fn_vVectorPlaneProjection(&stYVector, &stYVector,_p_stAxisZ,_p_stAxisZ);
/*END FB*/
 
	if (MTH_M_bLess(MTH3D_M_xNormVector(&stYVector),MTH_M_xFloatToReal(0.2f)))
		POS_fn_vGetRotationMatrix(HIE_fn_hGetSuperObjectGlobalMatrix(_h_SupObj), &stXVector, &stYVector, _p_stAxisZ);
 
	/* To be sure that Y and Z are not colinear */
	MTH3D_M_vCrossProductVectorWithoutBuffer(_p_stAxisX, _p_stAxisY, _p_stAxisZ);
	if (MTH3D_M_bIsNullVector(_p_stAxisX))
	{
		POS_fn_vGetRotationMatrix(HIE_fn_hGetSuperObjectGlobalMatrix(_h_SupObj), &stXVector, &stYVector, _p_stAxisZ);
		MTH3D_M_vCrossProductVectorWithoutBuffer(_p_stAxisX, _p_stAxisY, _p_stAxisZ);
	}
	MTH3D_M_vNormalizeVector(_p_stAxisX, _p_stAxisX);
 
	MTH3D_M_vCrossProductVectorWithoutBuffer(_p_stAxisZ, _p_stAxisX, _p_stAxisY);
	MTH3D_M_vNormalizeVector(_p_stAxisZ, _p_stAxisZ);
}
 
/*
 *=================================================================================================
 * Main update function.
 *=================================================================================================
 */
struct DNM_stDynamics *DNM_p_stDynamicsCameraUpdate
(
	struct DNM_stDynamics		*_p_stDynamics,
	DNM_tdstParsingDatas		*_p_stExternData,
	HIE_tdxHandleToSuperObject	_h_SupObj,
	struct DNM_stMecObstacle	*_p_stObstacle,
	MTH3D_tdstVector			*_p_stNewPosition,
	MTH3D_tdstVector			*_p_stNewTarget
)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	DNM_tdstReport				*p_stReport;
	tdstMACDPID					*p_stMACDPID;
	MTH3D_tdstVector			stXVector, stYVector, stZVector;
/*	MTH3D_tdstVector			a_stVectors[2];*/
	/* LOL MTH3D_tdstVector			*p_stTempVector = a_stVectors;*/
	/* LOL MTH3D_tdstVector			*p_stTempVector1 = a_stVectors + 1;*/
 
/*FB170798*/
	POS_tdxHandleToPosition		hMatrix;
/*END FB*/
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
#ifdef DNM_DEBUG
    if(_p_stDynamics == NULL)
        return NULL;
#endif
 
	p_stMACDPID = DNM_M_p_stDynamicsGetExternalDatas(_p_stDynamics);
	p_stReport = DNM_M_p_stDynamicsGetReport(_p_stDynamics);
 
	/*
	 *	Fixed oritentation. Camera keep its current orientation.
	 */ 
/*FB170798*/
	hMatrix = HIE_fn_hGetSuperObjectMatrix(_h_SupObj);
/*END FB*/
 
	if(!(M_uwMACDPIDGetFlags(p_stMACDPID) & DNM_CAM_C_FixedOrientation))
	{
		MTH3D_M_vCopyVector(&stZVector, M_p_stMACDPIDGetRefAxisZ(p_stMACDPID));
 
		/* 
		 * Compute camera system axis.
		 *----------------------------
		 */
		CAM_fn_vComputeCameraSystemAxis
		(
			_p_stDynamics,
			_h_SupObj,
			_p_stNewPosition,
			_p_stNewTarget,
			&stXVector, 
			&stYVector, 
			&stZVector
		);
 
		/*
		 *	Update local matrix for target.
		 */
/*FB170798*/
		/*POS_fn_vSetRotationMatrix(HIE_fn_hGetSuperObjectMatrix(_h_SupObj), &stXVector, &stYVector, &stZVector);*/
		POS_fn_vSetRotationMatrix(hMatrix, &stXVector, &stYVector, &stZVector);
/*END FB*/
	}
 
	/*
	 * Update local matrix for position
	 *----------------------------------
	 */
/*FB170798*/
	/*POS_fn_vSetTranslationVector(HIE_fn_hGetSuperObjectMatrix(_h_SupObj), _p_stNewPosition);*/
	POS_fn_vSetTranslationVector(hMatrix, _p_stNewPosition);
/*END FB*/
 
	/* 
	 * Update global matrix 
	 *----------------------
	 */
/*FB170798*/
	/*POS_fn_vCopyMatrix(HIE_fn_hGetSuperObjectGlobalMatrix(_h_SupObj), HIE_fn_hGetSuperObjectMatrix(_h_SupObj));*/
	POS_fn_vCopyMatrix(HIE_fn_hGetSuperObjectGlobalMatrix(_h_SupObj), hMatrix);
/*END FB*/
 
    return _p_stDynamics;
}
 
/*
 **************************************************************************************************
 **************************************************************************************************
 *	Main meca function.
 **************************************************************************************************
 **************************************************************************************************
 */
DNM_tdstDynamics *DNM_p_stDynamicsCameraMechanics
(
	DNM_tdstDynamics				*_p_stDynamics,
	HIE_tdxHandleToSuperObject		_hSupObj,
	DNM_tdstParsingDatas			*_p_stParsingDatas,
	DNM_tdxHandleToMecIdentityCard	_h_MecIdCard,
	MTH_tdxReal						_xDT
)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
	DNM_tdstMecObstacle		stObstacle;
	MTH3D_tdstVector		stVectorVertical;
	MTH3D_tdstVector		stPosition, stTarget, stPosition1;
	tdstMACDPID				*p_stMACDPID;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
   
#ifdef DNM_DEBUG
	if(_p_stDynamics == NULL)
		return NULL;
	if(_hSupObj == NULL)
		return NULL;
#endif
 
   /* 
	* Parsing part.
	*---------------
	*/
	DNM_p_stDynamicsCameraParsing
	(
		_p_stDynamics,
		_hSupObj,
		_p_stParsingDatas,
		_h_MecIdCard,	
		_xDT
	);
 
 
	p_stMACDPID = DNM_M_p_stDynamicsGetExternalDatas(_p_stDynamics);
	MTH3D_M_vSetVectorElements(&stVectorVertical, MTH_C_ZERO, MTH_C_ZERO, MTH_C_MinusONE);
   /* 
	* Obstacle part.
	*---------------
	*/
	MTH3D_M_vCopyVector(&stPosition1, DNM_M_p_stCPDGetPosition(_p_stParsingDatas));
	if
	(
		DNM_p_stObstacleCameraObstacle
		(
			&stObstacle,
			_p_stDynamics,
			_p_stParsingDatas,
			_hSupObj,
			&stPosition1,
			&stPosition,
			_xDT
		) == NULL
	)
		return NULL;
	MTH3D_M_vCopyVector(DNM_M_p_stCPDGetPosition(_p_stParsingDatas), &stPosition1 );
 
   /* 
    * Update part.
	*-------------
    */
	MTH3D_M_vCopyVector(&stTarget, DNM_M_p_stCPDGetTarget(_p_stParsingDatas));
	if
	(
		DNM_p_stDynamicsCameraUpdate
		(
			_p_stDynamics,
			_p_stParsingDatas,
			_hSupObj,
			&stObstacle,
			&stPosition,
			&stTarget
		) == NULL
	)
		return NULL;
	MTH3D_M_vCopyVector( DNM_M_p_stCPDGetTarget(_p_stParsingDatas), &stTarget);
   
	return _p_stDynamics;
}
