|
1: 2011-11-05 (Sat) 18:31:19 iseki  |
| | + | *** convertRot2SLData (of SLKinect-v1.0.0) [#c6a2d789] |
| | + | void CExKinectWnd::convertRot2SLData(int uid) |
| | + | { |
| | + | for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) { |
| | + | int n = OpenNI2SLJointNum[j]; |
| | + | if (n>=0) { |
| | + | 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(-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[ 2] = ~rotQuat[ 3]*rotQuat[ 2]; |
| | + | 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 |
| | + | for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) { |
| | + | int n = OpenNI2SLJointNum[j]; |
| | + | if (n>=0) { |
| | + | 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; |
| | + | } |
| | + | } |
| | + | } |
| | + | |
| | + | // Position Data from XN_SKEL_TORSO(3) to mPelvis(0) |
| | + | double* shm = theApp.ptrShm[0]; |
| | + | if (shm!=NULL) { |
| | + | shm[1] = (vect.x - startPos.x)/1000.; |
| | + | shm[2] = (vect.y - startPos.y)/1000.; |
| | + | shm[3] = (vect.z - startPos.z)/1000.; |
| | + | } |
| | + | } |
