flagflag  
1: 2011-11-05 (Sat) 18:31:19 iseki source Cur: 2011-12-15 (Thu) 21:44:41 admin source
Line 1: Line 1:
-*** convertRot2SLData (of SLKinect-v1.0.0) [#c6a2d789] + void    CExKinectWnd::convertRot2SLData(int uid)
- void CExKinectWnd::convertRot2SLData(int uid)+
 {  {
 +   Vector<double> vect;
 +   bool normal_joint[OPENNI_MAX_JOINT_NUM];
 + 
 +   //
   for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {    for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
-       int n = OpenNI2SLJointNum[j]; +       if (OpenNI2SLJointNum[j]>;=0) { 
-        if (n>=0) { +            if (jointPositionConfidence((XnSkeletonJoint​)j)>EXKINECT_WND_CONFIDENCE) { 
-            XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j); +                XnVector3D  pos = jointPositionData((XnSkeletonJoint)j); 
- +                posVect[j].set(-pos.Z, pos.X, pos.Y); 
-            double m11 = rot.elements[0]; +                prvVect[j] = posVect[j]; 
-            double m12 = rot.elements[1]; +           } 
-           double m13 = rot.elements[2]; +           else { 
-           double m21 = rot.elements[3]; +                posVect[j] = prvVect[j]; 
-           double m31 = rot.elements[6]; +           }
-            double m32 = rot.elements[7]; +
-           double m33 = rot.elements[8]; +
-           Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33); +
-           Vector<double> vct(-eul.x, -eul.y, eul.z); +
-           rotQuat[j].setEulerYZX(vct);+
       }        }
   }    }
- +  
-   XnVector3D  pos = jointPositionData((XnSkeletonJoint)3); +  
-   Vector&lt;double&gt; vect(-pos.Z, pos.X, pos.Y); +   // mPelvis 
-   if (!isCalibrated) setStartPosData(vect); +   posVect[0] = (posVect[17] + posVect[21])*0.5; 
 +   vect = posVect[21] - posVect[17]; 
 +&nbsp;   double thz = atan2(-vect.x, vect.y); 
 +&nbsp;   rotQuat[0].setRotation(thz, 0.0, 0.0, 1.0); 
 +   if (!isCalibrated) setStartPosData(posVect[0]); 
 +  
 + 
   //    //
-   rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2]; +   for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) { 
-    rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6]; +       normal_joint[j] = true; 
-    rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7]; +  
-    rotQuat[12] = ~rotQuat[ 2]*rotQuat[12]; +       if (OpenNI2SLJointNum[j]>=0) { 
-    rotQuat[13] = ~rotQuat[12]*rotQuat[13]; +           if (jointRotationConfidence((XnSkeletonJoin​t)j)>EXKINECT_WND_CONFIDENCE) { 
-    rotQuat[18] = ~rotQuat[17]*rotQuat[18];+               XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j); 
 +               double m11 = rot.elements[0]; 
 +                double m12 = rot.elements[1]; 
 +               double m13 = rot.elements[2]; 
 +                double m21 = rot.elements[3]; 
 +               double m31 = rot.elements[6]; 
 +               double m32 = rot.elements[7]; 
 +                double m33 = rot.elements[8]; 
 +               Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33); 
 +  
 +               Vector<double> vct(-eul.x, -eul.y, eul.z);    // Mirror: vct(-eul.x, eul.y, -eul.z); 
 +               rotQuat[j].setEulerYZX(vct); 
 +                prvQuat[j] = rotQuat[j]; 
 +            } 
 +           else { 
 +               rotQuat[j] = prvQuat[j]; 
 +           } 
 +       } 
 +   } 
 +  
 +  
 +   // Leg
   rotQuat[22] = ~rotQuat[21]*rotQuat[22];    rotQuat[22] = ~rotQuat[21]*rotQuat[22];
- +   rotQuat[18] = ~rotQuat[17]*rotQuat[18]; 
 +  
 +   // Right Hand 
 +   rotQuat[13] = ~rotQuat[12]*rotQuat[13]; 
 +   rotQuat[12] = ~rotQuat[ 2]*rotQuat[12]; 
 +  
 +   // Left Hand 
 +   rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7]; 
 +   rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6]; 
 +  
 +   rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2]; 
 +   rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3]; 
 +  
 + 
   // Set to Shared Memory    // Set to Shared Memory
   for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {    for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
       int n = OpenNI2SLJointNum[j];        int n = OpenNI2SLJointNum[j];
-       if (n>=0) {+       if (n>=0 && normal_joint[j]) {
           double* shm = theApp.ptrShm[n];            double* shm = theApp.ptrShm[n];
           if (shm!=NULL) {            if (shm!=NULL) {
Line 47: Line 83:
       }        }
   }    }
- +  
-   // Position Data from XN_SKEL_TORSO(3) to mPelvis(0)+   // mPelvis(0)
   double* shm = theApp.ptrShm[0];    double* shm = theApp.ptrShm[0];
   if (shm!=NULL) {    if (shm!=NULL) {
-       shm[1] = (vect.x - startPos.x)/1000.; +       shm[1] = (posVect[0].x - startPos.x)/1000.; 
-       shm[2] = (vect.y - startPos.y)/1000.; +       shm[2] = (posVect[0].y - startPos.y)/1000.; 
-       shm[3] = (vect.z - startPos.z)/1000.;+       shm[3] = (posVect[0].z - startPos.z)/1000.; 
 +       shm[4] = rotQuat[0].x; 
 +       shm[5] = rotQuat[0].y; 
 +       shm[6] = rotQuat[0].z; 
 +       shm[7] = rotQuat[0].s;
   }    }
 }  }
Front page   New List of Pages Search Recent changes   Help   RSS of recent changes (RSS 1.0) RSS of recent changes (RSS 2.0) RSS of recent changes (RSS Atom)
Page owner: iseki
Site admin: admin

Site Search


Advanced Search

Login
Username:

Password:


Lost Password?
Register now!!

Sub Menu

Books

mini Calendar
Last MonthMay 2024Next Month
Su Mo Tu We Th Fr Sa
1 2 3 4
5 6 7 8 9 10 11
12 13 14 15 16 17 18
19 20 21 22 23 24 25
26 27 28 29 30 31
Today

Who's Online
74 user(s) are online (34 user(s) are browsing xpwiki)

Members: 0
Guests: 74

more...

Access Counter
Today : 4154415441544154
Yesterday : 4372437243724372
Total : 2366566723665667236656672366566723665667236656672366566723665667
Powered by XOOPS Cube 2.1© 2001-2006 XOOPS Cube Project
Design by XoopsDesign.com