/*
* Function: getPosition
*
* Returns the Position (enum defined in monitor.h) of the specified user.
* Positions are assigned based on joint locations and defined tolerances.
* Sets the location of the hospital bed if it has not yet been set and the patient
* is lying down.
*
* Parameters:
* XnUserID user - The id for the user whose position to get
*
* Return:
* The Position of the user
*/
Position KinectMonitor::getPosition(XnUserID user) {
XnSkeletonJointPosition headPos, torsoPos, leftPos, rightPos;
double head, torso, center, left, right;
Position position = UNKNOWN;
SkeletonCapability skeleton = userGenerator.GetSkeletonCap();
// Get Joint positions for the specified joint locations
// Joints are XN_SKEL_<option> where <option> is:
// HEAD, NECK, TORSO, WAIST, LEFT_COLLAR, LEFT_SHOULDER, LEFT_ELBOW, LEFT_WRIST, LEFT_HAND
// LEFT_FINGERTIP, RIGHT_COLLAR, RIGHT_SHOULDER, RIGHT_ELBOW, RIGHT_WRIST, RIGHT_HAND,
// RIGHT_FINGERTIP, LEFT_HIP, LEFT_KNEE, LEFT_ANKLE, LEFT_FOOT, RIGHT_HIP, RIGHT_KNEE,
// RIGHT_ANKLE, RIGHT_FOOT
skeleton.GetSkeletonJointPosition(user, XN_SKEL_HEAD, headPos);
skeleton.GetSkeletonJointPosition(user, XN_SKEL_TORSO, torsoPos);
skeleton.GetSkeletonJointPosition(user, XN_SKEL_LEFT_SHOULDER, leftPos);
skeleton.GetSkeletonJointPosition(user, XN_SKEL_RIGHT_SHOULDER, rightPos);
// Get the relevant coordinate for each joint (X, Y, or Z)
head = headPos.position.Z;
torso = torsoPos.position.Z;
center = torsoPos.position.X;
left = leftPos.position.Z;
right = rightPos.position.Z;
// Determine defined positions based on tolerances defined in monitor.h
if(head - LAYING_TOLERANCE > torso) {
position = LAYING;
out = false;
} else if( left < right - TURNED_TOLERANCE ||
left > right + TURNED_TOLERANCE ) {
position = TURNED;
} else {
position = FORWARD;
}
// Set/Check the patient vs the bed location
if( (!bedSet) && position == LAYING ) {
// If the bed location is not yet set, then set the bed at the patients torso X coordinate
bed = center;
bedSet = true;
} else if( bedSet ) {
// The patient is out of bed if they are outside the BED_TOLERANCE defined in monitor.h
out = ( center < bed - BED_TOLERANCE || center > bed + BED_TOLERANCE );
}
return position;
}
std::vector<std::vector<XnPoint3D> > KinectControl::getSkeleton()
{
SkeletonCapability skeleton = user_generator.GetSkeletonCap();
XnUserID user_id[15];
XnUInt16 user_count = sizeof(user_id) / sizeof(user_id[0]);
XnStatus nRetVal = user_generator.GetUsers(user_id, user_count);
CHECK_RC(nRetVal, "Get Users");
// 全ユーザ
std::vector<std::vector<XnPoint3D> > skeletons;
for(int i = 0; i < user_count; ++i)
{
// i番目のユーザがトラッキングされていたら
if(skeleton.IsTracking(user_id[i]))
{
// 個別ユーザ
std::vector<XnPoint3D> user_skeleton;
// 関節は全てで24個
XnSkeletonJointPosition joint[24];
// [0]はCenter of Mass
XnPoint3D pt[25];
for(int j = 1; j < 25; ++j)
{
skeleton.GetSkeletonJointPosition(user_id[i], (XnSkeletonJoint)(j), joint[j - 1]);
pt[j] = joint[j - 1].position;
}
user_generator.GetCoM(user_id[i], pt[0]);
depth_generator.ConvertRealWorldToProjective(25, pt, pt);
for(int j = 0; j < 25; ++j)
user_skeleton.push_back(pt[j]);
skeletons.push_back(user_skeleton);
}
}
return skeletons;
}
/**
* Load actual joint positions.
*
* Load the position of the joints in the joint array.
*
* @param player is the player ID who needs to load the joints
* from.
* @param skelCap is the skeletonCapability taken from the User
* Generator.
*/
void NeutralModel :: loadJoints (XnUserID player, SkeletonCapability skelCap)
{
// Skeleton Joint.
XnSkeletonJointPosition jointPos;
if (skelCap.IsTracking(player)) {
// Get skeleton jointPos positions.
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_HEAD,
jointPos
);
joint[HEAD] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_NECK,
jointPos
);
joint[NECK] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_LEFT_SHOULDER,
jointPos
);
joint[LSHOULDER] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_RIGHT_SHOULDER,
jointPos
);
joint[RSHOULDER] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_TORSO,
jointPos
);
joint[TORSO] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_LEFT_ELBOW,
jointPos
);
joint[LELBOW] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_RIGHT_ELBOW,
jointPos
);
joint[RELBOW] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_LEFT_HAND,
jointPos
);
joint[LHAND] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_RIGHT_HAND,
jointPos
);
joint[RHAND] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_LEFT_HIP,
jointPos
);
joint[LHIP] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_RIGHT_HIP,
jointPos
);
joint[RHIP] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_LEFT_KNEE,
jointPos
);
joint[LKNEE] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_RIGHT_KNEE,
jointPos
);
joint[RKNEE] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_LEFT_FOOT,
jointPos
);
joint[LFOOT] = jointPos.position;
skelCap.GetSkeletonJointPosition(
player,
XN_SKEL_RIGHT_FOOT,
jointPos
);
joint[RFOOT] = jointPos.position;
nm_DepthGenerator.ConvertRealWorldToProjective(15, joint, joint);
}
}
/**
* DrawNeutral function.
*
* This function draws the stick figure depending on the
* transforming stage of the player or draws a diamond if the
* player is not tracked.
*
* @param player is the ID of the player.
*
*/
void NeutralModel :: drawNeutral (XnUserID player)
{
// UserGenerator.
UserGenerator userGen;
// Color of the stick figure.
XnFloat color[3];
// Material properties.
GLfloat mat_specular[] = { 0.3, 0.3, 0.3, 0.3 };
GLfloat mat_shininess[] = { 10.0 };
// Center of mass.
XnPoint3D com;
// Player's stage.
int stage;
float ax;
Vector3D a;
Vector3D b;
Vector3D c;
Vector3D u;
Vector3D v;
Vector3D w;
GLuint mode;
// Select the players color according his ID.
color[0] = Colors[player % nColors][0];
color[1] = Colors[player % nColors][1];
color[2] = Colors[player % nColors][2];
// Set the material for the stick figure.
GLfloat materialColor[] = {color[0], color[1], color[2], 1.0f};
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, materialColor);
glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
userGen = nm_UserDetector -> retUserGenerator();
SkeletonCapability skelCap = userGen.GetSkeletonCap();
mode = GLM_SMOOTH | GLM_MATERIAL;
a = Vector3D(joint[LSHOULDER]);
b = Vector3D(joint[RSHOULDER]);
c = Vector3D(joint[RHIP]);
u = b - a;
v = c - a;
w = u.cross(v);
w.y = 0.0;
w.normalize();
ax = 57.2957795 * acos(w.z);
if (w.x <= 0.0) {
ax = -ax;
}
// Init the drawing process.
// Draws a stick figure if player is been tracked.
if (skelCap.IsTracking(player)) {
// Get player's stage.
stage = nm_UserDetector -> retStage(player);
loadJoints(player, skelCap);
// Drawing legs.
if ((stage >= 0) && (stage < 4)) {
// Left leg.
glPushMatrix();
orientMatrix(joint[RHIP],joint[RKNEE]);
glTranslatef( 0.0, 0.0, 50.0);
glScalef(250.0, 250.0, 250.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(ax, 0.0,-1.0, 0.0);
glmDraw(zamusModelParts.thigh, mode);
glPopMatrix();
glPushMatrix();
orientMatrix(joint[RKNEE],joint[RFOOT]);
glTranslatef( 0.0, 0.0, 50.0);
glScalef(100.0, 100.0, 100.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(ax, 0.0,-1.0, 0.0);
glmDraw(zamusModelParts.leg, mode);
glPopMatrix();
// Right leg.
glPushMatrix();
orientMatrix(joint[LHIP], joint[LKNEE]);
glTranslatef( 0.0, 0.0, 50.0);
glScalef(-250.0, 250.0, 250.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(-ax, 0.0,-1.0, 0.0);
glmDraw(zamusModelParts.thigh, mode);
glPopMatrix();
glPushMatrix();
orientMatrix(joint[LKNEE], joint[LFOOT]);
glTranslatef( 0.0, 0.0, 50.0);
glScalef(-100.0, 100.0, 100.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(-ax, 0.0,-1.0, 0.0);
glmDraw(zamusModelParts.leg, mode);
glPopMatrix();
// Foots.
glPushMatrix();
glTranslatef( joint[LFOOT].X, joint[LFOOT].Y, joint[LFOOT].Z);
glScalef(40.0,-40.0,-40.0);
glRotatef(ax, 0.0,-1.0, 0.0);
glTranslatef(0.0,-0.25, 0.5);
glmDraw(zamusModelParts.foot, mode);
glPopMatrix();
glPushMatrix();
glTranslatef( joint[RFOOT].X, joint[RFOOT].Y, joint[RFOOT].Z);
glScalef(-40.0,-40.0,-40.0);
glRotatef(-ax, 0.0,-1.0, 0.0);
glTranslatef(0.0,-0.25, 0.5);
glmDraw(zamusModelParts.foot, mode);
glPopMatrix();
}
else {
drawLimp(joint[LKNEE],joint[LFOOT]);
drawLimp(joint[RKNEE],joint[RFOOT]);
drawLimp(joint[LHIP],joint[LKNEE]);
drawLimp(joint[RHIP],joint[RKNEE]);
}
// Draw torso.
if ((stage > 0) && (stage < 4)) {
glPushMatrix();
orientMatrix(joint[NECK],joint[TORSO]);
glScalef(400.0, 400.0, 400.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(ax, 0.0,-1.0, 0.0);
glTranslatef(0.0, -0.1, 0.0);
glmDraw(zamusModelParts.chest, mode);
glPopMatrix();
}
else {
drawTorso();
}
// Drawing arms.
if (stage == 2) {
// Shoulders
glPushMatrix();
glTranslatef(joint[LSHOULDER].X,
joint[LSHOULDER].Y,
joint[LSHOULDER].Z);
glRotatef(-ax, 0.0,-2.0, 0.0);
glTranslatef(-30.0,-40.0, 0.0);
glScalef(500.0,-500.0, 500.0);
glmDraw(zamusModelParts.shoulder, mode);
glPopMatrix();
glPushMatrix();
glTranslatef(joint[RSHOULDER].X,
joint[RSHOULDER].Y,
joint[RSHOULDER].Z);
glRotatef(-ax, 0.0,-1.0, 0.0);
glTranslatef( 30.0,-40.0, 0.0);
glScalef(500.0,-500.0, 500.0);
glmDraw(zamusModelParts.shoulder, mode);
glPopMatrix();
// LeftArm
glPushMatrix();
orientMatrix(joint[RSHOULDER], joint[RELBOW]);
glTranslatef( 0.0, 0.0, 50.0);
glScalef(500.0, 500.0, 500.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(ax, 0.0,-1.0, 0.0);
glmDraw(zamusModelParts.arm, mode);
glPopMatrix();
glPushMatrix();
orientMatrix(joint[RELBOW], joint[RHAND]);
glTranslatef( 0.0, 0.0, 60.0);
glScalef(250.0, 250.0, 250.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(-ax, 0.0,-1.0, 0.0);
glmDraw(zamusModelParts.forearm, mode);
glPopMatrix();
glPushMatrix();
orientMatrix(joint[LSHOULDER], joint[LELBOW]);
glTranslatef( 0.0, 0.0, 50.0);
glScalef(500.0, 500.0, 500.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(ax, 0.0,-1.0, 0.0);
glmDraw(zamusModelParts.arm, mode);
glPopMatrix();
glPushMatrix();
orientMatrix(joint[LELBOW],joint[LHAND]);
glTranslatef( 0.0, 0.0, 80.0);
glScalef(300.0, 300.0, 300.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(-ax, 0.0,-1.0, 0.0);
glmDraw(zamusModelParts.cannon, mode);
glPopMatrix();
}
else if ((stage == 4) || (stage == 5)) {
glPushMatrix();
glTranslatef(joint[LSHOULDER].X,
joint[LSHOULDER].Y,
joint[LSHOULDER].Z);
glRotatef(-ax, 0.0,-2.0, 0.0);
glScalef(500.0,-500.0, 500.0);
glmDraw(linqModelParts.shoulder, mode);
glPopMatrix();
glPushMatrix();
orientMatrix(joint[LSHOULDER],joint[LELBOW]);
glTranslatef( 0.0, 0.0, 50.0);
glScalef(-180.0, 180.0, 180.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(-ax, 0.0,-1.0, 0.0);
glmDraw(linqModelParts.thigh, mode);
glPopMatrix();
glPushMatrix();
orientMatrix(joint[LELBOW],joint[LHAND]);
glTranslatef( 0.0, 0.0, 50.0);
glScalef(-80.0, 80.0, 80.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(-ax, 0.0,-1.0, 0.0);
glmDraw(linqModelParts.leg, mode);
glPopMatrix();
}
else {
drawLimp(joint[LSHOULDER],joint[LELBOW]);
drawLimp(joint[LELBOW],joint[LHAND]);
}
if (stage == 5) {
glPushMatrix();
glTranslatef(joint[RSHOULDER].X,
joint[RSHOULDER].Y,
joint[RSHOULDER].Z);
glRotatef(-ax, 0.0,-1.0, 0.0);
glScalef(500.0,-500.0, 500.0);
glmDraw(linqModelParts.shoulder, mode);
glPopMatrix();
glPushMatrix();
orientMatrix(joint[RSHOULDER],joint[RELBOW]);
glTranslatef( 0.0, 0.0, 50.0);
glScalef(350.0, 350.0, 350.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(ax, 0.0,-1.0, 0.0);
glmDraw(linqModelParts.arm, mode);
glPopMatrix();
glPushMatrix();
orientMatrix(joint[RELBOW],joint[RHAND]);
glTranslatef( 0.0, 0.0, 60.0);
glScalef(250.0, 250.0, 250.0);
glRotatef(90, -1.0, 0.0, 0.0);
glRotatef(-ax, 0.0,-1.0, 0.0);
glmDraw(linqModelParts.forearm, mode);
glPopMatrix();
}
else {
drawLimp(joint[RSHOULDER],joint[RELBOW]);
drawLimp(joint[RELBOW],joint[RHAND]);
}
// Draw head.
drawHead();
}
// Draws a diamond in the player's center of mass.
else {
userGen.GetCoM(player, com);
nm_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
glPushMatrix();
glTranslatef(com.X, com.Y, com.Z);
glutSolidSphere(60.0, 4, 2);
glPopMatrix();
}
}