1: 2011-11-05 (Sat) 18:29:03 iseki  |
Cur: 1970-01-01 (Thu) 18:00:00 |
| - | *** 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.; | |
| - | } | |
| - | } | |
