util_transform.h

C arm 3D traslate 模擬C手臂在三度空間移動

#ifndef __TSKERNEL_TRANSFORM_UTILITY_H__
#define __TSKERNEL_TRANSFORM_UTILITY_H__

//[object transform utility]
//rotation transf from Euler angles or Fixed angles
TS_DLL_KERNEL CTransf RotationTransf( double ang1, double ang2, double ang3, int nType );

//rotation transf from angle-axis prepresentation
TS_DLL_KERNEL CTransf RotationTransf( CVector axis, double ang );

//translation transf
TS_DLL_KERNEL CTransf TranslationTransf( CVector offset );

//[reference frame utility]
//cooridnate transformation
inline TS_DLL_KERNEL CVector VectorWorldToLocal( CTransf& oRefFrame, CVector& v );
inline TS_DLL_KERNEL CVector VectorLocalToWorld( CTransf& oRefFrame, CVector& v_local );
inline TS_DLL_KERNEL CVector CoordWorldToLocal ( CTransf& oRefFrame, CVector& p );
inline TS_DLL_KERNEL CVector CoordLocalToWorld ( CTransf& oRefFrame, CVector& p_local );
inline TS_DLL_KERNEL CTransf RefFrameWorldToLocal ( CTransf& oRefFrame, CTransf& tr );
inline TS_DLL_KERNEL CTransf RefFrameLocalToWorld ( CTransf& oRefFrame, CTransf& tr_local );

//construct the reference frame from nx, ny, nz, org
//if ny not perpendicular to nx, it will be forced to perpendicular to nx
TS_DLL_KERNEL CTransf ConstructRefFrameXY( CVector nx, CVector ny, CVector org );

//if nz not perpendicular to ny, it will be forced to perpendicular to ny
TS_DLL_KERNEL CTransf ConstructRefFrameYZ( CVector ny, CVector nz, CVector org );

//if nx not perpendicular to nz, it will be forced to perpendicular to nz
TS_DLL_KERNEL CTransf ConstructRefFrameZX( CVector nz, CVector nx, CVector org );

//rotate ref frame (ang) about the rotation axis passing its own origin
//rotation axis is referred to world ref frame
TS_DLL_KERNEL void SelfRotate( CTransf& oRefFrame, double ang, CVector oRotationAxis );

//rotate ref frame (ang) about its x, y, or z axis (XAXIS=0,YAXIS=1,ZAXIS=2)
TS_DLL_KERNEL void SelfRotate( CTransf& oRefFrame, double ang, int nAxis );

//translate ref frame an offset which is referred to this ref frame, not world ref frame
TS_DLL_KERNEL void SelfTranslate( CTransf& oRefFrame, CVector offset );

//align x, y, or z axis of ref frame with a vector.  option axis is given when
//the aligned axis is counter-parallel to the vector (flip)
TS_DLL_KERNEL void AlignAxisFloating( CTransf& oRefFrame, int nAxis, CVector v, int nOptionRotAxis );

//aling x, y, or z axis of ref frame with a vector, w.r.t. fixed axis
//that is, fixed axis is kept intact when aligning
TS_DLL_KERNEL void AlignAxisFixed( CTransf& oRefFrame, int nAxis, CVector v, int nFixedAxis );


//return the most parallel axis
//return enum { XAXIS=0, YAXIS=1, ZAXIS=2, N_XAXIS=3, N_YAXIS=4, N_ZAXIS=5  }
TS_DLL_KERNEL int GetCloseAxis( CVector oV );

//[reference frame utility]
//cooridnate transformation
inline TS_DLL_KERNEL CVector VectorWorldToLocal( CTransf& oRefFrame, CVector& v )
{
  return oRefFrame.GetRotation().Transpose()*v;
}

inline TS_DLL_KERNEL CVector VectorLocalToWorld( CTransf& oRefFrame, CVector& v_local )
{
  return oRefFrame.GetRotation()*v_local;
}

inline TS_DLL_KERNEL CVector CoordWorldToLocal ( CTransf& oRefFrame, CVector& p )
{
  return oRefFrame.Inverse()*p;
}

inline TS_DLL_KERNEL CVector CoordLocalToWorld ( CTransf& oRefFrame, CVector& p_local )
{
  return oRefFrame*p_local;
}

inline TS_DLL_KERNEL CTransf RefFrameWorldToLocal ( CTransf& oRefFrame, CTransf& tr )
{
  return oRefFrame.Inverse()*tr;
}

inline TS_DLL_KERNEL CTransf RefFrameLocalToWorld ( CTransf& oRefFrame, CTransf& tr_local )
{
  return oRefFrame*tr_local;
}
#endif