void send(const xn::DepthMetaData& dmd)
{
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
userGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
memset(&SendBuf, '\0', BufLen); // this may be expensive
if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
strcpy(&SendBuf[0], "sensor-update");
for(int j = 0; j < 24 ; j++)
{
if(jointPos(aUsers[i], jointList[j]) == 0)
{
sprintf(&SendBuf[strlen(SendBuf)], " \"%s_x\" %f", jointNames[j], jointCoords[1]); // I know, I know :(
sprintf(&SendBuf[strlen(SendBuf)], " \"%s_y\" %f", jointNames[j], jointCoords[2]);
}
}
// Before we send each bundle, we need to send some size code
unsigned char sizeBytes[4];
//strcpy(SendBuf, P.c_str());
int len = strlen(SendBuf);
sizeBytes[0] =(unsigned char)( len >> 24 );
sizeBytes[1] =(unsigned char)( (len << 8) >> 24 );
sizeBytes[2] =(unsigned char)( (len << 16) >> 24 );
sizeBytes[3] =(unsigned char)( (len << 24) >> 24 );
if (debug)
printf("%s\n\n", SendBuf);
sendto(SendSocket, (char*)sizeBytes, 4, 0, (struct sockaddr *) &RecvAddr, sizeof(RecvAddr));
sendto(SendSocket, SendBuf, strlen(SendBuf), 0, (struct sockaddr *) &RecvAddr, sizeof(RecvAddr));
}
}
void CharacterSkeleton::IKChain::ikStep(const glm::vec3& e) {
vector<glm::vec3> J;
for (unsigned int i = 0; i < ikChain.size(); ++i) {
glm::vec3 jointPos(ikChain[i]->globalTransformation * glm::vec4(0, 0, 0, 1));
glm::vec3 j;
j = glm::vec3(ikChain[i]->globalTransformation * glm::vec4(1, 0, 0, 0));
j = glm::cross(j, endEffector - jointPos);
J.push_back(j);
j = glm::vec3(ikChain[i]->globalTransformation * glm::vec4(0, 0, 1, 0));
j = glm::cross(j, endEffector - jointPos);
J.push_back(j);
}
// TODO: change to mat3
vector<glm::vec3> J_JT;
for (unsigned int i = 0; i < 3; ++i) {
glm::vec3 rowI;
for (unsigned int j = 0; j < 3; ++j) {
float c = 0;
for (unsigned int k = 0; k < J.size(); ++k)
c += J[k][i] * J[k][j];
rowI[j] = c;
}
J_JT.push_back(rowI);
}
glm::vec3 J_JT_e;
for (unsigned int i = 0; i < 3; ++i)
J_JT_e[i] = glm::dot(J_JT[i], e);
float alpha = glm::dot(e, J_JT_e) / glm::dot(J_JT_e, J_JT_e);
unsigned int cRowJT = 0;
for (unsigned int i = 0; i < ikChain.size(); ++i) {
float xRot = alpha * glm::dot(J[cRowJT], e);
++cRowJT;
float zRot = alpha * glm::dot(J[cRowJT], e);
++cRowJT;
glm::quat rotDelta(glm::vec3(xRot, 0, zRot));
ikChain[i]->rotation = ikChain[i]->rotation * rotDelta;
glm::mat4 scalingM = glm::scale(glm::mat4(), ikChain[i]->scaling);
glm::mat4 rotationM = glm::mat4_cast(ikChain[i]->rotation);
glm::mat4 translationM = glm::translate(glm::mat4(), ikChain[i]->translation);
glm::mat4 nodeTransformation = translationM * rotationM * scalingM;
//ikChain[i]->nodeTransformation = nodeTransformation;
glm::mat4 parentTransform;
if (ikChain[i]->parent)
parentTransform = ikChain[i]->parent->globalTransformation;
ikChain[i]->globalTransformation = parentTransform * nodeTransformation;
}
/*IKLink* currentLink = linkHierarchy;
vector<glm::vec3> J;
while (currentLink) {
glm::vec3 jointPos(currentLink->globalTransformation * glm::vec4(0, 0, 0, 1));
//glm::vec3 j = glm::cross(glm::vec3(0, 0, 1), endEffector - jointPos);
//J.push_back(j);
glm::vec3 j;
j = glm::vec3(currentLink->globalTransformation * glm::vec4(1, 0, 0, 0));
j = glm::cross(j, endEffector - jointPos);
J.push_back(j);
j = glm::vec3(currentLink->globalTransformation * glm::vec4(0, 0, 1, 0));
j = glm::cross(j, endEffector - jointPos);
J.push_back(j);
currentLink = currentLink->child;
}
// TODO: change to mat3
vector<glm::vec3> J_JT;
for (unsigned int i = 0; i < 3; ++i) {
glm::vec3 rowI;
for (unsigned int j = 0; j < 3; ++j) {
float c = 0;
for (unsigned int k = 0; k < J.size(); ++k)
c += J[k][i] * J[k][j];
rowI[j] = c;
}
J_JT.push_back(rowI);
}
glm::vec3 e = targetPosition - endEffector;
glm::vec3 J_JT_e;
for (unsigned int i = 0; i < 3; ++i)
J_JT_e[i] = glm::dot(J_JT[i], e);
float alpha = glm::dot(e, J_JT_e) / glm::dot(J_JT_e, J_JT_e);
currentLink = linkHierarchy;
unsigned int cRowJT = 0;
while (currentLink) {
//currentLink->angle += alpha * glm::dot(J[cRowJT], e);
//++cRowJT;
float xRot = alpha * glm::dot(J[cRowJT], e);
++cRowJT;
float zRot = alpha * glm::dot(J[cRowJT], e);
++cRowJT;
glm::quat rotDelta(glm::vec3(xRot, 0, zRot));
currentLink->rotation = currentLink->rotation * rotDelta;
currentLink = currentLink->child;
}*/
}
void sendOSC() {
if (handMode) {
sendHandOSC();
return;
}
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
userGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i) {
if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
lo_bundle bundle = lo_bundle_new(LO_TT_IMMEDIATE);
if (jointPos(aUsers[i], XN_SKEL_HEAD) == 0) {
oscFunc(&bundle, "head", 0);
}
if (jointPos(aUsers[i], XN_SKEL_NECK) == 0) {
oscFunc(&bundle, "neck", 1);
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_COLLAR) == 0) {
oscFunc(&bundle, "l_collar", 2);
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_SHOULDER) == 0) {
oscFunc(&bundle, "l_shoulder", 3);
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_ELBOW) == 0) {
oscFunc(&bundle, "l_elbow", 4);
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_WRIST) == 0) {
oscFunc(&bundle, "l_wrist", 5);
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_HAND) == 0) {
oscFunc(&bundle, "l_hand", 6);
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_FINGERTIP) == 0) {
oscFunc(&bundle, "l_fingertip", 7);
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_COLLAR) == 0) {
oscFunc(&bundle, "r_collar", 8);
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_SHOULDER) == 0) {
oscFunc(&bundle, "r_shoulder", 9);
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_ELBOW) == 0) {
oscFunc(&bundle, "r_elbow", 10);
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_WRIST) == 0) {
oscFunc(&bundle, "r_wrist", 11);
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_HAND) == 0) {
oscFunc(&bundle, "r_hand", 12);
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_FINGERTIP) == 0) {
oscFunc(&bundle, "r_fingertip", 13);
}
if (jointPos(aUsers[i], XN_SKEL_TORSO) == 0) {
oscFunc(&bundle, "torso", 14);
}
if (jointPos(aUsers[i], XN_SKEL_WAIST) == 0) {
oscFunc(&bundle, "waist", 15);
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_HIP) == 0) {
oscFunc(&bundle, "l_hip", 16);
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_KNEE) == 0) {
oscFunc(&bundle, "l_knee", 17);
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_ANKLE) == 0) {
oscFunc(&bundle, "l_ankle", 18);
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_FOOT) == 0) {
oscFunc(&bundle, "l_foot", 19);
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_HIP) == 0) {
oscFunc(&bundle, "r_hip", 20);
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_KNEE) == 0) {
oscFunc(&bundle, "r_knee", 21);
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_ANKLE) == 0) {
oscFunc(&bundle, "r_ankle", 22);
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_FOOT) == 0) {
oscFunc(&bundle, "r_foot", 23);
}
if (lo_send_bundle(addr, bundle) != 0) {
printf("error: unable to send bundle\n");
lo_bundle_pp(bundle);
}
lo_bundle_free_messages(bundle);
}
else {
//Send user's center of mass
sendUserPosMsg(aUsers[i]);
}
}
}
void sendOSC() {
if (handMode) {
sendHandOSC();
return;
}
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
userGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i) {
if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
osc::OutboundPacketStream p(osc_buffer, OUTPUT_BUFFER_SIZE);
p << osc::BeginBundleImmediate;
if (jointPos(aUsers[i], XN_SKEL_HEAD) == 0) {
oscFunc(&p, "head");
}
if (jointPos(aUsers[i], XN_SKEL_NECK) == 0) {
oscFunc(&p, "neck");
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_COLLAR) == 0) {
oscFunc(&p, "l_collar");
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_SHOULDER) == 0) {
oscFunc(&p, "l_shoulder");
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_ELBOW) == 0) {
oscFunc(&p, "l_elbow");
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_WRIST) == 0) {
oscFunc(&p, "l_wrist");
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_HAND) == 0) {
oscFunc(&p, "l_hand");
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_FINGERTIP) == 0) {
oscFunc(&p, "l_fingertip");
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_COLLAR) == 0) {
oscFunc(&p, "r_collar");
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_SHOULDER) == 0) {
oscFunc(&p, "r_shoulder");
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_ELBOW) == 0) {
oscFunc(&p, "r_elbow");
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_WRIST) == 0) {
oscFunc(&p, "r_wrist");
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_HAND) == 0) {
oscFunc(&p, "r_hand");
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_FINGERTIP) == 0) {
oscFunc(&p, "r_fingertip");
}
if (jointPos(aUsers[i], XN_SKEL_TORSO) == 0) {
oscFunc(&p, "torso");
}
if (jointPos(aUsers[i], XN_SKEL_WAIST) == 0) {
oscFunc(&p, "waist");
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_HIP) == 0) {
oscFunc(&p, "l_hip");
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_KNEE) == 0) {
oscFunc(&p, "l_knee");
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_ANKLE) == 0) {
oscFunc(&p, "l_ankle");
}
if (jointPos(aUsers[i], XN_SKEL_LEFT_FOOT) == 0) {
oscFunc(&p, "l_foot");
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_HIP) == 0) {
oscFunc(&p, "r_hip");
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_KNEE) == 0) {
oscFunc(&p, "r_knee");
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_ANKLE) == 0) {
oscFunc(&p, "r_ankle");
}
if (jointPos(aUsers[i], XN_SKEL_RIGHT_FOOT) == 0) {
oscFunc(&p, "r_foot");
}
p << osc::EndBundle;
transmitSocket->Send(p.Data(), p.Size());
}
else {
//Send user's center of mass
sendUserPosMsg(aUsers[i]);
}
}
}
