SLKinect1/sourcecode/convertRot2SLData
をテンプレートにして作成
[
トップ
] [
タイトル一覧
|
ページ一覧
|
新規
|
検索
|
最終更新
|
ヘルプ
|
ログイン
]
開始行:
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)...
XnVector3D pos = jointPositionData((XnSk...
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)...
XnMatrix3X3 rot = jointRotationData((XnSk...
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 = RotMatrixElements2Eu...
Vector<double> 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;
}
}
終了行:
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)...
XnVector3D pos = jointPositionData((XnSk...
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)...
XnMatrix3X3 rot = jointRotationData((XnSk...
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 = RotMatrixElements2Eu...
Vector<double> 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;
}
}
ページ名:
![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
