/**
* Constructor of the class.
* @param ud pointer to a User Detector type.
* @param sa pointer to a Scene Analyzer type.
* @param zd pointer to Zamus Listener type.
* @ṕaram ld pointer to Linq Listener type.
* @param mp max numer of players allowed.
*/
SuperFiremanBrothers :: SuperFiremanBrothers (UserDetector *ud,
SceneAnalyzer *sa,
Zamus *zd,
Linq *ld,
int mp)
{
XnPlane3D floor;
DepthGenerator dgen;
dgen = ud -> retDepthGenerator();
sa -> GetFloor(floor);
dgen.ConvertRealWorldToProjective(1,
&floor.ptPoint,
&floor.ptPoint);
floorLevel = floor.ptPoint.Y + 100;
userDetector = ud;
sceneAnalyzer = sa;
zamusDetector = zd;
linqDetector = ld;
players = map <XnUserID, int> ();
fireBalls = vector <Flame> (MAX_FIREBALLS, Flame());
level = 0;
numFlames = 0;
maxPlayers = mp;
winGame = false;
lostGame = false;
gameStatus = NOT_STARTED;
flameModel = FlameModel();
}
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);
}
void OpenNIUser::renderBone( int joint1, int joint2 )
{
DepthGenerator* depth = _device->getDepthGenerator();
if( !depth ) return;
OpenNIBone* bone1 = mBoneList[joint1-1];
OpenNIBone* bone2 = mBoneList[joint2-1];
// Convert a point from world coordinates to screen coordinates
XnPoint3D point1, point2;
XnPoint3D realJointPos1, realJointPos2;
realJointPos1.X = bone1->position[0];
realJointPos1.Y = bone1->position[1];
realJointPos1.Z = bone1->position[2];
realJointPos2.X = bone2->position[0];
realJointPos2.Y = bone2->position[1];
realJointPos2.Z = bone2->position[2];
depth->ConvertRealWorldToProjective( 1, &realJointPos1, &point1 );
depth->ConvertRealWorldToProjective( 1, &realJointPos2, &point2 );
glLineWidth( 2 );
glBegin( GL_LINES );
glColor4f( mColor[0], mColor[1], mColor[2], 1 );
glVertex3f( point1.X, point1.Y, 0 ); //point.Z );
glVertex3f( point2.X, point2.Y, 0 ); //point.Z );
glEnd();
}
/**
* 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);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int i, j;
int width, height;
mxArray *tmpDepth;
mxArray *context_initialised_array;
mxArray *has_depth_node_array;
unsigned short* projective;
float* real_world_coord;
XnPoint3D* ni_projective;
XnPoint3D* ni_real_world_coord;
DepthGenerator* depth;
bool context_initialised = false;
bool has_depth_node = false;
mwSize dims2[3];
const mwSize *dims;
dims = mxGetDimensions(prhs[1]);
width = dims[0];
height = dims[1];
//---------------------------------
// Read input variables
//---------------------------------
// depth_obj
tmpDepth = mxGetField(prhs[0], 0, "depth_obj");
memcpy((void*)&depth, mxGetPr(tmpDepth), sizeof(DepthGenerator*));
// context_initialised
context_initialised_array = mxGetField(prhs[0], 0, "context_initialised");
context_initialised = mxGetScalar(context_initialised_array);
has_depth_node_array = mxGetField(prhs[0], 0, "has_depth_node");
has_depth_node = mxGetScalar(has_depth_node_array);
real_world_coord = (float*)mxGetPr(prhs[1]);
//-------------------------------
// Create output variables
//-------------------------------
dims2[0] = width;
dims2[1] = height;
dims2[2] = 3;
plhs[0] = mxCreateNumericArray(3, dims2, mxUINT16_CLASS, mxREAL);
projective = (unsigned short*)mxGetPr(plhs[0]);
if(!context_initialised || !has_depth_node){
return;
}
//-------------------------------
// Run
//-------------------------------
ni_projective = new XnPoint3D[height*width];
ni_real_world_coord = new XnPoint3D[height*width];
int size = width*height;
for(i = 0;i < height;i++){
for(j = 0;j < width;j++){
ni_real_world_coord[i*width+j].X = (float)real_world_coord[i*width+j];
ni_real_world_coord[i*width+j].Y = (float)real_world_coord[i*width+j+size];
ni_real_world_coord[i*width+j].Z = (float)real_world_coord[i*width+j+2*size];
}
}
XnStatus status = depth->ConvertRealWorldToProjective(size, ni_real_world_coord, ni_projective);
if(status == XN_STATUS_OK){
for(i=0;i<height;i++){
for(j = 0;j < width;j++){
projective[i*width+j] = (unsigned short)ni_projective[i*width+j].X;
projective[i*width+j+size] = (unsigned short)ni_projective[i*width+j].Y;
projective[i*width+j+2*size] = (unsigned short)ni_projective[i*width+j].Z;
}
}
}
delete []ni_real_world_coord;
delete []ni_projective;
}
/**
* 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);
}
}
void OpenNIUser::renderBone( int joint1, int joint2, float width/*=640*/, float height/*=480*/, float depth/*=1*/, bool doProjective /*= true*/, bool renderDepthInProjective/*=false */ )
{
if( mUserState != USER_TRACKING ) return;
if( doProjective )
{
DepthGenerator* depthGen = _device->getDepthGenerator();
if( !depthGen )
{
std::stringstream ss;
ss << "(renderBone) Failed to get depth generator on user: '******'" << std::endl;
DEBUG_MESSAGE( ss.str().c_str() );
return;
}
if( mBoneList.size() == 0 )
{
std::stringstream ss;
ss << "(renderBone) Bone count is zero on user: '******'" << std::endl;
DEBUG_MESSAGE( ss.str().c_str() );
return;
}
OpenNIBone* bone1 = mBoneList[joint1-1];
OpenNIBone* bone2 = mBoneList[joint2-1];
// Convert a point from world coordinates to screen coordinates
XnPoint3D point1, point2;
XnPoint3D realJointPos1, realJointPos2;
realJointPos1.X = bone1->position[0];
realJointPos1.Y = bone1->position[1];
realJointPos1.Z = bone1->position[2];
realJointPos2.X = bone2->position[0];
realJointPos2.Y = bone2->position[1];
realJointPos2.Z = bone2->position[2];
depthGen->ConvertRealWorldToProjective( 1, &realJointPos1, &point1 );
depthGen->ConvertRealWorldToProjective( 1, &realJointPos2, &point2 );
point1.X *= 0.0015625f; // div by 640
point1.Y *= 0.00208333333333333333333333333333f; // div by 480
point2.X *= 0.0015625f; // div by 640
point2.Y *= 0.00208333333333333333333333333333f; // div by 480
point1.Z *= depth;
point2.Z *= depth;
glBegin( GL_LINES );
glColor4f( mColor[0], mColor[1], mColor[2], 1.0f );
glVertex3f( point1.X*width, point1.Y*height, (renderDepthInProjective)?point1.Z:0 );
glVertex3f( point2.X*width, point2.Y*height, (renderDepthInProjective)?point2.Z:0 );
glEnd();
}
else
{
OpenNIBone* bone1 = mBoneList[joint1-1];
OpenNIBone* bone2 = mBoneList[joint2-1];
// Convert a point from world coordinates to screen coordinates
XnPoint3D point1, point2;
point1.X = bone1->position[0];
point1.Y = bone1->position[1];
point1.Z = bone1->position[2];
point2.X = bone2->position[0];
point2.Y = bone2->position[1];
point2.Z = bone2->position[2];
glBegin( GL_LINES );
glColor4f( mColor[0], mColor[1], mColor[2], 1.0f );
glVertex3f( point1.X, point1.Y, point1.Z );
glVertex3f( point2.X, point2.Y, point2.Z );
glEnd();
}
}
void OpenNIUser::updatePixels()
{
// Get device generators
DepthGenerator* depth = _device->getDepthGenerator();
UserGenerator* user = _device->getUserGenerator();
// Get metadata
//xn::DepthMetaData depthMetaData;
//depth->GetMetaData( depthMetaData );
xn::DepthMetaData* depthMetaData = _device->getDepthMetaData();
xn::SceneMetaData* sceneMetaData = _device->getUserMetaData();
// Get Center Of Mass
XnPoint3D com;
user->GetCoM( mId, com );
// Convert to screen coordinates
depth->ConvertRealWorldToProjective( 1, &com, &com );
mCenter[0] = com.X;
mCenter[1] = com.Y;
mCenter[2] = com.Z;
if( _enablePixels )
{
// Get user pixels
user->GetUserPixels( mId, *sceneMetaData );
//xn::SceneMetaData sceneMetaData;
//user->GetUserPixels( mId, sceneMetaData );
// Get labels
const XnLabel* labels = sceneMetaData->Data();
if( labels )
{
//
// Generate a bitmap with the user pixels colored
//
uint16_t* pDepth = _device->getDepthMap();
int depthWidth = depthMetaData->XRes();
int depthHeight = depthMetaData->YRes();
if( !_userPixels || (mWidth != depthWidth) || (mHeight != depthHeight) )
{
mWidth = depthWidth;
mHeight = depthHeight;
allocate( depthWidth, depthHeight );
}
xnOSMemSet( _backUserPixels, 0, depthWidth*depthHeight*3 );
uint8_t* pixels = _backUserPixels;
int index = 0;
for( int j=0; j<depthHeight; j++ )
{
for( int x=0; x<depthWidth; x++ )
{
// Only fill bitmap with current user's data
/*if( *labels != 0 && *labels == mId )
{
// Pixel is not empty, deal with it.
uint32_t nValue = *pDepth;
mColor[0] = g_Colors[mId][0];
mColor[1] = g_Colors[mId][1];
mColor[2] = g_Colors[mId][2];
if( nValue != 0 )
{
int nHistValue = _device->getDepthMap24()[nValue];
pixels[index+0] = 0xff & static_cast<uint8_t>(nHistValue * mColor[0]);//g_Colors[nColorID][0]);
pixels[index+1] = 0xff & static_cast<uint8_t>(nHistValue * mColor[1]);//g_Colors[nColorID][1]);
pixels[index+2] = 0xff & static_cast<uint8_t>(nHistValue * mColor[2]);//g_Colors[nColorID][2]);
}
}****/
// Check out for every user visible and fill bitmap with correct coloring
// NOTE! This should be removed in the future. User should only know about its own data and not all
if( *labels != 0 )
{
// Pixel is not empty, deal with it.
uint32_t nValue = *pDepth;
XnLabel label = *labels;
XnUInt32 nColorID = label % nColors;
if( label == 0 )
{
nColorID = nColors;
}
mColor[0] = g_Colors[mId][0];
mColor[1] = g_Colors[mId][1];
mColor[2] = g_Colors[mId][2];
if( nValue != 0 )
{
int nHistValue = _device->getDepthMap24()[nValue];
pixels[index+0] = 0xff & static_cast<uint8_t>(nHistValue * g_Colors[nColorID][0]);
pixels[index+1] = 0xff & static_cast<uint8_t>(nHistValue * g_Colors[nColorID][1]);
pixels[index+2] = 0xff & static_cast<uint8_t>(nHistValue * g_Colors[nColorID][2]);
}
}
pDepth++;
labels++;
index += 3;
}
}
memcpy( _userPixels, _backUserPixels, mWidth*mHeight*3 );
}
}
}
void OpenNIUser::renderJoints( float pointSize )
{
if( _device->getUserGenerator()->GetSkeletonCap().IsTracking(mId) )
{
DepthGenerator* depth = _device->getDepthGenerator();
if( !depth ) return;
// Old OpenGL rendering method. it's fine for now.
//glDisable( GL_TEXTURE_2D );
glBegin( GL_QUADS );
int index = 1;
for( std::vector<OpenNIBone*>::iterator it = mBoneList.begin(); it != mBoneList.end(); it++, index++ )
{
OpenNIBone* bone = *it;
// Convert a point from world coordinates to screen coordinates
XnPoint3D point;
XnPoint3D realJointPos;
realJointPos.X = bone->position[0];
realJointPos.Y = bone->position[1];
realJointPos.Z = bone->position[2];
depth->ConvertRealWorldToProjective( 1, &realJointPos, &point );
float sx = pointSize;
float sy = pointSize;
glColor4f( 1, 1, 1, 1 );
glVertex3f( -sx+point.X, -sy+point.Y, 0 );//point.Z );
glVertex3f( sx+point.X, -sy+point.Y, 0 );//point.Z );
glVertex3f( sx+point.X, sy+point.Y, 0 );//point.Z );
glVertex3f( -sx+point.X, sy+point.Y, 0 );//point.Z );
}
glEnd();
//
// Render body connecting lines
//
renderBone( SKEL_HEAD, SKEL_NECK );
renderBone( SKEL_NECK, SKEL_LEFT_SHOULDER );
renderBone( SKEL_LEFT_SHOULDER, SKEL_LEFT_ELBOW );
renderBone( SKEL_LEFT_ELBOW, SKEL_LEFT_HAND );
renderBone( SKEL_NECK, SKEL_RIGHT_SHOULDER );
renderBone( SKEL_RIGHT_SHOULDER, SKEL_RIGHT_ELBOW );
renderBone( SKEL_RIGHT_ELBOW, SKEL_RIGHT_HAND );
renderBone( SKEL_LEFT_SHOULDER, SKEL_TORSO );
renderBone( SKEL_RIGHT_SHOULDER, SKEL_TORSO );
renderBone( SKEL_TORSO, SKEL_LEFT_HIP );
renderBone( SKEL_LEFT_HIP, SKEL_LEFT_KNEE );
renderBone( SKEL_LEFT_KNEE, SKEL_LEFT_FOOT );
renderBone( SKEL_TORSO, SKEL_RIGHT_HIP );
renderBone( SKEL_RIGHT_HIP, SKEL_RIGHT_KNEE );
renderBone( SKEL_RIGHT_KNEE, SKEL_RIGHT_FOOT );
renderBone( SKEL_LEFT_HIP, SKEL_RIGHT_HIP );
renderBone( SKEL_WAIST, SKEL_TORSO );
renderBone( SKEL_WAIST, SKEL_NECK );
// Restore texture
//glEnable( GL_TEXTURE_2D );
}
}