void EntityController_Rotator::onMouseMove(float x, float y)
{
if (m_pickedEntity != NULL) {
Vector3 startVec = screenPoint2SpherePoint(m_oldX, m_oldY);
Vector3 endVec = screenPoint2SpherePoint(x, y);
assert(startVec.isUnit());
assert(endVec.isUnit());
Vector3 axis = startVec.crossProduct(endVec);
float angle = radian2Degree(asin(axis.length()));
axis = axis.normalize();
const Camera *camera = m_sceneMgr->getCamera();
Matrix4x4 viewMat = camera->getViewMatrix();
Matrix4x4 mat = m_oldEntWorldMat * viewMat;
Vector4 trans(mat[3]);
mat *=
Matrix4x4::fromTranslate(-trans.x, -trans.y, -trans.z) *
Matrix4x4::fromRotateAxis(axis, angle) *
Matrix4x4::fromTranslate(trans.x, trans.y, trans.z) *
camera->getSceneNode()->getWorldMatrix();
m_pickedEntity->getSceneNode()->setWorldMatrix(mat);
m_sceneMgr->notifyCameraSpaceChanged();
}
}
void SampleViewer::Display()
{
// namespace bg = boost::geometry;
nite::UserTrackerFrameRef userTrackerFrame;
openni::VideoFrameRef depthFrame;
nite::Status rc = m_pUserTracker->readFrame(&userTrackerFrame);
if (rc != nite::STATUS_OK)
{
printf("GetNextData failed\n");
return;
}
depthFrame = userTrackerFrame.getDepthFrame();
if (m_pTexMap == NULL)
{
// Texture map init
m_nTexMapX = MIN_CHUNKS_SIZE(depthFrame.getVideoMode().getResolutionX(), TEXTURE_SIZE);
m_nTexMapY = MIN_CHUNKS_SIZE(depthFrame.getVideoMode().getResolutionY(), TEXTURE_SIZE);
m_pTexMap = new openni::RGB888Pixel[m_nTexMapX * m_nTexMapY];
}
const nite::UserMap& userLabels = userTrackerFrame.getUserMap();
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -1.0, 1.0);
if (depthFrame.isValid() && g_drawDepth)
{
calculateHistogram(m_pDepthHist, MAX_DEPTH, depthFrame);
}
memset(m_pTexMap, 0, m_nTexMapX*m_nTexMapY*sizeof(openni::RGB888Pixel));
float factor[3] = {1, 1, 1};
// check if we need to draw depth frame to texture
if (depthFrame.isValid() && g_drawDepth)
{
const nite::UserId* pLabels = userLabels.getPixels();
const openni::DepthPixel* pDepthRow = (const openni::DepthPixel*)depthFrame.getData();
openni::RGB888Pixel* pTexRow = m_pTexMap + depthFrame.getCropOriginY() * m_nTexMapX;
int rowSize = depthFrame.getStrideInBytes() / sizeof(openni::DepthPixel);
for (int y = 0; y < depthFrame.getHeight(); ++y)
{
const openni::DepthPixel* pDepth = pDepthRow;
openni::RGB888Pixel* pTex = pTexRow + depthFrame.getCropOriginX();
for (int x = 0; x < depthFrame.getWidth(); ++x, ++pDepth, ++pTex, ++pLabels)
{
if (*pDepth != 0)
{
if (*pLabels == 0)
{
if (!g_drawBackground)
{
factor[0] = factor[1] = factor[2] = 0;
}
else
{
factor[0] = Colors[colorCount][0];
factor[1] = Colors[colorCount][1];
factor[2] = Colors[colorCount][2];
}
}
else
{
factor[0] = Colors[*pLabels % colorCount][0];
factor[1] = Colors[*pLabels % colorCount][1];
factor[2] = Colors[*pLabels % colorCount][2];
}
// // Add debug lines - every 10cm
// else if ((*pDepth / 10) % 10 == 0)
// {
// factor[0] = factor[2] = 0;
// }
int nHistValue = m_pDepthHist[*pDepth];
pTex->r = nHistValue*factor[0];
pTex->g = nHistValue*factor[1];
pTex->b = nHistValue*factor[2];
factor[0] = factor[1] = factor[2] = 1;
}
}
pDepthRow += rowSize;
pTexRow += m_nTexMapX;
}
}
glTexParameteri(GL_TEXTURE_2D, GL_GENERATE_MIPMAP_SGIS, GL_TRUE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, m_nTexMapX, m_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, m_pTexMap);
// Display the OpenGL texture map
glColor4f(1,1,1,1);
glEnable(GL_TEXTURE_2D);
glBegin(GL_QUADS);
g_nXRes = depthFrame.getVideoMode().getResolutionX();
g_nYRes = depthFrame.getVideoMode().getResolutionY();
// upper left
glTexCoord2f(0, 0);
glVertex2f(0, 0);
// upper right
glTexCoord2f((float)g_nXRes/(float)m_nTexMapX, 0);
glVertex2f(GL_WIN_SIZE_X, 0);
// bottom right
glTexCoord2f((float)g_nXRes/(float)m_nTexMapX, (float)g_nYRes/(float)m_nTexMapY);
glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
// bottom left
glTexCoord2f(0, (float)g_nYRes/(float)m_nTexMapY);
glVertex2f(0, GL_WIN_SIZE_Y);
glEnd();
glDisable(GL_TEXTURE_2D);
const nite::Array<nite::UserData>& users = userTrackerFrame.getUsers();
// 11.16.15 remove for loop for tracking only one user
// for (int i = 0; i < users.getSize(); ++i)
// {
//const nite::UserData& user = users[i];
if (users.getSize() > 0) {
const nite::UserData& user = users[0];
updateUserState(user, userTrackerFrame.getTimestamp());
if (user.isNew())
{
m_pUserTracker->startSkeletonTracking(user.getId());
m_pUserTracker->startPoseDetection(user.getId(), nite::POSE_CROSSED_HANDS);
}
else if (!user.isLost())
{
if (g_drawStatusLabel)
{
DrawStatusLabel(m_pUserTracker, user);
}
if (g_drawCenterOfMass)
{
DrawCenterOfMass(m_pUserTracker, user);
}
if (g_drawBoundingBox)
{
DrawBoundingBox(user);
}
if (user.getSkeleton().getState() == nite::SKELETON_TRACKED && g_drawSkeleton)
{
DrawSkeleton(m_pUserTracker, user);
}
}
if (m_poseUser == 0 || m_poseUser == user.getId())
{
const nite::PoseData& pose = user.getPose(nite::POSE_CROSSED_HANDS);
if (pose.isEntered())
{
// Start timer
sprintf(g_generalMessage, "In exit pose. Keep it for %d second%s to exit\n", g_poseTimeoutToExit/1000, g_poseTimeoutToExit/1000 == 1 ? "" : "s");
printf("Counting down %d second to exit\n", g_poseTimeoutToExit/1000);
m_poseUser = user.getId();
m_poseTime = userTrackerFrame.getTimestamp();
}
else if (pose.isExited())
{
memset(g_generalMessage, 0, sizeof(g_generalMessage));
printf("Count-down interrupted\n");
m_poseTime = 0;
m_poseUser = 0;
}
else if (pose.isHeld())
{
// tick
if (userTrackerFrame.getTimestamp() - m_poseTime > g_poseTimeoutToExit * 1000)
{
printf("Count down complete. Exit...\n");
Finalize();
exit(2);
}
}
} //user
char buffer[80] = "";
// ---------------- calculate right shoulder 2 DOF ----------------------
int rightShoulderX, rightShoulderY, rightShoulderZ;
rightShoulderX = user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().x -
user.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER).getPosition().x;
rightShoulderY = user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().y -
user.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER).getPosition().y;
rightShoulderZ = user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().z -
user.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER).getPosition().z;
Spherical rightShoulderSpherical;
rightShoulderSpherical = Cartesian2Spherical(rightShoulderX, rightShoulderY, rightShoulderZ);
int rightShoulderPhi, rightShoulderTheta;
rightShoulderTheta = radian2Degree(rightShoulderSpherical.radianTheta, rightShoulderThetaInit); // horizontal rise it's -60 degree
rightShoulderPhi = radian2Degree(rightShoulderSpherical.radianPhi, rightShoulderPitchInit); // when hand's down, it's 70 degree
int rightShoulderYawNow = rightShoulderPhi * sin(rightShoulderTheta * PI / 180);
int rightShoulderPitchNow = rightShoulderPhi * cos(rightShoulderTheta * PI / 180);
sprintf(buffer,"(rightShoulderTheta=%d, rightShoulderYaw=%d, rightShoulderPitch=%d)", rightShoulderTheta, rightShoulderYawNow, rightShoulderPitchNow);
glColor3f(1.0f, 0.0f, 0.0f);
glRasterPos2i(20, 20);
glPrintString(GLUT_BITMAP_HELVETICA_18, buffer);
// ------------------- calculate right elbow 2 DOF -----------------------
int rightElbowX, rightElbowY, rightElbowZ;
rightElbowX = user.getSkeleton().getJoint(nite::JOINT_RIGHT_HAND).getPosition().x -
user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().x;
rightElbowY = user.getSkeleton().getJoint(nite::JOINT_RIGHT_HAND).getPosition().y -
user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().y;
rightElbowZ = user.getSkeleton().getJoint(nite::JOINT_RIGHT_HAND).getPosition().z -
user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW).getPosition().z;
Spherical rightElbowSpherical;
rightElbowSpherical = Cartesian2Spherical(rightElbowX, rightElbowY, rightElbowZ);
int rightElbowThetaNow = - radian2Degree(rightElbowSpherical.radianTheta, rightElbowThetaInit); // reverse for system
int rightElbowYawNow = radian2Degree(rightElbowSpherical.radianPhi, rightElbowYawInit);
sprintf(buffer,"(rightElbowThetaNow=%d, rightElbowYawNow=%d)", rightElbowThetaNow, rightElbowYawNow);
glColor3f(1.0f, 0.0f, 0.0f);
glRasterPos2i(20, 60);
glPrintString(GLUT_BITMAP_HELVETICA_18, buffer);
// ---------------- calculate left shoulder 2 DOF ----------------------
int leftShoulderX, leftShoulderY, leftShoulderZ;
leftShoulderX = user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().x -
user.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER).getPosition().x;
leftShoulderY = user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().y -
user.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER).getPosition().y;
leftShoulderZ = user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().z -
user.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER).getPosition().z;
Spherical leftShoulderSpherical;
leftShoulderSpherical = Cartesian2Spherical(leftShoulderX, leftShoulderY, leftShoulderZ);
int leftShoulderPhi, leftShoulderTheta;
leftShoulderTheta = radian2Degree(leftShoulderSpherical.radianTheta, leftShoulderThetaInit); // horizontal rise it's -60 degree
leftShoulderPhi = radian2Degree(leftShoulderSpherical.radianPhi, leftShoulderPitchInit); // when hand's down, it's 70 degree
// need to reverse in left side
int leftShoulderYawNow = - leftShoulderPhi * sin(leftShoulderTheta * PI / 180);
int leftShoulderPitchNow = - leftShoulderPhi * cos(leftShoulderTheta * PI / 180);
sprintf(buffer,"(leftShoulderTheta=%d, leftShoulderYaw=%d, leftShoulderPitch=%d)", leftShoulderTheta, leftShoulderYawNow, leftShoulderPitchNow);
glColor3f(1.0f, 0.0f, 0.0f);
glRasterPos2i(20, 100);
glPrintString(GLUT_BITMAP_HELVETICA_18, buffer);
// ------------------- calculate left elbow 2 DOF -----------------------
int leftElbowX, leftElbowY, leftElbowZ;
leftElbowX = user.getSkeleton().getJoint(nite::JOINT_LEFT_HAND).getPosition().x -
user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().x;
leftElbowY = user.getSkeleton().getJoint(nite::JOINT_LEFT_HAND).getPosition().y -
user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().y;
leftElbowZ = user.getSkeleton().getJoint(nite::JOINT_LEFT_HAND).getPosition().z -
user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().z;
Spherical leftElbowSpherical;
leftElbowSpherical = Cartesian2Spherical(leftElbowX, leftElbowY, leftElbowZ);
int leftElbowThetaNow = radian2Degree(leftElbowSpherical.radianTheta, leftElbowThetaInit);
int leftElbowYawNow = radian2Degree(leftElbowSpherical.radianPhi, leftElbowYawInit);
sprintf(buffer,"(leftElbowTheta=%d, leftElbowYawNow=%d)", leftElbowThetaNow, leftElbowYawNow);
glColor3f(1.0f, 0.0f, 0.0f);
glRasterPos2i(20, 140);
glPrintString(GLUT_BITMAP_HELVETICA_18, buffer);
// ---------------- constraint movement and publish message ------------
int rightShoulderYawDiff = abs(rightShoulderYaw - rightShoulderYawNow);
int rightShoulderPitchDiff = abs(rightShoulderPitch - rightShoulderPitchNow);
int rightElbowThetaDiff = abs(rightElbowTheta - rightElbowThetaNow);
int rightElbowYawDiff = abs(rightElbowYaw - rightElbowYawNow);
int leftShoulderYawDiff = abs(leftShoulderYaw - leftShoulderYawNow);
int leftShoulderPitchDiff = abs(leftShoulderPitch - leftShoulderPitchNow);
int leftElbowThetaDiff = abs(leftElbowTheta - leftElbowThetaNow);
int leftElbowYawDiff = abs(leftElbowYaw - leftElbowYawNow);
if ((rightShoulderYawDiff < moveLimitDegree) && (rightShoulderPitchDiff < moveLimitDegree) && (rightElbowThetaDiff < moveLimitDegree) && (rightElbowYawDiff < moveLimitDegree) && (rightShoulderTheta >= 0)) {
// in range then refresh robot angle
rightShoulderYaw = rightShoulderYawNow;
rightShoulderPitch = rightShoulderPitchNow;
rightElbowTheta = rightElbowThetaNow;
rightElbowYaw = rightElbowYawNow;
// change the angle to 0 to 360 and publish
rightShoulderYawPub = angleHandler(rightShoulderYaw);
rightShoulderPitchPub = angleHandler(rightShoulderPitch);
rightElbowThetaPub = angleHandler(rightElbowTheta);
rightElbowYawPub = angleHandler(rightElbowYaw);
sprintf(buffer,"tracking!");
glColor3f(0.0f, 0.0f, 1.0f);
glRasterPos2i(20, 180);
glPrintString(GLUT_BITMAP_HELVETICA_18, buffer);
actionPublish(rightShoulderYawPub, rightShoulderPitchPub, rightElbowThetaPub, rightElbowYawPub, leftShoulderYawPub, leftShoulderPitchPub, leftElbowThetaPub, leftElbowYawPub);
}
else {
sprintf(buffer,"soulder yaw: %d, soulder pitch: %d, elbow yaw: %d, rightShoulderTheta > 0: %d", rightShoulderYaw - rightShoulderYawNow, rightShoulderPitch - rightShoulderPitchNow, rightElbowYaw - rightElbowYawNow, rightShoulderTheta);
glColor3f(1.0f, 0.0f, 0.5f);
glRasterPos2i(20, 180);
glPrintString(GLUT_BITMAP_HELVETICA_18, buffer);
}
if((leftShoulderYawDiff < moveLimitDegree) && (leftShoulderPitchDiff < moveLimitDegree) && (leftElbowThetaDiff < moveLimitDegree) && (leftElbowYawDiff < moveLimitDegree) && (leftShoulderTheta >= 0)) {
leftShoulderYaw = leftShoulderYawNow;
leftShoulderPitch = leftShoulderPitchNow;
leftElbowTheta = leftElbowThetaNow;
leftElbowYaw = leftElbowYawNow;
leftShoulderYawPub = angleHandler(leftShoulderYaw);
leftShoulderPitchPub = angleHandler(leftShoulderPitch);
leftElbowThetaPub = angleHandler(leftElbowTheta);
leftElbowYawPub = angleHandler(leftElbowYaw);
sprintf(buffer,"tracking!");
glColor3f(0.0f, 0.3f, 1.0f);
glRasterPos2i(20, 220);
glPrintString(GLUT_BITMAP_HELVETICA_18, buffer);
actionPublish(rightShoulderYawPub, rightShoulderPitchPub, rightElbowThetaPub, rightElbowYawPub, leftShoulderYawPub, leftShoulderPitchPub, leftElbowThetaPub, leftElbowYawPub);
}
else {
sprintf(buffer,"soulder yaw: %d, soulder pitch: %d, elbow yaw: %d, leftShoulderTheta > 0: %d", leftShoulderYaw - leftShoulderYawNow, leftShoulderPitch - leftShoulderPitchNow, leftElbowYaw - leftElbowYawNow, leftShoulderTheta);
glColor3f(0.7f, 0.8f, 2.0f);
glRasterPos2i(20, 220);
glPrintString(GLUT_BITMAP_HELVETICA_18, buffer);
}
}
if (g_drawFrameId)
{
DrawFrameId(userTrackerFrame.getFrameIndex());
}
if (g_generalMessage[0] != '\0')
{
char *msg = g_generalMessage;
glColor3f(1.0f, 0.0f, 0.0f);
glRasterPos2i(100, 20);
glPrintString(GLUT_BITMAP_HELVETICA_18, msg);
}
// Swap the OpenGL display buffers
glutSwapBuffers();
}
