void DrawCubeFront(nite::UserTracker* pUserTracker, const nite::UserData& userData)
{
const nite::SkeletonJoint& head = userData.getSkeleton().getJoint(nite::JOINT_HEAD);
const nite::SkeletonJoint& neck = userData.getSkeleton().getJoint(nite::JOINT_NECK);
const nite::Quaternion& headOrientation = head.getOrientation();
const nite::SkeletonJoint& rShoulder = userData.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER);
const nite::SkeletonJoint& lShoulder= userData.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER);
float diff = rShoulder.getPosition().z - (float)lShoulder.getPosition().z;
float coordinates[3] = {0};
pUserTracker->convertJointCoordinatesToDepth(head.getPosition().x, head.getPosition().y, head.getPosition().z, &coordinates[0], &coordinates[1]);
float neckCoordinates[3] = {0};
pUserTracker->convertJointCoordinatesToDepth(neck.getPosition().x, neck.getPosition().y, neck.getPosition().z, &neckCoordinates[0], &neckCoordinates[1]);
coordinates[0] *= GL_WIN_SIZE_X/(float)g_nXRes;
coordinates[1] *= GL_WIN_SIZE_Y/(float)g_nYRes;
neckCoordinates[0] *= GL_WIN_SIZE_X/(float)g_nXRes;
neckCoordinates[1] *= GL_WIN_SIZE_Y/(float)g_nYRes;
float headOffset = 0.75*(coordinates[1]-neckCoordinates[1]);
glPushMatrix();
glTranslatef(coordinates[0], coordinates[1], 0);
float pi = 3.14159;
float theta;
if (diff >= 0) {
theta = 2*pi-2*(float)acos(headOrientation.w);
} else {
theta = 2*(float)acos(headOrientation.w);
}
// printf("%f\n", theta*180/pi);
// printf("%f\n", headOrientation.w);
glRotatef(theta*180/pi, headOrientation.x/(float)sin(theta/2.f), headOrientation.y/(float)sin(theta/2.f), headOrientation.z/(float)sin(theta/2.f));
glBegin(GL_QUADS); // Begin drawing the color cube with 6 quads
// Front face (z = 1.0f)
glColor3f(1.0f, 0.0f, 0.0f); // Red
glVertex3f( headOffset, 2*headOffset, headOffset);
glColor3f(0, 1, 0);
glVertex3f(-headOffset, 2*headOffset, headOffset);
glColor3f(0, 1, 1);
glVertex3f(-headOffset, 0, headOffset);
glColor3f(0, 0, 1);
glVertex3f( headOffset, 0, headOffset);
glEnd(); // End of drawing color-cube
glPopMatrix();
}
void CGraph::JointFun(const nite::UserTracker&usertracker, const nite::UserData&data, const int&mapx, const int&mapy)
{
static const unsigned int total = JOINTDATA_SIZE - 2;
float cood[3 * total];
memset(cood, 0, sizeof(float) * 3 * total);
for (int i = 0; i < total; i++)
{
//if ((i == SHOULDER_ASIDE || i == HEAD_ASIDE)) continue;
x[i] = data.getSkeleton().getJoint(jType[i]).getPosition().x;
y[i] = data.getSkeleton().getJoint(jType[i]).getPosition().y;
z[i] = data.getSkeleton().getJoint(jType[i]).getPosition().z;
}
if (x[0] == 0 || y[0] == 0 || x[1] == 0 || y[1] == 0 || x[2] == 0 || y[2] == 0)
{
istracked = 0;
m_angle = 0;
return;
}
istracked = 1;
/*if (abs(x[LEFT_SHOULDER] - x[RIGHT_SHOULDER]) <= 10)
{
m_playmode = PLAYMODE_DEPTH_SIDE;
}
else
{
m_playmode = PLAYMODE_DEPTH;
}*/
for (int i = 0; i < total; i++)
{
usertracker.convertJointCoordinatesToDepth(x[i], y[i], z[i], cood + 3 * i, cood + 3 * i + 1);
cood[3 * i + 0] *= m_width*1.0 / mapx;
cood[3 * i + 1] *= m_height*1.0 / mapy;
cood[3 * i + 2] *= 0;
}
glColor3f(1, 0.5, 0);
glEnable(GL_CULL_FACE);
glPolygonMode(GL_FRONT, GL_FILL);
glCullFace(GL_BACK);
for (int i = 1; i < total; i++)
{
glPushMatrix();
glLoadIdentity();
glTranslatef(cood[i * 3], cood[i * 3 + 1], cood[i * 3 + 2]);
glutSolidSphere(10, 100, 100);
glPopMatrix();
}
glDisable(GL_CULL_FACE);
};
/**
Gets the coordinates of a given joint and stores it in a User structure
@param [out] user User skeleton data.
@param [in] jointType Name of the joint to get the coordinates.
@param [out] coordX Returns the value of the x-coordinate.
@param [out] coordY Returns the value of the y-coordinate.
@return True if the coordinates were obtained, false otherwise.
*/
int Kinect::getJointCoordinates(const nite::UserData& user, nite::JointType jointType, float &coordX, float &coordY)
{
// Gets the requested joint
const nite::SkeletonJoint& joint = user.getSkeleton().getJoint(jointType);
// If there is enough confidence in the coordinates
if (joint.getPositionConfidence() > 0.5)
{
// Gets the depth frame
depthFrame = userTrackerFrame.getDepthFrame();
// Converts the coordinates from the 'Real World' system to the 'Projective' system
niteRc = userTracker.convertJointCoordinatesToDepth(joint.getPosition().x, joint.getPosition().y, joint.getPosition().z, &coordX, &coordY);
if(niteRc != nite::STATUS_OK)
cout << "ERROR: Coordinates convertion failed." << endl;
// Gets the resolution of the frame
int g_nXRes = depthFrame.getVideoMode().getResolutionX();
int g_nYRes = depthFrame.getVideoMode().getResolutionY();
// Adjusts the coordinates to the window size
coordX *= WIN_SIZE_X/(float)g_nXRes;
coordY *= WIN_SIZE_Y/(float)g_nYRes;
return 1;
}
else
{
return 0;
}
}
void User::updateUserData(const nite::UserData& data)
{
userdata = data;
for (int i = 0; i < NITE_JOINT_COUNT; i++)
{
const nite::SkeletonJoint &o = data.getSkeleton().getJoint((nite::JointType)i);
joints[i].updateJointData(o);
}
stringstream ss;
ss << "[" << data.getId() << "]" << endl;
ss << (data.isVisible() ? "Visible" : "Out of Scene") << endl;;
switch (data.getSkeleton().getState())
{
case nite::SKELETON_NONE:
ss << "Stopped tracking.";
break;
case nite::SKELETON_CALIBRATING:
ss << "Calibrating...";
break;
case nite::SKELETON_TRACKED:
ss << "Tracking!";
break;
case nite::SKELETON_CALIBRATION_ERROR_NOT_IN_POSE:
case nite::SKELETON_CALIBRATION_ERROR_HANDS:
case nite::SKELETON_CALIBRATION_ERROR_LEGS:
case nite::SKELETON_CALIBRATION_ERROR_HEAD:
case nite::SKELETON_CALIBRATION_ERROR_TORSO:
ss << "Calibration Failed... :-|";
break;
}
status_string = ss.str();
const nite::Point3f& pos = userdata.getCenterOfMass();
center_of_mass.set(pos.x, pos.y, -pos.z);
Joint &torso = joints[nite::JOINT_TORSO];
activity += (torso.getPosition().distance(center_of_bone) - activity) * 0.1;
center_of_bone = torso.getPosition();
}
void SaveJoint(nite::UserTracker* pUserTracker, const nite::UserData& userData, nite::JointType jointType) {
nite::Point3f joint = userData.getSkeleton().getJoint(jointType).getPosition();
sideJoints[jointType][0] = joint.x;
sideJoints[jointType][1] = joint.y;
sideJoints[jointType][2] = joint.z;
side2top(sideJoints, topJoints);
pUserTracker->convertJointCoordinatesToDepth(joint.x, joint.y, joint.z, &sideJoints[jointType][3], &sideJoints[jointType][4]);
pUserTracker->convertJointCoordinatesToDepth(topJoints[jointType][0], topJoints[jointType][1], topJoints[jointType][2], &topJoints[jointType][3], &topJoints[jointType][4]);
}
void updateUserState(const nite::UserData& user, uint64_t ts)
{
if (user.isNew())
{
USER_MESSAGE("New");
}
else if (user.isVisible() && !g_visibleUsers[user.getId()])
printf("[%08" PRIu64 "] User #%d:\tVisible\n", ts, user.getId());
else if (!user.isVisible() && g_visibleUsers[user.getId()])
printf("[%08" PRIu64 "] User #%d:\tOut of Scene\n", ts, user.getId());
else if (user.isLost())
{
USER_MESSAGE("Lost");
}
g_visibleUsers[user.getId()] = user.isVisible();
if(g_skeletonStates[user.getId()] != user.getSkeleton().getState())
{
switch(g_skeletonStates[user.getId()] = user.getSkeleton().getState())
{
case nite::SKELETON_NONE:
USER_MESSAGE("Stopped tracking.")
break;
case nite::SKELETON_CALIBRATING:
USER_MESSAGE("Calibrating...")
break;
case nite::SKELETON_TRACKED:
USER_MESSAGE("Tracking!")
break;
case nite::SKELETON_CALIBRATION_ERROR_NOT_IN_POSE:
case nite::SKELETON_CALIBRATION_ERROR_HANDS:
case nite::SKELETON_CALIBRATION_ERROR_LEGS:
case nite::SKELETON_CALIBRATION_ERROR_HEAD:
case nite::SKELETON_CALIBRATION_ERROR_TORSO:
USER_MESSAGE("Calibration Failed... :-|")
break;
}
}
}
void updateUserState(const nite::UserData& user, unsigned long long ts)
{
if (user.isNew())
USER_MESSAGE("New")
else if (user.isVisible() && !g_visibleUsers[user.getId()])
USER_MESSAGE("Visible")
else if (!user.isVisible() && g_visibleUsers[user.getId()])
USER_MESSAGE("Out of Scene")
else if (user.isLost())
USER_MESSAGE("Lost")
g_visibleUsers[user.getId()] = user.isVisible();
if(g_skeletonStates[user.getId()] != user.getSkeleton().getState())
{
switch(g_skeletonStates[user.getId()] = user.getSkeleton().getState())
{
case nite::SKELETON_NONE:
USER_MESSAGE("Stopped tracking.")
break;
case nite::SKELETON_CALIBRATING:
USER_MESSAGE("Calibrating...")
break;
case nite::SKELETON_TRACKED:
USER_MESSAGE("Tracking!")
break;
case nite::SKELETON_CALIBRATION_ERROR_NOT_IN_POSE:
case nite::SKELETON_CALIBRATION_ERROR_HANDS:
case nite::SKELETON_CALIBRATION_ERROR_LEGS:
case nite::SKELETON_CALIBRATION_ERROR_HEAD:
case nite::SKELETON_CALIBRATION_ERROR_TORSO:
USER_MESSAGE("Calibration Failed... :-|")
break;
}
}
}
void DrawHat(nite::UserTracker* pUserTracker, const nite::UserData& userData)
{
const nite::SkeletonJoint& head = userData.getSkeleton().getJoint(nite::JOINT_HEAD);
const nite::SkeletonJoint& neck = userData.getSkeleton().getJoint(nite::JOINT_NECK);
const nite::Quaternion& headOrientation = head.getOrientation();
float coordinates[3] = {0};
pUserTracker->convertJointCoordinatesToDepth(head.getPosition().x, head.getPosition().y, head.getPosition().z, &coordinates[0], &coordinates[1]);
float neckCoordinates[3] = {0};
pUserTracker->convertJointCoordinatesToDepth(neck.getPosition().x, neck.getPosition().y, neck.getPosition().z, &neckCoordinates[0], &neckCoordinates[1]);
coordinates[0] *= GL_WIN_SIZE_X/(float)g_nXRes;
coordinates[1] *= GL_WIN_SIZE_Y/(float)g_nYRes;
neckCoordinates[0] *= GL_WIN_SIZE_X/(float)g_nXRes;
neckCoordinates[1] *= GL_WIN_SIZE_Y/(float)g_nYRes;
float headOffset = 0.5*(coordinates[1]-neckCoordinates[1]);
glPushMatrix();
glTranslatef(coordinates[0], coordinates[1], 0);
float pi = 3.14159;
float theta = 2*pi-2*(float)acos(headOrientation.w);
glRotatef(theta*180/pi, headOrientation.x/(float)sin(theta/2.f), headOrientation.y/(float)sin(theta/2.f), headOrientation.z/(float)sin(theta/2.f));
glBegin(GL_TRIANGLES);
glColor3f(0,0,1);
glVertex2f(-headOffset/1.5, headOffset);
glColor3f(1,0,0);
glVertex2f(headOffset/1.5, headOffset);
glColor3f(0,1,0);
glVertex2f(0, 2.5*headOffset);
glEnd();
glPopMatrix();
}
void DrawSkeleton(nite::UserTracker* pUserTracker, const nite::UserData& userData)
{
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_HEAD), userData.getSkeleton().getJoint(nite::JOINT_NECK), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER), userData.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW), userData.getSkeleton().getJoint(nite::JOINT_LEFT_HAND), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_HAND), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER), userData.getSkeleton().getJoint(nite::JOINT_TORSO), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER), userData.getSkeleton().getJoint(nite::JOINT_TORSO), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_TORSO), userData.getSkeleton().getJoint(nite::JOINT_LEFT_HIP), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_TORSO), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_HIP), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_HIP), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_HIP), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_HIP), userData.getSkeleton().getJoint(nite::JOINT_LEFT_KNEE), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_KNEE), userData.getSkeleton().getJoint(nite::JOINT_LEFT_FOOT), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_RIGHT_HIP), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_KNEE), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_RIGHT_KNEE), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_FOOT), userData.getId() % colorCount);
//sprintf_s(g_generalMessage, "test");
}
void CUserData::UpdateState(const nite::UserData& state)
{
m_userData=state;
m_transformedSkeleton=state.getSkeleton();
}
void DrawSkeleton(nite::UserTracker* pUserTracker, const nite::UserData& userData)
{
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_HEAD), userData.getSkeleton().getJoint(nite::JOINT_NECK), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER), userData.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW), userData.getSkeleton().getJoint(nite::JOINT_LEFT_HAND), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_HAND), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER), userData.getSkeleton().getJoint(nite::JOINT_TORSO), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER), userData.getSkeleton().getJoint(nite::JOINT_TORSO), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_TORSO), userData.getSkeleton().getJoint(nite::JOINT_LEFT_HIP), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_TORSO), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_HIP), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_HIP), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_HIP), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_HIP), userData.getSkeleton().getJoint(nite::JOINT_LEFT_KNEE), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_LEFT_KNEE), userData.getSkeleton().getJoint(nite::JOINT_LEFT_FOOT), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_RIGHT_HIP), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_KNEE), userData.getId() % colorCount);
DrawLimb(pUserTracker, userData.getSkeleton().getJoint(nite::JOINT_RIGHT_KNEE), userData.getSkeleton().getJoint(nite::JOINT_RIGHT_FOOT), userData.getId() % colorCount);
SaveJoint(pUserTracker, userData, nite::JOINT_HEAD);
SaveJoint(pUserTracker, userData, nite::JOINT_NECK);
SaveJoint(pUserTracker, userData, nite::JOINT_LEFT_SHOULDER);
SaveJoint(pUserTracker, userData, nite::JOINT_RIGHT_SHOULDER);
SaveJoint(pUserTracker, userData, nite::JOINT_LEFT_ELBOW);
SaveJoint(pUserTracker, userData, nite::JOINT_RIGHT_ELBOW);
SaveJoint(pUserTracker, userData, nite::JOINT_LEFT_HAND);
SaveJoint(pUserTracker, userData, nite::JOINT_RIGHT_HAND);
SaveJoint(pUserTracker, userData, nite::JOINT_TORSO);
SaveJoint(pUserTracker, userData, nite::JOINT_LEFT_HIP);
SaveJoint(pUserTracker, userData, nite::JOINT_RIGHT_HIP);
SaveJoint(pUserTracker, userData, nite::JOINT_LEFT_KNEE);
SaveJoint(pUserTracker, userData, nite::JOINT_RIGHT_KNEE);
SaveJoint(pUserTracker, userData, nite::JOINT_LEFT_FOOT);
SaveJoint(pUserTracker, userData, nite::JOINT_RIGHT_FOOT);
}
/**
* update and event out on state of user and skeleton tracking
*/
void updateUserState(const nite::UserData& user)
{
if (user.isNew()) {
eventIDToSend = NEW_USER;
async.data = (void*) &eventIDToSend;
uv_async_send(&async);
} else if (!isUserVisible && user.isVisible() && user.getId() == 1) {
isUserVisible = 1;
eventIDToSend = USER_IS_VISIBLE;
async.data = (void*) &eventIDToSend;
uv_async_send(&async);
} else if (isUserVisible && !user.isVisible() && user.getId() == 1) {
isUserVisible = 0;
eventIDToSend = USER_IS_OUT_OF_SCENE;
async.data = (void*) &eventIDToSend;
uv_async_send(&async);
}
g_visibleUsers[user.getId()] = user.isVisible();
if(g_skeletonStates[user.getId()] != user.getSkeleton().getState())
{
switch(g_skeletonStates[user.getId()] = user.getSkeleton().getState())
{
case nite::SKELETON_NONE:
if (isSkeletonTracking) {
isSkeletonTracking = false;
isSkeletonCalibrating = false;
eventIDToSend = SKELETON_STOPPED_TRACKING;
async.data = (void*) &eventIDToSend;
uv_async_send(&async);
}
break;
case nite::SKELETON_CALIBRATING:
if (!isSkeletonCalibrating) {
isSkeletonCalibrating = true;
eventIDToSend = SKELETON_CALIBRATING;
async.data = (void*) &eventIDToSend;
uv_async_send(&async);
}
break;
case nite::SKELETON_TRACKED:
if (!isSkeletonTracking) {
isSkeletonTracking = true;
isSkeletonCalibrating = false;
eventIDToSend = SKELETON_TRACKING;
async.data = (void*) &eventIDToSend;
uv_async_send(&async);
}
break;
case nite::SKELETON_CALIBRATION_ERROR_NOT_IN_POSE:
case nite::SKELETON_CALIBRATION_ERROR_HANDS:
case nite::SKELETON_CALIBRATION_ERROR_LEGS:
case nite::SKELETON_CALIBRATION_ERROR_HEAD:
case nite::SKELETON_CALIBRATION_ERROR_TORSO:
eventIDToSend = SKELETON_CALIBRATION_FAILED;
async.data = (void*) &eventIDToSend;
uv_async_send(&async);
isSkeletonTracking = false;
isSkeletonCalibrating = false;
break;
}
}
}