2: 2011-11-11 (金) 15:34:38 iseki  |
3: 2011-12-11 (日) 21:25:50 iseki |
| - | *** 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++) { |
| | + | if (OpenNI2SLJointNum[j]>;=0) { |
| | + | if (jointPositionConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) { |
| | + | XnVector3D pos = jointPositionData((XnSkeletonJoint)j); |
| | + | posVect[j].set(-pos.Z, pos.X, pos.Y); |
| | + | prvVect[j] = posVect[j]; |
| | + | } |
| | + | else { |
| | + | posVect[j] = prvVect[j]; |
| | + | } |
| | + | } |
| | + | } |
| | + | |
| | + | |
| | + | // mPelvis |
| | + | posVect[0] = (posVect[17] + posVect[21])*0.5; |
| | + | vect = posVect[21] - posVect[17]; |
| | + | double thz = atan2(-vect.x, vect.y); |
| | + | rotQuat[0].setRotation(thz, 0.0, 0.0, 1.0); |
| | + | if (!isCalibrated) setStartPosData(posVect[0]); |
| | | | |
| - | double m11 = rot.elements[0]; | + | // |
| - | double m12 = rot.elements[1]; | + | for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) { |
| - | double m13 = rot.elements[2]; | + | normal_joint[j] = true; |
| - | 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; | + | |
| | | | |
| - | if (j==3) { | + | if (OpenNI2SLJointNum[j]>=0) { |
| - | vct.set(0.0, -eul.y/2, 0.0); | + | if (jointRotationConfidence((XnSkeletonJoint)j)>;EXKINECT_WND_CONFIDENCE) { |
| - | rotQuat[0].setEulerYZX(vct); | + | XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j); |
| - | vct.set(-eul.x, -eul.y/2, eul.z); | + | double m11 = rot.elements[0]; |
| - | } | + | double m12 = rot.elements[1]; |
| - | else { | + | double m13 = rot.elements[2]; |
| - | vct.set(-eul.x, -eul.y, eul.z); | + | double m21 = rot.elements[3]; |
| - | } | + | double m31 = rot.elements[6]; |
| - | rotQuat[j].setEulerYZX(vct); | + | double m32 = rot.elements[7]; |
| - | } | + | double m33 = rot.elements[8]; |
| - | } | + | Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33); |
| | | | |
| - | XnVector3D pos = jointPositionData((XnSkeletonJoint)3); | + | if (j==2 || j==3 || j==6 || j==12 || j==17 || j==21) { |
| - | Vector<double> vect(-pos.Z, pos.X, pos.Y); | + | if (Xabs(-eul.y-thz)>PI_DIV2) eul.y = -thz; |
| - | if (!isCalibrated) setStartPosData(vect); | + | } |
| | + | 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]; |
| | + | } |
| | + | } |
| | + | } |
| | | | |
| - | // | |
| - | //rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3]; | |
| - | rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2]; | |
| | | | |
| - | rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6]; | + | // Leg |
| - | rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7]; | + | rotQuat[22] = ~rotQuat[21]*rotQuat[22]; |
| | + | rotQuat[18] = ~rotQuat[17]*rotQuat[18]; |
| | | | |
| - | rotQuat[12] = ~rotQuat[ 2]*rotQuat[12]; | + | // Right Hand |
| - | rotQuat[13] = ~rotQuat[12]*rotQuat[13]; | + | rotQuat[13] = ~rotQuat[12]*rotQuat[13]; |
| | + | rotQuat[12] = ~rotQuat[ 2]*rotQuat[12]; |
| | | | |
| - | //rotQuat[17] = ~rotQuat[ 3]*rotQuat[17]; | + | // Left Hand |
| - | rotQuat[18] = ~rotQuat[17]*rotQuat[18]; | + | rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7]; |
| | + | rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6]; |
| | | | |
| - | //rotQuat[21] = ~rotQuat[ 3]*rotQuat[21]; | + | rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2]; |
| - | rotQuat[22] = ~rotQuat[21]*rotQuat[22]; | + | 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) { |
| - | shm[4] = rotQuat[j].x; | + | shm[4] = rotQuat[j].x; |
| - | shm[5] = rotQuat[j].y; | + | shm[5] = rotQuat[j].y; |
| - | shm[6] = rotQuat[j].z; | + | shm[6] = rotQuat[j].z; |
| - | shm[7] = rotQuat[j].s; | + | shm[7] = rotQuat[j].s; |
| - | } | + | } |
| - | } | + | } |
| - | } | + | } |
| | | | |
| - | // 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[4] = rotQuat[0].x; |
| - | shm[5] = rotQuat[0].y; | + | shm[5] = rotQuat[0].y; |
| - | shm[6] = rotQuat[0].z; | + | shm[6] = rotQuat[0].z; |
| - | shm[7] = rotQuat[0].s; | + | shm[7] = rotQuat[0].s; |
| - | } | + | } |
| | } | | } |
