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; |
| + | } |
| } | | } |