3: 2011-12-11 (Sun) 21:25:50 iseki  |
Cur: 2011-12-15 (Thu) 21:44:41 admin |
| | Vector<double> vect; | | Vector<double> vect; |
| | bool normal_joint[OPENNI_MAX_JOINT_NUM]; | | 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++) { |
| | } | | } |
| | } | | } |
| - | | + | |
| | + | |
| | // mPelvis | | // mPelvis |
| | posVect[0] = (posVect[17] + posVect[21])*0.5; | | posVect[0] = (posVect[17] + posVect[21])*0.5; |
| | rotQuat[0].setRotation(thz, 0.0, 0.0, 1.0); | | rotQuat[0].setRotation(thz, 0.0, 0.0, 1.0); |
| | if (!isCalibrated) setStartPosData(posVect[0]); | | if (!isCalibrated) setStartPosData(posVect[0]); |
| - | | + | |
| | + | |
| | // | | // |
| | for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) { | | for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) { |
| | normal_joint[j] = true; | | normal_joint[j] = true; |
| | + | |
| | if (OpenNI2SLJointNum[j]>=0) { | | if (OpenNI2SLJointNum[j]>=0) { |
| | if (jointRotationConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) { | | if (jointRotationConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) { |
| | double m33 = rot.elements[8]; | | double m33 = rot.elements[8]; |
| | Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33); | | Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33); |
| - | | + | |
| - | if (j==2 || j==3 || j==6 || j==12 || j==17 || j==21) { | + | |
| - | if (Xabs(-eul.y-thz)>PI_DIV2) eul.y = -thz; | + | |
| - | } | + | |
| | Vector<double> vct(-eul.x, -eul.y, eul.z); // Mirror: vct(-eul.x, eul.y, -eul.z); | | Vector<double> vct(-eul.x, -eul.y, eul.z); // Mirror: vct(-eul.x, eul.y, -eul.z); |
| | rotQuat[j].setEulerYZX(vct); | | rotQuat[j].setEulerYZX(vct); |
| | } | | } |
| | } | | } |
| - | | + | |
| | + | |
| | // Leg | | // Leg |
| | rotQuat[22] = ~rotQuat[21]*rotQuat[22]; | | rotQuat[22] = ~rotQuat[21]*rotQuat[22]; |
| | rotQuat[18] = ~rotQuat[17]*rotQuat[18]; | | rotQuat[18] = ~rotQuat[17]*rotQuat[18]; |
| | + | |
| | // Right Hand | | // Right Hand |
| | rotQuat[13] = ~rotQuat[12]*rotQuat[13]; | | rotQuat[13] = ~rotQuat[12]*rotQuat[13]; |
| | rotQuat[12] = ~rotQuat[ 2]*rotQuat[12]; | | rotQuat[12] = ~rotQuat[ 2]*rotQuat[12]; |
| | + | |
| | // Left Hand | | // Left Hand |
| | rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7]; | | rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7]; |
| | rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6]; | | rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6]; |
| | + | |
| | rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2]; | | rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2]; |
| | rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3]; | | 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++) { |
| | } | | } |
| | } | | } |
| | + | |
| | // mPelvis(0) | | // mPelvis(0) |
| | double* shm = theApp.ptrShm[0]; | | double* shm = theApp.ptrShm[0]; |
