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