开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > OpenCV > 查看源码
skelecton.cpp
资源名称：HumanMotionTrack.rar　[点击查看]
上传用户：jsylhbnbhn
上传日期：2013-11-03
资源大小：119k
文件大小：17k
源码类别：

OpenCV

开发平台：

Visual C++

skelecton.cpp：源码内容
  1. // skelecton.cpp: implementation of the skelecton class.
  2. //
  3. //////////////////////////////////////////////////////////////////////
  6. #include "skelecton.h"
  7. #include "kalman.h"
  8. #include <string.h>
  9. #include <math.h>
  10. #include "vector3f.h"
  11. #include <assert.h>
  13. //////////////////////////////////////////////////////////////////////
  14. // Construction/Destruction
  15. //////////////////////////////////////////////////////////////////////
  16. int coordinator_X,coordinator_Y=0; //图像中心坐标
  17. const double MaxMinZero=0.0000001;  //小于这个值认为是0
  18. const double f=200;  //pin-hole的有效焦距
  19. const double StardardLength=500;  //对应现实世界的人体的高度，实际上是对应的象素
  20. const double StardardLengthNoiseRatio = 0.3 ; //测量误差占整个测量的比率
  21. double arc_y= 0;//y轴旋转15度
  22. double arc_z= 0;//z轴旋转15度
  25. skelecton::skelecton()
  26. {
  27. bone *pbone = new bone (1,"neck",0);
  28. bonevec.push_back (*pbone);
  30. pbone = new bone (2,"head",1);
  31. pbone->start_arc_x = 30.0;   //角度定义为右手坐标系
  32. pbone->end_arc_x = -30.0;
  33. pbone->start_arc_z = 10.0;
  34. pbone->end_arc_z = -80.0;
  35. bonevec.push_back (*pbone);
  37. pbone = new bone (3,"rShoulder",1);
  38. bonevec.push_back (*pbone);
  40. pbone = new bone (4,"lShoulder",1);
  41. bonevec.push_back (*pbone);
  43. pbone = new bone (5,"rElbow",3);
  44. pbone->start_arc_y = -90;
  45. pbone->end_arc_y = 90;
  46. pbone->start_arc_z = 90;
  47. pbone->end_arc_z = -60.0;
  48. pbone->CID = 7;
  49. bonevec.push_back (*pbone);
  51. pbone = new bone  (6,"lElbow",4);
  52. pbone->start_arc_y = -90;
  53. pbone->end_arc_y = 90;
  54. pbone->start_arc_z = -90;
  55. pbone->end_arc_z = 60.0;
  56. pbone->LengthRatio = 0.15;
  57. pbone->CID = 8;
  58. bonevec.push_back (*pbone);
  60. pbone = new bone (7,"rWrist",5);
  61. bonevec.push_back (*pbone);
  63. pbone = new bone (8,"lWrist",6);
  64. bonevec.push_back (*pbone);
  66. bonevec.push_back (bone (9,"Waist",1));
  68. pbone = new bone (10,"rKnee",9);
  69. pbone->CID = 12;
  70. bonevec.push_back (*pbone);
  72. pbone = new bone(11,"lKnee",9);
  73. pbone->CID = 13;
  74. bonevec.push_back (*pbone);
  76. bonevec.push_back (bone (12,"rAnkle",10));
  77. bonevec.push_back (bone (13,"lAnkle",11));
  79. delete pbone; 
  80. pbone=NULL;
  82. pre_ske=NULL;
  83. //pgl = m_OpenGL;
  84. //pgl->Init();
  86. }
  88. skelecton::~skelecton()
  89. {
  90.   //这里释放出错，需要再次检查是否有循环引用
  91. /*
  92. if(pre_ske!=NULL) 
  93. {
  94. delete pre_ske;
  95. pre_ske = NULL;
  96. }
  98. if(pgl!=NULL) 
  99. {
  100. delete pgl;
  101. pgl = NULL;
  102. }
  103. */
  104. bonevec.empty(); 
  106. }
  108. int skelecton::draw2D(CvArr* motion_track_img)
  109. {
  110. bone_vector::iterator theIterator;
  111. bone *pbone = 0;
  112. bone *ppbone = 0;
  114.     for (theIterator = bonevec.begin(); theIterator != bonevec.end();
  115.          theIterator++)
  116.     {
  117.   pbone = theIterator;    
  118.   cvCircle( motion_track_img, cvPoint(pbone->x,pbone->y),10, CV_RGB(0,255,0),3, CV_AA, 0 );
  119.   if ( 0 != pbone->PID )  
  120.   {
  121.   ppbone = get_bone_by_ID(pbone->PID); 
  122.   cvLine( motion_track_img, cvPoint(pbone->x,pbone->y),cvPoint(ppbone->x,ppbone->y),CV_RGB(0,255,0),2,CV_AA, 0 );
  123.   }
  125. } 
  126. return 0;
  127. }
  129. bone* skelecton::get_bone_by_name(char *name)
  130. {
  131. bone_vector::iterator theIterator;
  132. bone_vector::iterator theIterator2;
  133. theIterator2  = 0;
  136.     for (theIterator = bonevec.begin(); theIterator != bonevec.end();
  137.          theIterator++)
  138.     {
  139.  if (0 == strncmp(theIterator->name, name , 4 ))
  140.  {
  141.  theIterator2=theIterator;
  142.  break;
  143.  }                                         
  144. } 
  145. return (theIterator2);
  146. }
  148. bone* skelecton::get_bone_by_ID(int ID)
  149. {
  151. return (&bonevec[ID-1]);
  152. }
  154. int skelecton::reset()
  155. {
  156. bone_vector::iterator theIterator;
  157. bone *pbone = 0;
  160.     for (theIterator = bonevec.begin(); theIterator != bonevec.end();
  161.          theIterator++)
  162.     {
  163.   pbone = theIterator;    
  164.   pbone->x =0 ;
  165.   pbone->y =0 ;
  166.   pbone->is_marked = 0;
  167.  
  168. } 
  169. return 0;
  170. }
  172. int skelecton::draw3D(CvArr *motion_track_img)
  173. {
  174. //2d坐标转化为3d坐标
  175. bone_vector::iterator theIterator;
  176. bone *pbone = 0;
  177. bone *ppbone = 0;
  179. ppbone = get_bone_by_name("head"); 
  180. ppbone->r_z =0;  //初始化head的坐标为0
  181. ppbone->r_x=ppbone->x - coordinator_X;
  182. ppbone->r_y=ppbone->y - coordinator_Y;
  185. CvSize ImageSize; 
  186. ImageSize = cvGetSize(motion_track_img);
  188. coordinator_X = (int)ImageSize.width/2;
  189. coordinator_Y = (int)ImageSize.height/2;
  191. CalcBonePosition();
  193. //绘制3d坐标系
  194. if(ske_pause) arc_y+=5;
  195. const double pi = 3.1415926535;
  196. double cosx = cos(arc_y/90*pi);
  197. double sinx = sin(arc_y/90*pi);
  198. double cosx_z = cos(arc_z/90*pi);
  199. double sinx_z = sin(arc_z/90*pi);
  201. const float ZX[] = { 1, 0, 0, 0,
  202. 0,cosx,sinx,0,
  203. 0,-sinx,cosx,0,
  204. 0,0,0,1
  205. };
  207. const float ZY[] = { cosx,0,sinx,0,
  208.  0,1,0,0,
  209. -sinx,0,cosx,0,
  210. 0,0,0,1
  211. };
  213. const float ZZ[] = { cosx_z,sinx_z, 0, 0,
  214. -sinx_z,cosx_z,0,0,
  215. 0,0,1,0,
  216. 0,0,0,1
  217. };
  219. const float Zoom[] = { 
  220. 1,0,0,0,
  221. 0,1,0,0,
  222. 0,0,1,0,
  223. coordinator_X,coordinator_Y,coordinator_X,1
  224. };  //缩放比率,并且平移100
  225. CvMat* RotateMatrix = cvCreateMat( 4, 4, CV_32FC1 ); //旋转矩阵
  226. memcpy( RotateMatrix->data.fl, ZY, sizeof(ZY));      
  227. CvMat* tempMatrix = cvCreateMat( 4, 4, CV_32FC1 );
  228. memcpy( tempMatrix->data.fl, ZZ, sizeof(ZZ));
  229. cvMatMul( RotateMatrix, tempMatrix, RotateMatrix);
  230. memcpy( tempMatrix->data.fl, Zoom, sizeof(Zoom));
  231. cvMatMul( RotateMatrix, tempMatrix, RotateMatrix);  
  233. CvMat* PointVector = cvCreateMat( 1, 4, CV_32FC1 ); //点矢量矩阵
  234. float pt[] = { 0,0, 0, 1};
  235. int ox,oy=0; //原点
  236. memcpy( PointVector->data.fl, pt, sizeof(pt));
  237. cvMatMul(PointVector,RotateMatrix,PointVector);
  238. ox = PointVector->data.fl[0];
  239. oy = PointVector->data.fl[1];
  241. pt[0]=300 ;
  242. pt[1]=0 ;
  243. pt[2]=0;
  244. memcpy( PointVector->data.fl, pt, sizeof(pt));
  245. cvMatMul(PointVector,RotateMatrix,PointVector);
  246. cvLine( motion_track_img, cvPoint(ox,oy),cvPoint(PointVector->data.fl[0],PointVector->data.fl[1]),CV_RGB(255,255,255),2,CV_AA, 0 );
  247. //printf( "PointVector->data.fl[0,1]: %f,%f n",PointVector->data.fl[0],PointVector->data.fl[1]);
  249. pt[0]=0 ;
  250. pt[1]=300 ;
  251. pt[2]=0;
  252. memcpy( PointVector->data.fl, pt, sizeof(pt));
  253. cvMatMul(PointVector,RotateMatrix,PointVector);
  254. cvLine( motion_track_img, cvPoint(ox,oy),cvPoint(PointVector->data.fl[0],PointVector->data.fl[1]),CV_RGB(0,255,255),2,CV_AA, 0 );
  255. //printf( "PointVector->data.fl[0,1]: %f,%f n",PointVector->data.fl[0],PointVector->data.fl[1]);
  257. pt[0]=0 ;
  258. pt[1]=0 ;
  259. pt[2]=300;
  260. memcpy( PointVector->data.fl, pt, sizeof(pt));
  261. cvMatMul(PointVector,RotateMatrix,PointVector);
  262. cvLine( motion_track_img, cvPoint(ox,oy),cvPoint(PointVector->data.fl[0],PointVector->data.fl[1]),CV_RGB(255,255,0),2,CV_AA, 0 );
  263. //printf( "PointVector->data.fl[0,1]: %f,%f n",PointVector->data.fl[0],PointVector->data.fl[1]);
  267.     for (theIterator = bonevec.begin(); theIterator != bonevec.end();
  268.          theIterator++)
  269.     {
  270.   pbone = theIterator;
  271.   
  272. pt[0]=pbone->r_x ;
  273. pt[1]=pbone->r_y ;
  274. pt[2]=pbone->r_z ;
  275. memcpy( PointVector->data.fl, pt, sizeof(pt));
  276. cvMatMul(PointVector,RotateMatrix,PointVector);
  277. pbone->rotated_X = PointVector->data.fl[0];
  278. pbone->rotated_Y = PointVector->data.fl[1];
  280. if(pbone->is_marked==0)  continue; //未标记则不画
  281. /*
  282. printf( "%s,r_x,r_y,r_z: %d,%d,%d n",pbone->name,pbone->r_x,pbone->r_y,pbone->r_z);
  283. printf( "rotated_X,Y: %d,%d n",pbone->rotated_X,pbone->rotated_Y);
  284. printf( "---------------------------------------------------n");
  285. */
  286. cvCircle( motion_track_img, cvPoint(pbone->rotated_X,pbone->rotated_Y),5, CV_RGB(0,255,0),2, CV_AA, 0 );
  287.   if ( 0 != pbone->PID )  
  288.   {
  289.   ppbone = get_bone_by_ID(pbone->PID); 
  290.   cvLine( motion_track_img, cvPoint(pbone->rotated_X,pbone->rotated_Y),cvPoint(ppbone->rotated_X,ppbone->rotated_Y),CV_RGB(0,255,0),2,CV_AA, 0 );
  291.   
  292.   }
  294. } 
  296. pgl->display();
  297.     return 0;
  298. }
  300. //假设pin hole模型
  301. int skelecton::CalcBoneRealWorldPosition(char *pname, char *ppName,double LengthRatio)
  302. {
  304. bone *ppbone = 0;
  305. bone *pbone = 0;
  306. int cx0,cy0=0; //校正后的坐标
  307. int cx,cy=0; //校正后的坐标
  308. double A,B,C,det,sqdet,x1,x2=0;
  310. ppbone = get_bone_by_name(pname); 
  311. pbone = get_bone_by_name(ppName); 
  312. cx0= ppbone->r_x;
  313. cy0= ppbone->r_y;
  314. cx= pbone->x - coordinator_X ;
  315. cy= pbone->y - coordinator_Y ;
  317. //'求解二元一次方程
  319. A = pow(cx/f,2)  + pow(cy/f,2) + 1;
  320. B = -2*(cx/f)*ppbone ->r_x -2*(cy/f)*ppbone ->r_y - 2*ppbone->r_z;
  321. C = pow(ppbone->r_x,2) + pow(ppbone->r_y,2) + pow(ppbone->r_z,2) - pow(LengthRatio*StardardLength,2);
  322. //'B^2-4AC
  323. det = pow(B,2) - 4*A*C;
  324. //'det>0 两个解
  325. if (det>MaxMinZero)
  326. {
  327. //需要舍弃一个无意义的解
  328. sqdet = sqrt(det);
  329. x1=(-B+sqdet)/(2*A);
  330. x2=(-B-sqdet)/(2*A);
  331. }
  332. //'det==0 单解
  333. else if (det<MaxMinZero && det>0)
  334. {
  335. x1=x2=(-B)/(2*A);
  336. } 
  337. //'det<0 无解
  338. else if (det<0)
  339. {
  340. //需要再次探索
  341. x1=x2=0;
  342. }
  343. pbone->r_z =x1;
  344. pbone ->r_x = pbone->x * pbone->r_z / f;
  345. pbone ->r_y = pbone->y * pbone->r_z / f;
  347. /*
  348. printf( "A: %f n",A);
  349. printf( "B: %f n",B);
  350. printf( "C: %f n",C);
  352. */
  354. return 0;
  356. }
  358. //假设平行光模型
  359. int skelecton::CalcBoneRealWorldPosition_parallel(char *pname, char *ppName)
  360. {
  362. bone *ppbone = 0;
  363. bone *pbone = 0;
  364. int cx0,cy0=0; //校正后的坐标
  365. ppbone = get_bone_by_name(pname); 
  366. pbone = get_bone_by_name(ppName); 
  367. if(pbone!=NULL && pbone->is_marked)
  368. {
  369. cx0= ppbone->x - coordinator_X ;
  370. cy0= ppbone->y - coordinator_Y ;
  372. ppbone ->r_x = cx0;
  373. ppbone ->r_y = cy0;
  374. double A,B,C,det,sqdet,x1,x2=0;
  376. int cx,cy=0; //校正后的坐标
  378. cx= pbone->x - coordinator_X ;
  379. cy= pbone->y - coordinator_Y ;
  381. //'求解二元一次方程
  382. A = 1;
  383. B = - 2*ppbone->r_z;
  384. C = pow(cx-cx0,2) + pow(cy-cy0,2) + pow(ppbone->r_z,2) - pow(pbone->LengthRatio*StardardLength,2);
  385. //'B^2-4ACy
  386. det = pow(B,2) - 4*A*C;
  387. //'det>0 两个解
  388. if (det>MaxMinZero)
  389. {
  390. //需要舍弃一个无意义的解
  391. sqdet = sqrt(det);
  392. x1=(-B+sqdet)/(2*A);
  393. x2=(-B-sqdet)/(2*A);
  394. x1=get_porper_Z(x1,x2,pbone);
  395. }
  396. //'det==0 单解
  397. else if (det<MaxMinZero && det>0)
  398. {
  399. x1=x2=(-B)/(2*A);
  400. } 
  401. //'det<0 无解
  402. else if (det<0)
  403. {
  404. //需要再次探索
  405. x1=x2=0;
  406. }
  407. pbone->r_z =x1;
  408. pbone ->r_x = cx ;
  409. pbone ->r_y = cy ;
  412. /*
  413. printf( "A: %f n",A);
  414. printf( "B: %f n",B);
  415. printf( "C: %f n",C);
  417. */
  418. }
  419. return 0;
  421. }
  423. void skelecton::CorrectPosition(int ID,CvPoint2D32f &pt,kalman &bone_kalman,const CvArr* grey)
  424. {
  425. /*
  426. 一些假设：1。neck为支点不动，
  427.   2。支点坐标轴不偏斜，始终y向上,x向左，z向外,即人体运动垂直与摄像位置，
  428.   没有明显的侧身运动等。躯干运动不受限制。
  429.   head: L*-sinx2 < r_x < L*sinx1 ,   x^2 + y^2 <= L^2 ; 
  430.   shoulder: shoulder.x  <= neck.x + L ;
  431.   eblow: x^2 + y^2 <= L^2 ;
  432.   rWrist: x^2 + y^2 <= L^2 ; 
  433.   waist: waist.y = neck.y + L
  434.   keen:  x^2 + y^2 <= L^2 ; x<0;
  435.   ankle: x^2 + y^2 <= L^2 ;
  437. */
  438. //检查坐标，将经过跟踪后的2D坐标，代入表达式
  439. //如果位于人体运动范围内
  440. //
  441. //如果超出人体运动范围内（如丢掉特征点，自遮挡），开始利用kalman滤波，
  442. //如果kalman滤波位于人体运动范围内,以kalman预测的点周围寻找新的特征点
  443. //如果kalman滤波超出人体运动范围内,按照线性插值给一个特征点
  445. bone *pbone;
  446. bone *ppbone;
  447. bool is_inarea=true;
  448. CvPoint2D32f predict_point;
  449. double bone_length = 0; //对应骨架的长度
  451. pbone = get_bone_by_ID (ID);
  452. pbone->is_marked =1;
  453. double x,y,x0,y0;
  454. x=pt.x;
  455. y=pt.y;
  456. if(0!=pbone->PID)
  457. {
  458. bone_length = (pbone->LengthRatio * (1+StardardLengthNoiseRatio)) * StardardLength;
  459. ppbone = get_bone_by_ID(pbone->PID); 
  460. x0=ppbone->x;
  461. y0=ppbone->y;
  462. if(pow(x-x0,2) + pow(y-y0,2) > pow(bone_length,2)) 
  463. is_inarea = false;
  464. }
  466. //printf("before  : %f %f n",x,y); 
  468. predict_point = bone_kalman.get_predict(x,y);
  470. //printf("predict : %f %f n",predict_point.x,predict_point.y); 
  472. if(is_inarea) 
  473. {
  475. //左肩右肩，脖子，腰部，加刚性三角形约束
  476. }
  477. else
  478. {
  479. if(pow(predict_point.x-x0,2) + pow(predict_point.y-y0,2) > pow(bone_length,2)) 
  480. {
  481. //预测点也不在刚体约束内，则宣布特征点丢失
  482. //特征点丢失，利用运动连续性，根据上五帧的情况，按照匀速线性插值，预测一个点
  483. //to do list
  484. if (pre_ske!=NULL)
  485. {
  486. bone* pre_pbone =NULL;
  487. //pre_pbone = pre_ske->get_bone_by_ID (pbone->ID);
  488. //pt.x = pre_pbone->r_x;
  489. //pt.y = pre_pbone->r_y;
  491. }
  493. }
  494. else
  495. {
  496. //预测点在刚体约束内，添加在指定范围寻找特征点
  497. //to do list
  498. pt.x = predict_point.x;
  499. pt.y = predict_point.y;
  500. }
  501. /*
  502. cvFindCornerSubPix( grey, &pt, 1,
  503.         cvSize(10,10), cvSize(-1,-1),
  504.     cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
  505. */
  507. }
  508. pbone->x = (int)pt.x;
  509. pbone->y = (int)pt.y;
  511. }
  513. void skelecton::CalcLengthRatio(int ID,CvPoint2D32f pt)
  514. {
  515. bone *pbone;
  516. bone *ppbone;
  517. double bone_length;
  518. double x,y,x0,y0;
  520. pbone = get_bone_by_ID(ID); 
  521. if(pbone!=NULL) 
  522. {
  523. if(0!=pbone->PID)
  524. {
  525. x=pt.x;
  526. y=pt.y;
  528. ppbone = get_bone_by_ID(pbone->PID); 
  529. x0=ppbone->x;
  530. y0=ppbone->y;
  531. bone_length = sqrt(pow(x-x0,2) + pow(y-y0,2));
  532. pbone->LengthRatio = bone_length/StardardLength ;
  534. }
  535. else
  536. pbone->LengthRatio = 0;
  538. pbone->x = (int)pt.x;
  539. pbone->y = (int)pt.y;
  540. }
  543. }
  545. //根据人体模型来消除z坐标的二义性
  546. int skelecton::get_porper_Z(int x1, int x2, bone *pbone)
  547. {
  549. //return x2;
  550. //利用上一状态估计当前状态,取离上一状态最近的点
  551. bone *pre_pbone=NULL;
  552. int result = 0;
  553. int pre_z = 0 ;
  555. if (pre_ske!=NULL)
  556. {
  557. pre_pbone = pre_ske->get_bone_by_ID (pbone->ID);
  558. pre_z = pre_pbone->r_z ;
  559. if(abs(pre_z-x1)>abs(pre_z-x2)) 
  560. result= x2;
  561. else
  562. result= x1;
  563. }
  564. result= x1;
  565. //肢体本身的二义性
  566. if(pre_pbone!=NULL)
  567. {
  568. switch(pbone->ID)
  569. {
  570. case 5: //rElbow 胳膊肘不能向外拐
  571. result = check_porper_Z(x1,x2,pre_pbone,false);
  572. break;
  573. case 6: //lElbow 胳膊肘不能向外拐
  574. result = check_porper_Z(x1,x2,pre_pbone,true);
  575. break;
  576. case 10: //rKnee 膝盖不能内弯
  577. result = check_porper_Z(x1,x2,pre_pbone,false);
  578. break;
  579. case 11: //lKnee 膝盖不能内弯
  580. result = check_porper_Z(x1,x2,pre_pbone,true);
  581. break;
  582. }
  583. }
  584. return result;
  585. }
  587. void skelecton::CalcBonePosition()
  588. {
  589. CalcBoneRealWorldPosition_parallel("neck","head");
  590. CalcBoneRealWorldPosition_parallel("neck","rShoulder");
  591. CalcBoneRealWorldPosition_parallel("neck","lShoulder");
  592. CalcBoneRealWorldPosition_parallel("rShoulder","rElbow");
  593. CalcBoneRealWorldPosition_parallel("lShoulder","lElbow");
  594. CalcBoneRealWorldPosition_parallel("rElbow","rWrist");
  595. CalcBoneRealWorldPosition_parallel("lElbow","lWrist");
  596. CalcBoneRealWorldPosition_parallel("neck","Waist");
  597. CalcBoneRealWorldPosition_parallel("Waist","rKnee");
  598. CalcBoneRealWorldPosition_parallel("Waist","lKnee");
  599. CalcBoneRealWorldPosition_parallel("rKnee","rAnkle");
  600. CalcBoneRealWorldPosition_parallel("lKnee","lAnkle");
  602. }
  605. void skelecton::set_previous_skelecton(skelecton *pre_skelecton)
  606. {
  607. pre_ske = pre_skelecton;
  608. }
  610. //将坐标旋转到肩膀与手腕的向量上，求得到点的值是否在向量上方
  611. int skelecton::check_porper_Z(int x1, int x2, bone *pbone,bool IsLeft)
  612. {
  613. int xx0,zz0,xx1,xx2,zz1,zz2,result=0;
  614. float sinx,cosx,radiolen;
  615. float zz,zzz;
  616. bone *ppbone,*cpbone;
  617. ppbone = pre_ske->get_bone_by_ID (pbone->PID);
  618. cpbone = pre_ske->get_bone_by_ID (pbone->CID);
  619. if (ppbone!=NULL && cpbone!=NULL)
  620. {
  621. xx1 = ppbone->r_x;
  622. zz1 = ppbone->r_z;
  623. xx2 = cpbone->r_x;
  624. zz2 = cpbone->r_z;
  625. xx0=xx2-xx1;
  626. zz0=zz2-zz1;
  627. radiolen = sqrt(xx0*xx0 + zz0*zz0);
  628. sinx=xx0/radiolen;
  629. cosx=zz0/radiolen;
  630. zz=(x1-zz1)*sinx+(x1-zz1)*cosx;  
  631. zzz=(x2-zz1)*sinx+(x2-zz1)*cosx;
  632. if(IsLeft)
  633. {
  634. if(zz<0) 
  635. result = x1;
  636. if(zzz<0)
  637. result = x2;
  638. }
  639. else
  640. {
  641. if(zz>0) 
  642. result = x1;
  643. if(zzz>0)
  644. result = x2;
  645. }
  647. }
  648. return result;
  650. }
  652. int skelecton::drawOpenGL3D(CvArr *motion_track_img)
  653. {
  655. bone_vector::iterator theIterator;
  656. bone *pbone = 0;
  657. bone *ppbone = 0;
  658. Vector3f  vt1,vt2;
  660. ppbone = get_bone_by_name("neck"); 
  661. ppbone->r_z =0;  //初始化head的坐标为0
  664. CvSize ImageSize; 
  665. ImageSize = cvGetSize(motion_track_img);
  666. coordinator_X = (int)ImageSize.width/2;
  667. coordinator_Y = (int)ImageSize.height/2;
  669. CalcBonePosition();
  671. assert(pgl!=NULL);
  672. if(pgl!=NULL)  pgl->clear();
  675.     for (theIterator = bonevec.begin(); theIterator != bonevec.end();
  676.          theIterator++)
  677.     {
  678.   pbone = theIterator;
  680. if(pbone->is_marked==0)  continue; //未标记则不画
  681.   if ( 0 != pbone->PID )  
  682.   {
  683.   ppbone = get_bone_by_ID(pbone->PID);    
  684.   vt1.x = pbone->r_x;
  685.   vt1.y = pbone->r_y;
  686.   vt1.z = pbone->r_z;
  687.   vt2.x = ppbone->r_x;
  688.   vt2.y = ppbone->r_y;
  689.   vt2.z = ppbone->r_z;
  690.   if(pgl!=NULL)  pgl->draw2point(vt1,vt2);   
  691.   }
  693. } 
  697. if(pgl!=NULL) 
  698. {
  699. //pgl->Correct_3D_Layout(vt1,vt2);
  700. pgl->display();
  701. }
  702.     return 0;
  703. }
  705. void skelecton::view_up()
  706. {
  707. if(pgl!=NULL) 
  708.  pgl->x_arc = (pgl ->x_arc + 5) % 360;
  709. }
  711. void skelecton::view_down()
  712. {
  713. if(pgl!=NULL) 
  714. pgl->x_arc = (pgl ->x_arc - 5) % 360;
  715. }
  717. void skelecton::view_left()
  718. {
  719. if(pgl!=NULL) 
  720. pgl->y_arc = (pgl ->y_arc + 5) % 360;
  721. }
  723. void skelecton::view_right()
  724. {
  725. if(pgl!=NULL) 
  726. pgl->y_arc = (pgl ->y_arc - 5) % 360;
  727. }