util_transform.cpp

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

#include "stdafx.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 )
{
  CTransf oT;
  oT.SetRotation( RotationMatrix(ang1,ang2,ang3,nType) );
  return oT;
}

//rotation transf from angle-axis prepresentation
TS_DLL_KERNEL CTransf RotationTransf( CVector axis, double ang )
{
  CTransf oT;
  oT.SetRotation( RotationMatrix(axis,ang) );
  return oT;
}

//translation transf
TS_DLL_KERNEL CTransf TranslationTransf( CVector offset )
{
  CTransf oT;
  oT.SetTranslation( offset );
  return oT;
}

////////////////////////////////////////////////////////////////////////////////////
//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 )
{
  ASSERT( !IsZero( nx ) );
  ASSERT( !IsZero( ny ) );
  ASSERT( !IsParallel(nx,ny) );

  CVector nz;
  nz = nx * ny;
  ny = nz * nx;
  return CTransf ( CMatrix( Normalize(nx), Normalize(ny), Normalize(nz) ), 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 )
{
  ASSERT( !IsZero( ny ) );
  ASSERT( !IsZero( nz ) );
  ASSERT( !IsParallel(ny,nz) );

  CVector nx;
  nx = ny * nz;
  nz = nx * ny;
  return CTransf ( CMatrix( Normalize(nx), Normalize(ny), Normalize(nz) ), 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 )
{
  ASSERT( !IsZero( nz ) );
  ASSERT( !IsZero( nx ) );
  ASSERT( !IsParallel(nz,nx) );

  CVector ny;
  ny = nz * nx;
  nx = ny * nz;
  return CTransf ( CMatrix( Normalize(nx), Normalize(ny), Normalize(nz) ), 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 )
{
  if( IsZero( oRotationAxis ) )
  {
    ASSERT(FALSE);
    return;
  }

  //move to origin first
  CVector org = oRefFrame.GetTranslation();
  CTransf T1 = TranslationTransf(-org);            //move to origin
  CTransf T2 = RotationTransf(oRotationAxis,ang);  //rotate about axis
  CTransf T3 = TranslationTransf(org);             //move back to original location
  oRefFrame = T3*T2*T1*oRefFrame;
}

//rotate ref frame (ang) about its x, y, or z axis
TS_DLL_KERNEL void SelfRotate( CTransf& oRefFrame, double ang, int nAxis )
{
  //nAxis = XAXIS(0), X axis
  //        YAXIS(1), Y axis
  //        ZAXIS(2), Z axis
  ASSERT( XAXIS<= nAxis && nAxis <=ZAXIS );
  CVector v = oRefFrame.GetAxis(nAxis);
  SelfRotate( oRefFrame, ang, v );
}

//translate ref frame an offset which is referred to this ref frame, not WCS
TS_DLL_KERNEL void SelfTranslate( CTransf& oRefFrame, CVector offset )
{
  offset = VectorLocalToWorld( oRefFrame, offset );
  oRefFrame = TranslationTransf( offset )*oRefFrame;
}

//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 )
{
  //align one of X,Y,Z axis with v
  //nAxis = XAXIS(0), X axis
  //        YAXIS(1), Y axis
  //        ZAXIS(2), Z axis
  //can be used to level or align operation in ref. frame setting
  //note that there is no constraint on the other two axes, it is the user's
  //responsibility to prepare vector v if he want to do align operation,
  //eg projecting the vector on the working plane before calling this function

  //option axis is used for ang = 180 case
  ASSERT( nAxis != nOptionRotAxis );
  ASSERT( XAXIS <= nAxis && nAxis <= ZAXIS );
  ASSERT( XAXIS <= nOptionRotAxis && nOptionRotAxis <= ZAXIS );
  ASSERT( !IsZero( v ) );
  if( IsZero( v ) ) return;

  //use level operation convection (that is, nAxis is the 3rd axis
  //prepare 3rd axis and 1st axis (option axis)
  CVector v3 = oRefFrame.GetAxis(nAxis);
  CVector v1 = oRefFrame.GetAxis(nOptionRotAxis);

  //find the rotation angle
  CVector v_rot;
  double ang = BetweenAngle( v3, v );
  if( M_EPS10 < ang && ang < M_PI-M_EPS10 )
    v_rot = v3 * v;
  else
    v_rot = v1;

  SelfRotate( oRefFrame, ang, v_rot );
}

//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 )
{
  ASSERT( nAxis != nFixedAxis );
  ASSERT( XAXIS <= nAxis && nAxis <= ZAXIS );
  ASSERT( XAXIS <= nFixedAxis && nFixedAxis <= ZAXIS );
  if( IsZero(v) ) return;

  //project v vector into primary plane
  v = VectorWorldToLocal(oRefFrame,v);
  switch(nFixedAxis)
  {
  case XAXIS: v.SetX(0.0); break;
  case YAXIS: v.SetY(0.0); break;
  case ZAXIS: v.SetZ(0.0); break;
  default: ASSERT(FALSE); break;
  }
  v = VectorLocalToWorld(oRefFrame,v);

  //calc btw angle
  CVector v3 =oRefFrame.GetAxis(nAxis);
  double ang = BetweenAngle(v3,v);
  if( IsZero(ang) ) return;      //no rotation
  if( IsEqual(ang,M_PI) )        //180 deg rotation
  {
    SelfRotate(oRefFrame,M_PI,nFixedAxis);
    return;
  }

  //find rotation direction
  if( IsSameSide(oRefFrame.GetAxis(nFixedAxis),v3*v) )
  {
    SelfRotate(oRefFrame,ang,nFixedAxis);
  }
  else
  {
    SelfRotate(oRefFrame,-ang,nFixedAxis);
  }
}

//////////////////////////////////////////////////////////////////////////////////
TS_DLL_KERNEL int GetCloseAxis( CVector oV )
{
  //return enum { XAXIS=0, YAXIS=1, ZAXIS=2, N_XAXIS=3, N_YAXIS=4, N_ZAXIS=5  }
  
  double x = fabs(oV.X());
  double y = fabs(oV.Y());
  double z = fabs(oV.Z());

  if( x > y )
  {
    if( x > z )
      return oV.X()>0.0 ? XAXIS : N_XAXIS; // x axis
    else
      return oV.Z()>0.0 ? ZAXIS : N_ZAXIS; // z axis
  }
  else
  {
    if( y > z )
      return oV.Y()>0.0 ? YAXIS : N_YAXIS;  // y axis
    else
      return oV.Z()>0.0 ? ZAXIS : N_ZAXIS;  // z axis
  }
}