/*
 *=======================================================================================
 * Name        : DNMMTH.c
 * Author      : CB / YLG / VL               
 * Description : 
 *=======================================================================================
 */
 
#include "MEC/DNMMTH.h"
#include "MTH.h"
 
/*
 **************************************************************************************************
 **************************************************************************************************
	TOOLS
 **************************************************************************************************
 **************************************************************************************************
 */
/*
 *=================================================================================================
 Projection of a vector on a plane                       
 Beware : ||_p_stPlaneNorm|| = ||_p_stProjDirection)|| = 1 
 *=================================================================================================
 */
void MTH_fn_vVectorPlaneProjection(
	MTH3D_tdstVector *_p_stResult,
	MTH3D_tdstVector *_p_stSource,
	MTH3D_tdstVector *_p_stPlaneNorm,
	MTH3D_tdstVector *_p_stProjDirection
)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
  MTH3D_tdstVector	a_stVector[1];
  MTH3D_tdstVector	*p_stTmp = a_stVector;
 
  MTH_tdxReal		xAlpha;
  MTH_tdxReal		xDotProduct;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
  if(_p_stProjDirection != _p_stPlaneNorm)
    xDotProduct = MTH3D_M_xDotProductVector(_p_stProjDirection,_p_stPlaneNorm);
  else
    xDotProduct = MTH_C_ONE;
 
  if(!MTH_M_bEqualZero(xDotProduct)) 
  {
    xAlpha = MTH_M_xNeg(MTH3D_M_xDotProductVector(_p_stSource,_p_stPlaneNorm));
    xAlpha = MTH_M_xDiv(xAlpha,xDotProduct);
    
	MTH3D_M_vCopyVector(p_stTmp,_p_stProjDirection);
    MTH3D_M_vMulScalarVector(p_stTmp, xAlpha, p_stTmp);
 
    if(_p_stResult != _p_stSource)
      MTH3D_M_vCopyVector(_p_stResult,_p_stSource);
    MTH3D_M_vAddVector(_p_stResult,_p_stResult,p_stTmp);
  }
}
 
/*
 *=================================================================================================
 Compute a rotation thanks a source vector and an image vector 
 ||_p_stSrcUnitVector|| = ||_p_stImaUnitVector|| = 1           
 *=================================================================================================
 */
MTH_tdstRotation *MTH_p_stRotationComputeRotationWith2Vectors(MTH_tdstRotation *_p_stResult,
                                                              MTH3D_tdstVector *_p_stSrcUnitVector,
                                                              MTH3D_tdstVector *_p_stImaUnitVector)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
  MTH3D_tdstVector	*p_stAxis;
  MTH_tdxReal		xCos;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
  
#ifdef DNM_DEBUG
  if(_p_stResult == NULL)
    return NULL;
  if(_p_stSrcUnitVector == NULL)
    return NULL;
  if(_p_stImaUnitVector == NULL)
    return NULL;
