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(); }