SLKinect1/sourcecode/convertRot2SLData のバックアップ(No.1)

• バックアップ一覧
• 差分 を表示
• 現在との差分 を表示
• ソース を表示
• SLKinect1/sourcecode/convertRot2SLData へ行く。
  • 1 (2024-01-04 (木) 01:44:05)

void    CExKinectWnd::convertRot2SLData(int uid)
{
   Vector<double> vect;
   bool normal_joint[OPENNI_MAX_JOINT_NUM];

   //
   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]);


   //
   for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
       normal_joint[j] = true;

       if (OpenNI2SLJointNum[j]>=0) {
           if (jointRotationConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) { 
               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);    // Mirror: vct(-eul.x, eul.y, -eul.z);
               rotQuat[j].setEulerYZX(vct);
               prvQuat[j] = rotQuat[j];
           }
           else {
               rotQuat[j] = prvQuat[j];
           }
       }
   }


   // Leg
   rotQuat[22] = ~rotQuat[21]*rotQuat[22];
   rotQuat[18] = ~rotQuat[17]*rotQuat[18];

   // Right Hand
   rotQuat[13] = ~rotQuat[12]*rotQuat[13];
   rotQuat[12] = ~rotQuat[ 2]*rotQuat[12];

   // Left Hand
   rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7];
   rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6];

   rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2];
   rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3];


   // Set to Shared Memory
   for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
       int n = OpenNI2SLJointNum[j];
       if (n>=0 && normal_joint[j]) {
           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;
           }
       }
   }

   // mPelvis(0)
   double* shm = theApp.ptrShm[0];
   if (shm!=NULL) {
       shm[1] = (posVect[0].x - startPos.x)/1000.;
       shm[2] = (posVect[0].y - startPos.y)/1000.;
       shm[3] = (posVect[0].z - startPos.z)/1000.;
       shm[4] = rotQuat[0].x;
       shm[5] = rotQuat[0].y;
       shm[6] = rotQuat[0].z;
       shm[7] = rotQuat[0].s;
   }
}