Network System Laboratory - Backup diff of SLKinect1/sourcecode/convertRot2SLData vs current(No. 2)

View the diff.
View the source.
Go to SLKinect1/sourcecode/convertRot2SLData.

--- 2: 2011-11-11 (Fri) 15:34:38 iseki
+++ Cur: 2011-12-15 (Thu) 21:44:41 admin
@@ Line 1: / Line 1: @@
-*** convertRot2SLData (of SLKinect-v1.0.1) [#c6a2d789]
+ void    CExKinectWnd::convertRot2SLData(int uid)
- void    CExKinectWnd::convertRot2SLData(int uid)
  {
-     for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
+    Vector<double> vect;
-         int n = OpenNI2SLJointNum[j];
+    bool normal_joint[OPENNI_MAX_JOINT_NUM];
-         if (n>=0) {
-             XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j);
+    //
+    for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
-             double m11 = rot.elements[0];
+        if (OpenNI2SLJointNum[j]>=0) {
-             double m12 = rot.elements[1];
+            if (jointPositionConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) {
-             double m13 = rot.elements[2];
+                XnVector3D  pos = jointPositionData((XnSkeletonJoint)j);
-             double m21 = rot.elements[3];
+                posVect[j].set(-pos.Z, pos.X, pos.Y);
-             double m31 = rot.elements[6];
+                prvVect[j] = posVect[j];
-             double m32 = rot.elements[7];
+            }
-             double m33 = rot.elements[8];
+            else {
-             Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33);
+                posVect[j] = prvVect[j];
-             Vector<double> vct;
+            }
+        }
-             if (j==3) {
+    }
-                 vct.set(0.0, -eul.y/2, 0.0);
-                 rotQuat[0].setEulerYZX(vct);
-                 vct.set(-eul.x, -eul.y/2, eul.z);
+    // mPelvis
-             }
+    posVect[0] = (posVect[17] + posVect[21])*0.5;
-             else {
+    vect = posVect[21] - posVect[17];
-                 vct.set(-eul.x, -eul.y, eul.z);
+    double thz = atan2(-vect.x, vect.y);
-             }
+    rotQuat[0].setRotation(thz, 0.0, 0.0, 1.0);
-             rotQuat[j].setEulerYZX(vct);
+    if (!isCalibrated) setStartPosData(posVect[0]);
-         }
-     }
+    //
-     XnVector3D  pos = jointPositionData((XnSkeletonJoint)3);
+    for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
-     Vector&lt;double&gt; vect(-pos.Z, pos.X, pos.Y);
+        normal_joint[j] = true;
-     if (!isCalibrated) setStartPosData(vect);
+        if (OpenNI2SLJointNum[j]>=0) {
-     //
+            if (jointRotationConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) {
-     //rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3];
+                XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j);
-     rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2];
+                double m11 = rot.elements[0];
+                double m12 = rot.elements[1];
-     rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6];
+                double m13 = rot.elements[2];
-     rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7];
+                double m21 = rot.elements[3];
+                double m31 = rot.elements[6];
-     rotQuat[12] = ~rotQuat[ 2]*rotQuat[12];
+                double m32 = rot.elements[7];
- ;    rotQuat[13] = ~rotQuat[12]*rotQuat[13];
+                double m33 = rot.elements[8];
+                Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33);
-     //rotQuat[17] = ~rotQuat[ 3]*rotQuat[17];
-     rotQuat[18] = ~rotQuat[17]*rotQuat[18];
+                Vector<double> vct(-eul.x, -eul.y, eul.z);    // Mirror: vct(-eul.x, eul.y, -eul.z);
+                rotQuat[j].setEulerYZX(vct);
-     //rotQuat[21] = ~rotQuat[ 3]*rotQuat[21];
+                prvQuat[j] = rotQuat[j];
-     rotQuat[22] = ~rotQuat[21]*rotQuat[22];
+            }
+            else {
+                rotQuat[j] = prvQuat[j];
-     // Set to Shared Memory
+            }
-     for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
+&nbsp;       }
-         int n = OpenNI2SLJointNum[j];
+&nbsp;   }
-         if (n>=0) {
+ ;
-             double* shm = theApp.ptrShm[n];
-             if (shm!=NULL) {
+    // Leg
-                 shm[4] = rotQuat[j].x;
+    rotQuat[22] = ~rotQuat[21]*rotQuat[22];
-                 shm[5] = rotQuat[j].y;
+    rotQuat[18] = ~rotQuat[17]*rotQuat[18];
-                 shm[6] = rotQuat[j].z;
-                 shm[7] = rotQuat[j].s;
+    // Right Hand
-             }
+    rotQuat[13] = ~rotQuat[12]*rotQuat[13];
-         }
+    rotQuat[12] = ~rotQuat[ 2]*rotQuat[12];
-     }
+    // Left Hand
-     // Position Data from XN_SKEL_TORSO(3) to mPelvis(0)
+    rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7];
-     double* shm = theApp.ptrShm[0];
+    rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6];
-     if (shm!=NULL) {
-         shm[1] = (vect.x - startPos.x)/1000.;
+    rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2];
-         shm[2] = (vect.y - startPos.y)/1000.;
+    rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3];
-         shm[3] = (vect.z - startPos.z)/1000.;
-         shm[4] = rotQuat[0].x;
-         shm[5] = rotQuat[0].y;
+    // Set to Shared Memory
-         shm[6] = rotQuat[0].z;
+    for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
-         shm[7] = rotQuat[0].s;
+        int n = OpenNI2SLJointNum[j];
-     }
+        if (n>=0 && normal_joint[j]) {
+            double* shm = theApp.ptrShm[n];
+            if (shm!=NULL) {
+                shm[4] = rotQuat[j].x;
+                shm[5] = rotQuat[j].y;
+                shm[6] = rotQuat[j].z;
+                shm[7] = rotQuat[j].s;
+            }
+        }
+    }
+    // mPelvis(0)
+    double* shm = theApp.ptrShm[0];
+    if (shm!=NULL) {
+        shm[1] = (posVect[0].x - startPos.x)/1000.;
+        shm[2] = (posVect[0].y - startPos.y)/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;
+    }
  }