1: 2011-11-05 (土) 18:31:19 iseki  |
2: 2011-11-11 (金) 15:34:38 iseki |
| - | *** convertRot2SLData (of SLKinect-v1.0.0) [#c6a2d789] | + | *** 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++) { | + | for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) { |
| - | int n = OpenNI2SLJointNum[j]; | + | int n = OpenNI2SLJointNum[j]; |
| - | if (n>=0) { | + | if (n>=0) { |
| - | XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j); | + | 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; |
| | + | |
| | + | 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); |
| | + | } |
| | + | else { |
| | + | vct.set(-eul.x, -eul.y, eul.z); |
| | + | } |
| | + | rotQuat[j].setEulerYZX(vct); |
| | + | } |
| | + | } |
| | + | |
| | + | XnVector3D pos = jointPositionData((XnSkeletonJoint)3); |
| | + | Vector<double> vect(-pos.Z, pos.X, pos.Y); |
| | + | if (!isCalibrated) setStartPosData(vect); |
| | + | |
| | + | // |
| | + | //rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3]; |
| | + | rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2]; |
| | + | |
| | + | rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6]; |
| | + | rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7]; |
| | | | |
| - | double m11 = rot.elements[0]; | + | rotQuat[12] = ~rotQuat[ 2]*rotQuat[12]; |
| - | double m12 = rot.elements[1]; | + | rotQuat[13] = ~rotQuat[12]*rotQuat[13]; |
| - | 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); | + | |
| - | rotQuat[j].setEulerYZX(vct); | + | |
| - | } | + | |
| - | ; } | + | |
| | | | |
| - | XnVector3D pos = jointPositionData((XnSkeletonJoint)3); | + | //rotQuat[17] = ~rotQuat[ 3]*rotQuat[17]; |
| - | Vector<double> vect(-pos.Z, pos.X, pos.Y); | + | rotQuat[18] = ~rotQuat[17]*rotQuat[18]; |
| - | if (!isCalibrated) setStartPosData(vect); | + | |
| | | | |
| - | // | + | //rotQuat[21] = ~rotQuat[ 3]*rotQuat[21]; |
| - | rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2]; | + | rotQuat[22] = ~rotQuat[21]*rotQuat[22]; |
| - | rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6]; | + | |
| - | rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7]; | + | |
| - | rotQuat[12] = ~rotQuat[ 2]*rotQuat[12]; | + | |
| - | rotQuat[13] = ~rotQuat[12]*rotQuat[13]; | + | |
| - | rotQuat[18] = ~rotQuat[17]*rotQuat[18]; | + | |
| - | rotQuat[22] = ~rotQuat[21]*rotQuat[22]; | + | |
| | | | |
| | | | |
| - | // 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) { |
| - | 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) | + | // Position Data from XN_SKEL_TORSO(3) to 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] = (vect.x - startPos.x)/1000.; |
| - | shm[2] = (vect.y - startPos.y)/1000.; | + | shm[2] = (vect.y - startPos.y)/1000.; |
| - | shm[3] = (vect.z - startPos.z)/1000.; | + | shm[3] = (vect.z - startPos.z)/1000.; |
| - | } | + | shm[4] = rotQuat[0].x; |
| | + | shm[5] = rotQuat[0].y; |
| | + | shm[6] = rotQuat[0].z; |
| | + | shm[7] = rotQuat[0].s; |
| | + | } |
| | } | | } |
