www.pudn.com > 人体步态跟踪识别bate版.rar > IK.cpp

// IK.cpp: implementation of the CIK class. 
// 
////////////////////////////////////////////////////////////////////// 
 
#include "stdafx.h" 
#include "humantrack.h" 
#include "IK.h" 
#include "math.h" 
#include "dpoint2d.h" 
#include "Position.h" 
#include "dibapi.h" 
#ifdef _DEBUG 
#undef THIS_FILE 
static char THIS_FILE[]=__FILE__; 
#define new DEBUG_NEW 
#endif 
 
////////////////////////////////////////////////////////////////////// 
// Construction/Destruction 
////////////////////////////////////////////////////////////////////// 
 
CIK::CIK() 
{ 
} 
 
CIK::~CIK() 
{ 
 
} 
/** 
*   Calculate the included of link1 and link2 
* 
*   len1     : the length of link1 
*   len2     : the length of link2  
*   distance : the distance of the endpoints of link1 and link2 
*/ 
double CIK::IK_1D(double len1, double len2, double distance) 
 { 
 
 return acos((len1*len1 + len2*len2- distance*distance) / (2*len1*len2)); 
} 
 
/** 
*   if unsolvable, then return null 
* 
*      x-------x-------x 
*   point1   point   point2 
* 
*   link1    : the link from point1 to point 
*   link2    : the link from point2 to point 
*   len1     : the length of link1 
*   len2     : the length of link2 
*   distance : the distance between point1 and point2 
*   nVector  : the vector from point1 to the projection of point 
*              on the vector from point1 to point2 
*   hVector  : the vector from the projection of point on the vector 
*              from point1 to point2 to point 
*   flag     : true -> the vector from point1 to point2 X hVector >= 0 
*/ 
CDPoint2D CIK::IK_2D(CDPoint2D point1, double len1,CDPoint2D point2, double len2,bool flag)  
{ 
 
      double distance = point1.distance(point2); 
      len1 = abs(len1); 
      len2 = abs(len2); 
 
      CDPoint2D point ; 
 
      if(abs(len1 - len2)<= distance && (distance <= (len1 + len2)) && distance != 0)  
	  { 
 
         /** 
          *  theta : the included of link1 and link2 
          */ 
         double theta  = IK_1D(len1, len2, distance); 
 
         /** 
          *   lnVector : the length of nVector 
          *   a*b*sin(theta) = c*h 
          */ 
         double lnVector = len1 * len2 * sin(theta) / distance; 
 
         /** 
          *   lhVector : the length of hVector 
          */ 
         double lhVector = sqrt(len1*len1 - lnVector*lnVector); 
 
         /** 
          *   calculate the nVector 
          */ 
         double nVectorX = (point1.y - point2.y) * lnVector / distance; 
         double nVectorY = (point2.x - point1.x) * lnVector / distance; 
 
         /** 
          *   calculate the nVector 
          */ 
         double hVectorX = (point2.x - point1.x) * lhVector / distance; 
         double hVectorY = (point2.y - point1.y) * lhVector / distance; 
 
         if(!flag)  
		 { 
 
            nVectorX *= -1; 
            nVectorY *= -1; 
         } 
 
         if(cos((len1*len1 + distance*distance- len2*len2)/(2*len1*distance)) < 0) 
		 { 
 
            hVectorX *= -1; 
            hVectorY *= -1; 
         } 
 
       CDPoint2D point(point1.x + hVectorX + nVectorX,point1.y + hVectorY + nVectorY); 
      } 
 
      return point; 
   } 
 
CDPoint2D CIK::IK_2D(CDPoint2D point1, double len1,CDPoint2D point2, double len2)  
{ 
 
      return IK_2D(point1, len1, point2, len2, true); 
   } 
 
CPosition CIK::IK_3D(CPosition point1, double len1,CPosition point2, double len2,double dx, double dy, double dz) 
 { 
 
      double distance = point1.distance(point2); 
      len1 = abs(len1); 
      len2 = abs(len2); 
 
      CPosition point; 
 
      if(abs(len1 - len2) <= distance && (distance <= (len1 + len2)) && distance != 0) 
	  { 
 
    /** 
   *  theta : the included of link1 and link2 
   */ 
         double theta  = IK_1D(len1, len2, distance); 
 
         /** 
          *   lnVector : the length of nVector 
          *   a*b*sin(theta) = c*h 
          */ 
         double lnVector = len1 * len2 * sin(theta) / distance; 
 
         /** 
          *   lhVector : the length of hVector 
          */ 
         double lhVector = sqrt(len1*len1 - lnVector*lnVector); 
 
         double vx = point2.x - point1.x; // x of the vector of point1 to point2 
         double vy = point2.y - point1.y; // y of the vector of point1 to point2 
         double vz = point2.z - point1.z; // z of the vector of point1 to point2 
         /** 
          *   calculate the nVector => nVector = ((p1->p2) x dVector) x (p1->p2) 
          */ 
         double nVectorX = vz*vz*dx - vz*vx*dz - vy*vx*dy + vy*vy*dx; 
         double nVectorY = vx*vx*dy - vx*vy*dx - vz*vy*dz + vz*vz*dy; 
         double nVectorZ = vy*vy*dz - vy*vz*dy - vx*vz*dx + vx*vx*dz; 
 
         double l = sqrt(nVectorX*nVectorX + nVectorY*nVectorY + nVectorZ*nVectorZ); 
 
         nVectorX = nVectorX * lnVector / l; 
         nVectorY = nVectorY * lnVector / l; 
         nVectorZ = nVectorZ * lnVector / l; 
 
         /** 
          *   calculate the nVector 
          */ 
         double hVectorX = vx * lhVector / distance; 
         double hVectorY = vy * lhVector / distance; 
         double hVectorZ = vz * lhVector / distance; 
 
         if((len1*len1 + distance*distance - len2*len2)/(2*len1*distance) < 0)  
		 { 
 
            hVectorX *= -1; 
            hVectorY *= -1; 
            hVectorZ *= -1; 
         } 
 
         theta = ::DoubleModule((atan2(vy, vx)+2*PI),(2*PI)); 
 
        CPosition point(point1.x + hVectorX + nVectorX,point1.y + hVectorY + nVectorY,point1.z + hVectorZ + nVectorZ, theta); 
      } 
 
      return point; 
}