SLKinect1/sourcecode/convertRot2SLData の変更点

• 追加された行はこの色です。
• 削除された行はこの色です。
• SLKinect1/sourcecode/convertRot2SLData へ行く。
• SLKinect1/sourcecode/convertRot2SLData の差分を削除

#author("2024-01-03T16:44:05+00:00","default:iseki","iseki")
#author("2024-01-03T16:44:31+00:00","default:iseki","iseki")
 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;
    }
 }