XnVector3D OpenNIUser::GetForwardVector()
{
if( mId )
{
UserGenerator* userGen = _device->getUserGenerator();
// OpenNI's orientation does not work very well.
/*
XnSkeletonJointTransformation t;
m_userGen->GetSkeletonCap().GetSkeletonJoint(userID, XN_SKEL_TORSO, t);
float* e = t.orientation.orientation.elements;
return XV3(e[6], e[7], -e[8]);
*/
XnSkeletonJointPosition p0, p1, p2;
userGen->GetSkeletonCap().GetSkeletonJointPosition( mId, XN_SKEL_RIGHT_SHOULDER, p0 );
userGen->GetSkeletonCap().GetSkeletonJointPosition( mId, XN_SKEL_TORSO, p1 );
userGen->GetSkeletonCap().GetSkeletonJointPosition( mId, XN_SKEL_LEFT_SHOULDER, p2 );
XnVector3D v0(p0.position), v1(p1.position), v2(p2.position);
XnVector3D res1, res2;
res1 = sub( v1, v0 );
res2 = sub( v2, v0 );
XnVector3D res = cross( res1, res2 );
res = normalize( res );
}
return XnVector3D();
}
void getJointPosition(XnUserID player, XnSkeletonJoint eJoint1, unsigned char * dest)
{
if (!_userGenerator.GetSkeletonCap().IsTracking(player))
{
printf("not tracked!\n");
return;
}
XnSkeletonJointPosition joint1;
_userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
if (joint1.fConfidence < 0.5)
{
return;
}
XnPoint3D pt[1];
pt[0] = joint1.position;
_depth.ConvertRealWorldToProjective(1, pt, pt);
float _x, _y, _z;
_x = pt[0].X;
_y = pt[0].Y;
_z = pt[0].Z;
memcpy(dest, &_x, 4);
memcpy(dest+4, &_y, 4);
memcpy(dest+8, &_z, 4);
}
// Callback: Finished calibration
void XN_CALLBACK_TYPE UserCalibration_CalibrationComplete(xn::SkeletonCapability& capability, XnUserID nId, XnCalibrationStatus eStatus, void* pCookie)
{
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
if (eStatus == XN_CALIBRATION_STATUS_OK)
{
// Calibration succeeded
printf("%d Calibration complete, start tracking user %d\n", epochTime, nId);
g_UserGenerator.GetSkeletonCap().StartTracking(nId);
}
else
{
// Calibration failed
printf("%d Calibration failed for user %d\n", epochTime, nId);
/*if(eStatus==XN_CALIBRATION_STATUS_MANUAL_ABORT)
{
printf("Manual abort occured, stop attempting to calibrate!");
return;
}*/
if (g_bNeedPose)
{
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
}
else
{
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
}
// Callback: Detected a pose
void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, const XnChar* strPose, XnUserID nId, void* pCookie)
{
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
printf("%d Pose %s detected for user %d\n", epochTime, strPose, nId);
g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
// Detect if hands are forming "Ready Pose"
static bool isReadyPose(){
XnSkeletonJointPosition torso, leftHip, rightHip;
g_user.GetSkeletonCap().GetSkeletonJointPosition(g_userID, XN_SKEL_TORSO, torso);
g_user.GetSkeletonCap().GetSkeletonJointPosition(g_userID, XN_SKEL_LEFT_HIP, leftHip);
g_user.GetSkeletonCap().GetSkeletonJointPosition(g_userID, XN_SKEL_RIGHT_HIP, rightHip);
// Return if hands are within hips ad torso
return IS_BETWEEN(leftHandPosition, leftHip, torso, Y) && IS_BETWEEN(rightHandPosition, rightHip, torso, Y)
&& IS_BETWEEN(leftHandPosition, leftHip, rightHip, X) && IS_BETWEEN(rightHandPosition, leftHip, rightHip, X);
}
/*
* Function: loadCalibration
*
* Loads a skeletal calibration from a standard file.
* This means that a new user does not need to hold a pose to be tracked.
* The CALIBRATION_FILE macro is defined in monitor.h.
*
* Parameters:
* XnUserID user - The reference to the new user to be calibrated.
*/
void loadCalibration(XnUserID user) {
if( userGenerator.GetSkeletonCap().IsCalibrated(user) ) return;
// Load file
XnStatus status = userGenerator.GetSkeletonCap().
LoadCalibrationDataFromFile(user, CALIBRATION_FILE);
// Start tracking
if( status == XN_STATUS_OK )
userGenerator.GetSkeletonCap().StartTracking(user);
}
void XN_CALLBACK_TYPE UserTracker::UserPose_PoseDetected(xn::PoseDetectionCapability& capability, const XnChar* strPose, XnUserID nId, void* pCookie)
{
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
//printf("%d Pose %s detected for user %d\n", epochTime, strPose, nId);
UserGenerator *userGenerator = static_cast<xn::UserGenerator*>(pCookie);
if(userGenerator)
{
userGenerator->GetPoseDetectionCap().StopPoseDetection(nId);
userGenerator->GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
/**
* Draw the scores over the users in the game (openGL).
*/
void SuperFiremanBrothers :: drawGameInfo ()
{
float y;
int score;
char strLabel[20] = "";
char strLevel[20] = "";
char strStart[20] = "Calibrate to begin";
char strEnd[20] = "Game Over";
UserGenerator uGen;
DepthGenerator dGen;
XnUserID player;
XnPoint3D com;
map <XnUserID, int> :: iterator iter;
float amb[3] = {1.0, 1.0, 1.0};
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, amb);
glDisable(GL_LIGHTING);
y = -768;
uGen = userDetector -> retUserGenerator();
dGen = userDetector -> retDepthGenerator();
for (iter = players.begin(); iter != players.end(); iter++) {
player = iter -> first;
score = iter -> second;
sprintf(strLabel, "Score: %d", score);
uGen.GetCoM(player, com);
dGen.ConvertRealWorldToProjective(1, &com, &com);
glRasterPos3f( com.X + 100, com.Y - 300, com.Z);
glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
y += 150;
}
sprintf(strLevel, "Level %d", level);
glRasterPos2f( 500, -768);
glPrintString(GLUT_BITMAP_HELVETICA_18, strLevel);
if (gameStatus == NOT_STARTED) {
glRasterPos2f( 500, 0);
glPrintString(GLUT_BITMAP_HELVETICA_18, strStart);
}
else if (gameStatus > STARTED) {
glRasterPos2f( 500, 0);
glPrintString(GLUT_BITMAP_HELVETICA_18, strEnd);
}
glEnable(GL_LIGHTING);
}
XnStatus prepare(char useScene, char useDepth, char useHistogram)
{
//TODO handle possible failures!
if (useDepth)
{
mDepthGen.GetMetaData(depthMD);
nXRes = depthMD.XRes();
nYRes = depthMD.YRes();
pDepth = depthMD.Data();
if (useHistogram)
{
calcHist();
// rewind the pointer
pDepth = depthMD.Data();
}
}
if (useScene)
{
mUserGen.GetUserPixels(0, sceneMD);
nXRes = sceneMD.XRes();
nYRes = sceneMD.YRes();
pLabels = sceneMD.Data();
}
}
void XN_CALLBACK_TYPE UserPose_PoseDetected(PoseDetectionCapability& capability, const XnChar* strPose, XnUserID nId, void* pCookie)
{
if(_printUserTracking) printf("AS3OpenNI :: Pose %s detected for user: %d\n", strPose, nId);
_userGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
_userGenerator.GetSkeletonCap().RequestCalibration(nId, true);
char cValue[50];
sprintf(cValue, "user_tracking_pose_detected:%d", nId);
if(_useSockets)
{
#if (XN_PLATFORM == XN_PLATFORM_WIN32)
g_AS3Network.sendMessage(1,6,nId);
#else
sendToSocket(USER_TRACKING_SOCKET, cValue);
#endif
}
}
XnVector3D OpenNIUser::GetUpVector()
{
if( mId )
{
UserGenerator* userGen = _device->getUserGenerator();
XnSkeletonJointPosition p0, p1;
userGen->GetSkeletonCap().GetSkeletonJointPosition( mId, XN_SKEL_TORSO, p0 );
userGen->GetSkeletonCap().GetSkeletonJointPosition( mId, XN_SKEL_NECK, p1 );
XnVector3D v0(p0.position), v1(p1.position);
XnVector3D res1 = sub( v1, v0 );
res1 = sub( v1, v0 );
res1 = normalize( res1 );
return res1;
}
return XnVector3D();
}
/*
* Function: run
*
* Starts and continues generating data from the Kinect.
* A loop runs and updates data whenever new data is available from one of the Kinect
* devices, and then the data is processed to check for patient movement.
* The loop is controlled by the "quit" global boolean, which is set to false by the
* signal handler "stop()"
*/
void KinectMonitor::run() {
XnStatus status;
SceneMetaData scene;
DepthMetaData depth;
// Start the device
status = context.StartGeneratingAll();
// Running loop
while( !quit ) {
// Wait for any new incoming data
context.WaitOneUpdateAll(depthGenerator);
// Mark the new frame
xnFPSMarkFrame(&xnFPS);
// Get the depth data from the device
depthGenerator.GetMetaData(depth);
// Get the recognized users
XnUInt16 numUsers = 15;
XnUserID users[numUsers];
userGenerator.GetUsers(users, numUsers);
// Only track the patient if they are alone
if( numUsers != 1) continue;
// Get the user data
userGenerator.GetUserPixels(users[0], scene);
// Update patient position
previous = current;
current = getPosition(users[0]);
// Raise alerts based on the patient's state and position
if( previous != current ) {
if( current == TURNED && out == false ) {
// Patient is turned
printf("Patient getting out of bed.\n");
} else if( out && bedSet ) {
printf("Patient is out of bed.\n");
}
}
}
}
void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
{
if (bSuccess)
{
if(_printUserTracking) printf("AS3OpenNI :: Calibration complete, start tracking user: %d\n", nId);
_userGenerator.GetSkeletonCap().StartTracking(nId);
char cValue[50];
sprintf(cValue, "user_tracking_user_calibration_complete:%d", nId);
if(_useSockets)
{
#if (XN_PLATFORM == XN_PLATFORM_WIN32)
g_AS3Network.sendMessage(1,8,nId);
#else
sendToSocket(USER_TRACKING_SOCKET, cValue);
#endif
}
}
else
{
if(_printUserTracking) printf("AS3OpenNI :: Calibration failed for user: %d\n", nId);
if (_needPose)
{
_userGenerator.GetPoseDetectionCap().StartPoseDetection(_strPose, nId);
}
else
{
_userGenerator.GetSkeletonCap().RequestCalibration(nId, true);
}
char cValue[50];
sprintf(cValue, "user_tracking_user_calibration_failed:%d", nId);
if(_useSockets)
{
#if (XN_PLATFORM == XN_PLATFORM_WIN32)
g_AS3Network.sendMessage(1,9,nId);
#else
sendToSocket(USER_TRACKING_SOCKET, cValue);
#endif
}
}
}
// Record one frame from data
static void takeFrame(){
SkeletonRawData rawData;
XnSkeletonJointPosition pos_left_elbow, pos_right_elbow, pos_left_shoulder, pos_right_shoulder;
// Capture Data
xnOSMemCopy(&(rawData.leftHand), &(leftHandPosition.position), sizeof(XnPoint3D));
xnOSMemCopy(&(rawData.rightHand), &(rightHandPosition.position), sizeof(XnPoint3D));
g_user.GetSkeletonCap().GetSkeletonJointPosition(g_userID, XN_SKEL_LEFT_ELBOW, pos_left_elbow);
xnOSMemCopy(&(rawData.leftElbow), &(pos_left_elbow.position), sizeof(XnPoint3D));
g_user.GetSkeletonCap().GetSkeletonJointPosition(g_userID, XN_SKEL_RIGHT_ELBOW, pos_right_elbow);
xnOSMemCopy(&(rawData.rightElbow), &(pos_right_elbow.position), sizeof(XnPoint3D));
g_user.GetSkeletonCap().GetSkeletonJointPosition(g_userID, XN_SKEL_LEFT_SHOULDER, pos_left_shoulder);
xnOSMemCopy(&(rawData.leftShoulder), &(pos_left_shoulder.position), sizeof(XnPoint3D));
g_user.GetSkeletonCap().GetSkeletonJointPosition(g_userID, XN_SKEL_RIGHT_SHOULDER, pos_right_shoulder);
xnOSMemCopy(&(rawData.rightShoulder), &(pos_right_shoulder.position), sizeof(XnPoint3D));
rawDatas.push_back(rawData);
}
void XN_CALLBACK_TYPE UserTracker::User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
printf("%d New User %d\n", epochTime, nId);
UserGenerator *userGenerator = static_cast<xn::UserGenerator*>(pCookie);
// New user found
if (g_bNeedPose)
{
userGenerator->GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
}
else
{
if(userGenerator)
userGenerator->GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
/*
* Function: lostUser
*
* Gets the number of users remaining (including the lost user).
* If this was the last user, an alert is fired for "Patient not tracked."
*
* Parameters:
* UserGenerator& generator - A reference to the UserGenerator module. Not Used.
* XNUserID nID - The id referring to the lost user.
* void* pCookie
*/
void XN_CALLBACK_TYPE lostUser(UserGenerator &generator,
XnUserID nID, void *pCookie) {
// Get the available users (allowing enough space for up to 15 different users)
XnUInt16 numUsers = 15;
XnUserID users[numUsers];
userGenerator.GetUsers(users, numUsers);
// Raise an alert if there are no users being tracked (meaning we lost the patient).
if( numUsers <= 1) {
printf("Patient not tracked.\n");
}
}
void XN_CALLBACK_TYPE User_NewUser(UserGenerator& generator, XnUserID nId, void* pCookie)
{
if(_printUserTracking) printf("AS3OpenNI :: New User: %d\n", nId);
if(_needPose)
{
_userGenerator.GetPoseDetectionCap().StartPoseDetection(_strPose, nId);
}
else
{
_userGenerator.GetSkeletonCap().RequestCalibration(nId, true);
}
char cValue[50];
sprintf(cValue, "user_tracking_new_user:%d", nId);
if(_useSockets)
{
#if (XN_PLATFORM == XN_PLATFORM_WIN32)
g_AS3Network.sendMessage(1,2,nId);
#else
sendToSocket(USER_TRACKING_SOCKET, cValue);
#endif
}
}
/*
* 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;
}
static void cleanUp(){
g_motor.ChangeLED(KinectDevice::LED_RED);
svmFree();
g_motor.Close();
context.StopGeneratingAll();
g_depth.Release();
g_mockDepth.Release();
g_user.Release();
context.Release();
printf("Program end without errors\n");
fflush(stdout);
fclose(record);
}
void glutDisplay (void){
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Setup the OpenGL viewpoint
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
SceneMetaData sceneMD;
DepthMetaData depthMD;
ImageMetaData imageMD;
g_DepthGenerator.GetMetaData(depthMD);
glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0);
glDisable(GL_TEXTURE_2D);
//XnStatus rc = g_Context.WaitOneUpdateAll(g_DepthGenerator);
XnStatus rc = g_Context.WaitAnyUpdateAll();
CHECK_RC("Wait Data",rc);
g_DepthGenerator.GetMetaData(depthMD);
if(g_UserGenerator.IsValid())
g_UserGenerator.GetUserPixels(0, sceneMD);
g_ImageGenerator.GetMetaData(imageMD);
DrawDepthMap(depthMD, sceneMD);
DrawImage(imageMD);
glutSwapBuffers();
}//glutdisplay
XnStatus prepare(char useScene, char useDepth, char useImage, char useIr, char useHistogram)
{
//TODO handle possible failures! Gotcha!
if (useDepth)
{
mDepthGen.GetMetaData(depthMD);
nXRes = depthMD.XRes();
nYRes = depthMD.YRes();
pDepth = depthMD.Data();
if (useHistogram)
{
calcHist();
// rewind the pointer
pDepth = depthMD.Data();
}
}
if (useScene)
{
mUserGen.GetUserPixels(0, sceneMD);
nXRes = sceneMD.XRes();
nYRes = sceneMD.YRes();
pLabels = sceneMD.Data();
}
if (useImage)
{
mImageGen.GetMetaData(imageMD);
nXRes = imageMD.XRes();
nYRes = imageMD.YRes();
pRGB = imageMD.RGB24Data();
// HISTOGRAM?????
}
if (useIr)
{
mIrGen.GetMetaData(irMD);
nXRes = irMD.XRes();
nYRes = irMD.YRes();
pIR = irMD.Data();
// HISTOGRAM????
}
}
/*
* Class: org_OpenNI_Samples_Assistant_NativeMethods
* Method: initFromContext
* Signature: (JZZ)I
*/
JNIEXPORT jint JNICALL
Java_org_OpenNI_Samples_Assistant_NativeMethods_initFromContext
(JNIEnv *env, jclass cls, jlong pContext, jboolean _hasUserGen, jboolean _hasDepthGen)
{
LOGD("init_start");
hasUserGen = _hasUserGen;
hasDepthGen = _hasDepthGen;
mContext = new Context((XnContext*) pContext);
if (!(hasUserGen || hasDepthGen))
return XN_STATUS_BAD_PARAM;
int rc;
if (hasUserGen)
{
rc = mContext->FindExistingNode(XN_NODE_TYPE_USER, mUserGen);
if (rc != XN_STATUS_OK)
{
//TODO log&retval
printf("No user node exists!");
return 1;
}
mUserGen.GetUserPixels(0, sceneMD);
}
if (hasDepthGen)
{
rc = mContext->FindExistingNode(XN_NODE_TYPE_DEPTH, mDepthGen);
if (rc != XN_STATUS_OK)
{
//TODO log&retval
printf("No depth node exists! Check your XML.");
return 1;
}
mDepthGen.GetMetaData(depthMD);
}
initGraphics();
LOGD("init_end");
return XN_STATUS_OK;
}
/*
* Class: org_OpenNI_Samples_Assistant_NativeMethods
* Method: dispose
* Signature: ()I
*/
JNIEXPORT jint JNICALL
Java_org_OpenNI_Samples_Assistant_NativeMethods_dispose (JNIEnv *, jclass)
{
LOGD("dispose_start");
disposeGraphics();
mUserGen.Release();
hasUserGen = 0;
mDepthGen.Release();
hasDepthGen = 0;
mContext->Release();
delete mContext;
mContext = 0;
LOGD("dispose_end");
return XN_STATUS_OK;
}
/**
* 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();
}
}
//----------------------------------------------------
// 描画処理
//----------------------------------------------------
void glutDisplay (void){
xnFPSMarkFrame(&g_xnFPS); // FPSの計測開始？
XnStatus rc = XN_STATUS_OK;
// 更新されたノードを待つ(どれでもいい)
rc = g_context.WaitAnyUpdateAll();
if (rc != XN_STATUS_OK){
printf("Read failed: %s\n", xnGetStatusString(rc));
printf("test\n");
return;
}
// イメージ・デプス・ユーザのデータを取得
g_image.GetMetaData(g_imageMD);
g_depth.GetMetaData(g_depthMD);
g_user.GetUserPixels(0, g_sceneMD);
// カラー・デプスバッファをクリア
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// 設定
setDepthHistgram(g_depth, g_depthMD, g_pDepthHist); // ヒストグラムの計算・作成
setTexture(); // テクスチャ設定
// 描画
drawImage(); // イメージデータの描画
// デバッグモードの文字は描画の行列と隔離
glMatrixMode(GL_PROJECTION); // 射影変換の行列の設定
//glLoadIdentity(); // スタックのクリア
glMatrixMode(GL_MODELVIEW); // モデルビュー変換の行列の設定
glLoadIdentity();
if(g_debugMode) glDebug(); // デバッグモード
// 一度だけスクリーンショットをとる
if(g_screenShotImageMode){
ostringstream fname;
fname << OUT_IMAGE_PATH ;//出力ファイル名
std::string name = fname.str();
g_glScreenShot.screenshot(name.c_str(), 24);
g_screenShotImageMode = !g_screenShotImageMode; // トグル
}
// 一度だけ深さデータを取得する
if(g_screenShotDepthMode){
ofstream ofs(OUT_DEPTH_PATH);
const XnDepthPixel* pDepth = g_depthMD.Data();
for (XnUInt y = 0; y < KINECT_IMAGE_HEIGHT; y ++){
for (XnUInt x = 0; x < KINECT_IMAGE_WIDTH; x ++, pDepth ++){
if(*pDepth < 2000){
ofs << (int)((*pDepth) * 2) << ',';
}else{
ofs << (*pDepth) << ',';
}
}
ofs << endl;
}
g_screenShotDepthMode = !g_screenShotDepthMode; // トグル
}
// Swap the OpenGL display buffers
glutSwapBuffers();
}
//----------------------------------------------------
// OpenNI関連の初期化
//----------------------------------------------------
void xnInit(void){
XnStatus rc;
EnumerationErrors errors;
rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);
if (rc == XN_STATUS_NO_NODE_PRESENT){
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
exit(1);
}else if (rc != XN_STATUS_OK){
printf("Open failed: %s\n", xnGetStatusString(rc));
exit(1);
}
//playerInit();
rc = xnFPSInit(&g_xnFPS, 180); // FPSの初期化
//CHECK_RC(rc, "FPS Init");
// デプス・イメージ・ユーザジェネレータの作成
rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
errorCheck(rc, "g_depth"); // エラーチェック
rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
errorCheck(rc, "g_image");
rc = g_context.FindExistingNode(XN_NODE_TYPE_USER, g_user);
//rc = g_user.Create(g_context);
errorCheck(rc, "g_user");
// ユーザー検出機能をサポートしているか確認
if (!g_user.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
//throw std::runtime_error("ユーザー検出をサポートしてません");
cout << "ユーザー検出をサポートしてません" << endl;
exit(1);
}
// レコーダーの設定
//rc = setRecorder(g_recorder, rc);
// ユーザコールバックの登録
XnCallbackHandle userCallbacks;
g_user.RegisterUserCallbacks(UserDetected, UserLost, NULL, userCallbacks);
// デプス・イメージ・ユーザデータの取得
g_depth.GetMetaData(g_depthMD);
g_image.GetMetaData(g_imageMD);
g_user.GetUserPixels(0, g_sceneMD);
// Hybrid mode isn't supported in this sample
// イメージとデプスの大きさが違うとエラー
if (g_imageMD.FullXRes() != g_depthMD.FullXRes() || g_imageMD.FullYRes() != g_depthMD.FullYRes()){
printf ("The device depth and image resolution must be equal!\n");
exit(1);
}
// RGB is the only image format supported.
// フォーマットの確認
if (g_imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24){
printf("The device image format must be RGB24\n");
exit(1);
}
// Texture map init
// フルスクリーン画面の大きさ調整
g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes() - 1) / 512) + 1) * 512; // 大きさによって512の倍数に調整(1024)
g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes() - 1) / 512) + 1) * 512; // 512
g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel)); // スクリーンの大きさ分の色情報の容量を確保
// 座標ポインタの初期化
g_pPoint = (XnPoint3D*)malloc(KINECT_IMAGE_SIZE * sizeof(XnPoint3D)); // 座標を入れるポインタを作成
g_pBackTex = (XnRGB24Pixel*)malloc(KINECT_IMAGE_SIZE * sizeof(XnRGB24Pixel)); // 背景画像を入れるポインタを作成
g_pBackPoint = (XnPoint3D*)malloc(KINECT_IMAGE_SIZE * sizeof(XnPoint3D)); // 背景座標を入れるポインタを作成
g_pBackDepth = (XnDepthPixel*)malloc(KINECT_IMAGE_SIZE * sizeof(XnDepthPixel)); // 背景座標を入れるポインタを作成
}
static inline void updateHandPosition(){
g_user.GetSkeletonCap().GetSkeletonJointPosition(g_userID, XN_SKEL_LEFT_HAND, leftHandPosition);
g_user.GetSkeletonCap().GetSkeletonJointPosition(g_userID, XN_SKEL_RIGHT_HAND, rightHandPosition);
}
static inline void startRecord(){
actionStartTime = g_user.GetTimestamp();
rawDatas.clear();
takeFrame();
}
/**
* Method that controls the fireballs that are
* going to be spawned per frames.
*/
void SuperFiremanBrothers :: nextFrame ()
{
if (gameStatus != STARTED) {
return;
}
static int counter = 0;
static int spawnRate = (SPAWN_RATE_FIRST_LEVEL) * players.size();
static float speedRate = (SPEED_RATE_FIRST_LEVEL) * players.size();
static int flamesInLevel = (FLAMES_IN_FIRST_LEVEL) * players.size();
const static int riseSpawnRate = (RISE_SPAWN_RATE) * players.size();
const static int riseSpeedRate = (RISE_SPEED_RATE) * players.size();
const static int riseFlamesInLevel = (RISE_FLAMES_IN_LEVEL) * players.size();
int i;
int j;
int hp;
float x;
float y;
Vector3D position;
vector <ZamusShoot> zShoot;
vector <LinqSpawnIce> lShoot;
zShoot = zamusDetector -> shoots;
lShoot = linqDetector -> iceSpawn;
UserGenerator ugen;
DepthGenerator dgen;
map <XnUserID, int> :: iterator iter;
XnUserID player;
XnSkeletonJointPosition joint;
XnPoint3D foots[2];
ugen = userDetector -> retUserGenerator();
dgen = userDetector -> retDepthGenerator();
for (i = 0; i < fireBalls.size(); i++) {
for (j = 0; j < zShoot.size(); j++) {
if (fireBalls[i].isInBoundingBox(zShoot[j].position)) {
fireBalls[i].extinguish();
players[zShoot[j].player] += POINTS_PER_HIT;
zShoot.erase(zShoot.begin() + j);
j--;
}
}
for (j = 0; j < lShoot.size(); j++) {
if (fireBalls[i].isInBoundingBox(lShoot[j].position)) {
fireBalls[i].extinguish();
players[lShoot[j].player] += POINTS_PER_HIT;
lShoot.erase(lShoot.begin() + j);
j--;
}
}
for (iter = players.begin(); iter != players.end(); iter++) {
player = iter -> first;
ugen.GetSkeletonCap().GetSkeletonJointPosition(player,
XN_SKEL_RIGHT_FOOT,
joint);
foots[0] = joint.position;
ugen.GetSkeletonCap().GetSkeletonJointPosition(player,
XN_SKEL_RIGHT_FOOT,
joint);
foots[1] = joint.position;
dgen.ConvertRealWorldToProjective(2, foots, foots);
if (fireBalls[i].isInBoundingBox(foots[0])) {
fireBalls[i].extinguish();
players[player] += POINTS_PER_HIT;
j--;
}
if (fireBalls[i].isInBoundingBox(foots[1])) {
fireBalls[i].extinguish();
players[player] += POINTS_PER_HIT;
j--;
}
}
zamusDetector -> shoots = zShoot;
linqDetector -> iceSpawn = lShoot;
if (fireBalls[i].getZPos() > FIRE_LIMIT) {
lostGame = true;
return;
}
else {
fireBalls[i].advance(speedRate);
}
}
if ((counter == spawnRate) &&
(numFlames < flamesInLevel)){
x = (float)rand() / (float)RAND_MAX;
y = (float)rand() / (float)RAND_MAX;
x = (rand() % 800) + x;
y = (rand() % 600) + y - floorLevel;
hp = rand() % 3 + 1;
position = Vector3D(x, y, -3500.0);
fireBalls.push_back(Flame(position, hp, flameModel.flame));
counter = 0;
numFlames++;
}
else {
counter += 1;
}
if ((numFlames == flamesInLevel) &&
(fireBalls.size() == 0)) {
level++;
numFlames = 0;
counter = 0;
flamesInLevel += riseFlamesInLevel;
if ( (spawnRate - riseSpawnRate) > 0) {
spawnRate -= riseSpawnRate ;
}
else {
spawnRate = 1;
}
speedRate += riseSpeedRate;
printf("Level %d start!\n", level);
}
}
/**
* Initialize XN functions
*/
void initialize()
{
ImageMetaData imageMD;
XnStatus status;
int dummy;
srand ( time(NULL) );
// Initializing context and checking for enumeration errors
status = g_Context.InitFromXmlFile(XML_CONFIG_FILE, &g_Error);
checkEnumError(status, g_Error);
// Finding nodes and checking for errors
STATUS_CHECK(g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator), "Finding depth node");
STATUS_CHECK(g_Context.FindExistingNode(XN_NODE_TYPE_SCENE, g_SceneAnalyzer), "Finding scene analizer");
// Note: when the image generation node is handled the program gets
// too slow.
// STATUS_CHECK(g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator), "Finding image node");
// Set view point of Depth generator to the image generator point of
// view.
// STATUS_CHECK(g_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(g_ImageGenerator), "Set View Point");
STATUS_CHECK(g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator), "Finding user node");
//g_ImageGenerator.GetMetaData(imageMD);
// Checking camera pixel format
//if (imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24) {
// reportError("Camera format not supported...!\n");
//}
// Checking user generator capabilities
if(!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
reportError("Skeleton capability not supported\n");
}
if(!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) {
reportError("Pose detection capability not supported\n");
}
printf("Number of players: ");
dummy = scanf("%d", &g_MaxPlayers);
printf("\n");
//Initialize user detector object
g_UserDetector = UserDetector(g_UserGenerator, g_DepthGenerator);
g_UserDetector.registerCallbacks();
g_ZamusDetector = new Zamus(&g_UserDetector);
g_LinqDetector = new Linq(&g_UserDetector);
g_BusterDetector = new BusterDetector(g_ZamusDetector, &g_UserDetector);
g_IceRodDetector = new IceRodDetector(g_LinqDetector, &g_UserDetector);
// Initialize image render object
g_SceneRenderer = SceneRenderer(&g_ImageGenerator,
&g_DepthGenerator,
&g_SceneAnalyzer,
&g_UserDetector,
g_ZamusDetector,
g_LinqDetector);
STATUS_CHECK(g_Context.StartGeneratingAll(), "Context generation");
g_SFBgame = SuperFiremanBrothers(&g_UserDetector,
&g_SceneAnalyzer,
g_ZamusDetector,
g_LinqDetector,
g_MaxPlayers
);
}