#endif
  
  p_stAxis = MTH_M_p_stRotationGetAxis(_p_stResult);
  
  MTH3D_M_vCrossProductVectorWithoutBuffer(p_stAxis,_p_stSrcUnitVector,_p_stImaUnitVector);
  if 
	  (
	  MTH_M_bIsNullWithEpsilon(MTH3D_M_xGetXofVector(p_stAxis), MTH_M_xDoubleToReal(1.0e-5))
	  && MTH_M_bIsNullWithEpsilon(MTH3D_M_xGetYofVector(p_stAxis), MTH_M_xDoubleToReal(1.0e-5))
	  && MTH_M_bIsNullWithEpsilon(MTH3D_M_xGetZofVector(p_stAxis), MTH_M_xDoubleToReal(1.0e-5))
	  )
  {
/*CB
	  if(MTH_M_xSign(MTH3D_M_xGetXofVector(_p_stSrcUnitVector)) != MTH_M_xSign(MTH3D_M_xGetXofVector(_p_stImaUnitVector)))
	  {
		  MTH_M_xRotationSetAngle(_p_stResult,MTH_C_Pi);
	  }
	  else if(MTH_M_xSign(MTH3D_M_xGetYofVector(_p_stSrcUnitVector)) != MTH_M_xSign(MTH3D_M_xGetYofVector(_p_stImaUnitVector)))
	  {
		  MTH_M_xRotationSetAngle(_p_stResult,MTH_C_Pi);
	  }
	  else if(MTH_M_xSign(MTH3D_M_xGetZofVector(_p_stSrcUnitVector)) != MTH_M_xSign(MTH3D_M_xGetZofVector(_p_stImaUnitVector)))
	  {
		  MTH_M_xRotationSetAngle(_p_stResult,MTH_C_Pi);
	  }
	  else
	  {
		  MTH_M_RotationSetNull(_p_stResult);
	  }
	  */
	  if
		(
			MTH_M_bEqualWithEpsilon(MTH3D_M_xGetXofVector(_p_stSrcUnitVector), MTH3D_M_xGetXofVector(_p_stImaUnitVector), MTH_M_xDoubleToReal(1.0e-5))
		&&	MTH_M_bEqualWithEpsilon(MTH3D_M_xGetYofVector(_p_stSrcUnitVector), MTH3D_M_xGetYofVector(_p_stImaUnitVector), MTH_M_xDoubleToReal(1.0e-5))
		&&	MTH_M_bEqualWithEpsilon(MTH3D_M_xGetZofVector(_p_stSrcUnitVector), MTH3D_M_xGetZofVector(_p_stImaUnitVector), MTH_M_xDoubleToReal(1.0e-5))
		)
	  {
		  MTH_M_RotationSetNull(_p_stResult);
	  }
	  else
	  {
		  MTH_M_xRotationSetAngle(_p_stResult,MTH_C_Pi);
	  }
  }
  else
  {
	  MTH3D_M_vNormalizeVector(p_stAxis, p_stAxis);
	  
	  xCos = MTH3D_M_xDotProductVector(_p_stSrcUnitVector,_p_stImaUnitVector);
	  if( MTH_M_bLessEqual(xCos, MTH_C_MinusONE))
	  {
		  MTH_M_xRotationSetAngle(_p_stResult,MTH_C_Pi);
	  }
	  else if ( MTH_M_bGreaterEqual(xCos, MTH_C_ONE)) 
	  {
		  MTH_M_RotationSetNull(_p_stResult);
	  }
	  else
	  {
		  MTH_M_xRotationSetAngle(_p_stResult,MTH_M_xACos(xCos));
	  }
  }
  
  return _p_stResult;
}
 
/*
 *=================================================================================================
 Rotation(Vector/Point) 
 *=================================================================================================
 */
MTH3D_tdstVector *MTH_p_stRotationMulVector
(
	MTH_tdstRotation	*_p_stRotation,
    MTH3D_tdstVector	*_p_stSource,
    MTH3D_tdstVector	*_p_stResult
)
{
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
    MTH3D_tdstVector		stTmp0,stTmp1;
    MTH3D_tdstVector		*p_stAxis;
    MTH_tdxReal				xCos,xSin,xValue, xAngle;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
#ifdef DNM_DEBUG
    if(_p_stRotation == NULL)
        return NULL;
    if(_p_stSource == NULL)
        return NULL;
    if(_p_stResult == NULL)
        return NULL;
#endif
 
    /* R(r) = (cosq)r + (1 - cosq)n(n·r) + (sinq)n^r */
	xAngle = MTH_M_xRotationGetAngle(_p_stRotation);
    xCos = MTH_M_xCos(xAngle);
    xSin = MTH_M_xSin(xAngle);
    p_stAxis = MTH_M_p_stRotationGetAxis(_p_stRotation);
 
    MTH3D_M_vCrossProductVector(&stTmp0,p_stAxis,_p_stSource);
    MTH3D_M_vMulScalarVector(&stTmp0, xSin, &stTmp0);
 
    xValue = MTH_M_xMul(MTH3D_M_xDotProductVector(p_stAxis,_p_stSource),MTH_M_xSub(MTH_C_ONE,xCos));
    MTH3D_M_vCopyVector(&stTmp1,p_stAxis);
    MTH3D_M_vMulScalarVector(&stTmp1, xValue, &stTmp1);
 
    if(_p_stSource != _p_stResult)
        MTH3D_M_vCopyVector(_p_stResult,_p_stSource);
    MTH3D_M_vMulScalarVector(_p_stResult, xCos, _p_stResult);
 
    MTH3D_M_vAddVector(_p_stResult, _p_stResult, &stTmp1);
    MTH3D_M_vAddVector(_p_stResult, _p_stResult, &stTmp0);
 
    return _p_stResult;
}
 
 
 
 
/*
 *=================================================================================================
 Matrix -> Rotation 
 *=================================================================================================
 */
 
