275 lines
9.3 KiB
C
275 lines
9.3 KiB
C
/*
|
||
*=======================================================================================
|
||
* 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 */
|
||
|
||
|