#ifndef _FIRE_DEADCODE_U64_ /* Added by RUC */
MTH_tdstRotation *MTH_p_stRotationFromMatrix 
(
	MTH_tdstRotation *_p_stResult,  
	MTH3D_tdstMatrix *_p_stRotationMatrix
)
{
 
	static MTH_tdxReal s_xLimit = MTH_M_xDoubleToReal(2.999999);
	MTH3D_tdstVector *p_stAxis;
	MTH_tdxReal xTrace, xCos,xSin;
	MTH3D_tdstVector stI, stJ, stK;
  
#ifdef DNM_DEBUG
  if(_p_stResult == NULL)
    return NULL;
  if(_p_stRotationMatrix == NULL)
    return NULL;
#endif
  
	xTrace = MTH3D_M_xTraceMatrix( _p_stRotationMatrix);
 
	if (MTH_M_bLess(xTrace,s_xLimit))
	{
		MTH3D_M_vGetVectorsInMatrix ( &stI, &stJ, &stK, _p_stRotationMatrix );
 
		p_stAxis = MTH_M_p_stRotationGetAxis(_p_stResult);
    
		MTH3D_M_vSetXofVector ( p_stAxis, MTH_M_xSub ( MTH3D_M_xGetZofVector(&stJ), MTH3D_M_xGetYofVector(&stK) ) );
		MTH3D_M_vSetYofVector ( p_stAxis, MTH_M_xSub ( MTH3D_M_xGetXofVector(&stK), MTH3D_M_xGetZofVector(&stI) ) );
		MTH3D_M_vSetZofVector ( p_stAxis, MTH_M_xSub ( MTH3D_M_xGetYofVector(&stI), MTH3D_M_xGetXofVector(&stJ) ) );
 
		if (MTH3D_M_bIsNullVector(p_stAxis))
		{
			if (MTH_M_bEqual (MTH3D_M_xGetXofVector(&stI),MTH_C_ONE))
			{
				MTH3D_M_vSetBaseIVector (p_stAxis);
				MTH_M_xRotationSetAngle(_p_stResult,MTH_C_Pi);
			}
			else if (MTH_M_bEqual (MTH3D_M_xGetYofVector(&stJ),MTH_C_ONE))
			{
				MTH3D_M_vSetBaseJVector (p_stAxis);
				MTH_M_xRotationSetAngle(_p_stResult,MTH_C_Pi);
			}
			else if (MTH_M_bEqual (MTH3D_M_xGetZofVector(&stK),MTH_C_ONE))
			{
				MTH3D_M_vSetBaseKVector (p_stAxis);
				MTH_M_xRotationSetAngle(_p_stResult,MTH_C_Pi);
			}
		}
		else
		{
			MTH3D_M_vNormalizeVector (p_stAxis,p_stAxis);
			xCos = MTH_M_xMul ( MTH_M_xSub(xTrace,MTH_C_ONE) , MTH_C_Inv2 );
			xSin = MTH_M_xAdd3(MTH3D_M_xGetXofVector(&stJ),MTH3D_M_xGetXofVector(&stK),MTH3D_M_xGetYofVector(&stK));
 
			if (MTH_M_bLessEqual( xCos, MTH_C_MinusONE)) MTH_M_xRotationSetAngle ( _p_stResult, MTH_C_Pi);
			else if (MTH_M_bGreaterEqual( xCos, MTH_C_ONE)) MTH_M_xRotationSetAngle ( _p_stResult, MTH_C_ZERO );
			else 
				MTH_M_xRotationSetAngle (_p_stResult,MTH_M_bGreaterEqualZero(xSin)?MTH_M_xNeg(MTH_M_xACos(xCos)):MTH_M_xACos(xCos));
		}
	}
	else MTH_M_RotationSetNull(_p_stResult);
 
	return _p_stResult;
}
#endif /* _FIRE_DEADCODE_U64_ */ /* Added by RUC */