DrawScene.cpp

/***************************************************************************
 *   Copyright (C) 2011 by Teyssier Loïc                                  *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include <GL/glut.h>
#include "Tracknect.h"
#include <math.h>

void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, XnUserID player);

void DrawRectangle(float topLeftX, float topLeftY, float bottomRightX, float bottomRightY)
{
	GLfloat verts[8] = {	topLeftX, topLeftY,
		topLeftX, bottomRightY,
		bottomRightX, bottomRightY,
		bottomRightX, topLeftY
	};
	glVertexPointer(2, GL_FLOAT, 0, verts);
	glDrawArrays(GL_TRIANGLE_FAN, 0, 4);

	glFlush();
}
extern xn::UserGenerator g_UserGenerator;
extern xn::DepthGenerator g_DepthGenerator;

extern int g_iMode;

#define MAX_DEPTH 10000
float g_pDepthHist[MAX_DEPTH];
unsigned int getClosestPowerOfTwo(unsigned int n)
{
	unsigned int m = 2;
	while(m < n) m<<=1;

	return m;
}
GLuint initTexture(void** buf, int& width, int& height)
{
	GLuint texID = 0;
	glGenTextures(1,&texID);

	width = getClosestPowerOfTwo(width);
	height = getClosestPowerOfTwo(height); 
	*buf = new unsigned char[width*height*4];
	glBindTexture(GL_TEXTURE_2D,texID);

	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);

	return texID;
}

GLfloat texcoords[8];
void DrawTexture(float topLeftX, float topLeftY, float bottomRightX, float bottomRightY)
{
	glEnableClientState(GL_TEXTURE_COORD_ARRAY);
	glTexCoordPointer(2, GL_FLOAT, 0, texcoords);

	DrawRectangle(topLeftX, topLeftY, bottomRightX, bottomRightY);

	glDisableClientState(GL_TEXTURE_COORD_ARRAY);
}

XnFloat Colors[][3] =
{
	{0,1,1},
	{1,0,1},
	{0,1,0},
	{1,1,0},
	{1,0,0},
	{1,.5,0},
	{.5,1,0},
	{0,.5,1},
	{.5,0,1},
	{1,1,.5},
	{1,1,1}
};
XnUInt32 nColors = 10;

void glPrintString(void *font, char *str)
{
	int i,l = strlen(str);

	for(i=0; i<l; i++)
	{
		glutBitmapCharacter(font,*str++);
	}
}

void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
{
	if (!g_UserGenerator.GetSkeletonCap().IsCalibrated(player))
	{
		printf("not calibrated!\n");
		return;
	}
	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		printf("not tracked!\n");
		return;
	}

	XnSkeletonJointPosition joint1, joint2;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);

	if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
	{
		return;
	}

	XnPoint3D pt[2];
	pt[0] = joint1.position;
	pt[1] = joint2.position;

	glLineWidth(1);
	g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
	glVertex3i(pt[0].X, pt[0].Y, 0);
	glVertex3i(pt[1].X, pt[1].Y, 0);
}

void DrawVector(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
{
	if (!g_UserGenerator.GetSkeletonCap().IsCalibrated(player))
	{
		printf("not calibrated!\n");
		return;
	}
	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		printf("not tracked!\n");
		return;
	}

	XnSkeletonJointPosition joint1, joint2;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);

	if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
	{
		return;
	}

	XnPoint3D pt[2];
	pt[0] = joint1.position;
	pt[1] = joint2.position;


	glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
	g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
	glVertex3i(pt[0].X, pt[0].Y, 0);
	glVertex3i(pt[1].X, pt[1].Y, 0);
}

void DrawSkeleton(XnUserID player)
{
	if (player != 0)
	{
		glLineWidth(10);
		glBegin(GL_LINES);
		glColor4f(1,1,1,1);

		DrawLimb(player, XN_SKEL_HEAD, XN_SKEL_NECK);

		DrawLimb(player, XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
		DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
		DrawLimb(player, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);

		DrawLimb(player, XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
		DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
		DrawLimb(player, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);

		DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
		DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);

		DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
		DrawLimb(player, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
		DrawLimb(player, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);

		DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
		DrawLimb(player, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
		DrawLimb(player, (XnSkeletonJoint)22, XN_SKEL_RIGHT_FOOT);
		glEnd();

		if (g_iMode == TN_MODE_HEAD_TRACKING)
		{
			XnPoint3D pt[1];
			pt[0] = TnGetTrackedObjectCoordonates();

			int currentState = TnGetTrackingState();


			g_DepthGenerator.ConvertRealWorldToProjective(1, pt, pt);

			glColor4f(1,1,1,1);
			glRasterPos2i(20, 20);
			XnChar strLabel[200];

			XnSkeletonJointPosition joint1;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, XN_SKEL_HEAD, joint1);

			if (joint1.fConfidence > 0.5)
			{
				sprintf(strLabel, "Head Position :	X:%f	,	Y:%f	,	Z:%f", pt[0].X, pt[0].Y, pt[0].Z);

				if (currentState == TN_STATE_HEAD_TRACKED)
					glColor4f(0.0f,1.0f,0.0f,1.0f);
				else
					glColor4f(1.0f,0.0f,0.0f,1.0f);

				glPointSize(15);
				glBegin(GL_POINTS);
					glVertex2f(pt[0].X,pt[0].Y);
				glEnd();
			}

			else
				sprintf(strLabel, "Head lost");

			glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
		}

		else if (g_iMode == TN_MODE_HAND_TRACKING)
		{
			XnPoint3D pt[1];
			int currentState = TnGetTrackingState();

			XnSkeletonJointPosition joint1;
			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, XN_SKEL_LEFT_HAND, joint1); // Left Hand
			if (joint1.fConfidence > 0.5)
			{
				pt[0] = joint1.position;
				g_DepthGenerator.ConvertRealWorldToProjective(1, pt, pt);

				if (currentState == TN_STATE_LEFT_HAND_TRACKED || currentState == TN_STATE_BOTH_HANDS_TRACKED)
					glColor4f(0.0f,1.0f,0.0f,1.0f);
				else
					glColor4f(1.0f,0.0f,0.0f,1.0f);

				glPointSize(15);
				glBegin(GL_POINTS);
					glVertex2f(pt[0].X,pt[0].Y);
				glEnd();
			}


			g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, XN_SKEL_RIGHT_HAND, joint1); // Right Hand
			if (joint1.fConfidence > 0.5)
			{
				pt[0] = joint1.position;
				g_DepthGenerator.ConvertRealWorldToProjective(1, pt, pt);

				if (currentState == TN_STATE_RIGHT_HAND_TRACKED || currentState == TN_STATE_BOTH_HANDS_TRACKED)
					glColor4f(0.0f,1.0f,0.0f,1.0f);
				else
					glColor4f(1.0f,0.0f,0.0f,1.0f);

				glPointSize(15);
				glBegin(GL_POINTS);
					glVertex2f(pt[0].X,pt[0].Y);
				glEnd();
			}


		}

		else if (g_iMode == TN_MODE_RUNNING_GAME)
		{
			static int forward = 0;
			static int count = 0;

			glColor4f(1,1,1,1);
			glRasterPos2i(20, 20);
			static XnChar strLabel[200];

			if (TnGetTrackingState() == TN_STATE_JUMPING)
			{
				sprintf(strLabel, "Jumping !");
			}
			else{

				if (Tn_Y()>100 && forward ==0)
				{
					forward = 1;
					count++;
				}
				if (Tn_Y()<-100 && forward ==1)
				{
					forward = 0;
					count++;
				}

				sprintf(strLabel, "%d", count);
			}

			if (TnIsProtecting())
				sprintf(strLabel, "Protecting");



			glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
		}
	}

}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, XnUserID player)
{
	if (player == 0 || TnGetDrawDepthWhileTrackingSkeleton()) {
		static bool bInitialized = false;
		static GLuint depthTexID;
		static unsigned char* pDepthTexBuf;
		static int texWidth, texHeight;

		 float topLeftX;
		 float topLeftY;
		 float bottomRightY;
		 float bottomRightX;
		float texXpos;
		float texYpos;

		if(!bInitialized)
		{
			texWidth =  getClosestPowerOfTwo(dmd.XRes());
			texHeight = getClosestPowerOfTwo(dmd.YRes());

			depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;

			bInitialized = true;

			topLeftX = dmd.XRes();
			topLeftY = 0;
			bottomRightY = dmd.YRes();
			bottomRightX = 0;
			texXpos =(float)dmd.XRes()/texWidth;
			texYpos  =(float)dmd.YRes()/texHeight;

			memset(texcoords, 0, 8*sizeof(float));
			texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;

		}
		unsigned int nValue = 0;
		unsigned int nHistValue = 0;
		unsigned int nIndex = 0;
		unsigned int nX = 0;
		unsigned int nY = 0;
		unsigned int nNumberOfPoints = 0;
		XnUInt16 g_nXRes = dmd.XRes();
		XnUInt16 g_nYRes = dmd.YRes();

		unsigned char* pDestImage = pDepthTexBuf;

		//const XnDepthPixel* pDepth = dmd.Data();
		const unsigned short* pDepth = (unsigned short*)dmd.Data();
		const XnLabel* pLabels = smd.Data();

		// Calculate the accumulative histogram
		memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
		for (nY=0; nY<g_nYRes; nY++)
		{
			for (nX=0; nX<g_nXRes; nX++)
			{
				nValue = *pDepth;

				if (nValue != 0)
				{
					g_pDepthHist[nValue]++;
					nNumberOfPoints++;
				}

				pDepth++;
			}
		}

		for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
		{
			g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
		}
		if (nNumberOfPoints)
		{
			for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
			{
				g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
			}
		}

		pDepth = dmd.Data();
		{
			XnUInt32 nIndex = 0;
			// Prepare the texture map
			for (nY=0; nY<g_nYRes; nY++)
			{
				for (nX=0; nX < g_nXRes; nX++, nIndex++)
				{ // On parcours tous les pixels de la fenêtre

					nValue = *pDepth; // Profondeur

					// Si plabels est a 0 c'est qu'il n'y a pas de player à ce pixel,
					// sinon plabel est = au label du player à ce pixel
					XnLabel label = *pLabels;

					XnUInt32 nColorID = label % nColors;

					/*if (player != 0)
					{
						if (label == player)
							nColorID = 2 % nColors;
						else
							nColorID = nColors;
					}*/

					if (label == 0)
					{
						nColorID = nColors;
					}


					if (nValue != 0)
					{
						nHistValue = g_pDepthHist[nValue];

//						if (player != 0  )
//						{
//							XnPoint3D com;
//							g_UserGenerator.GetCoM(player, com);
//							if (com.Z != 0) // Mettre tous les pixels à noir quand le squelette est présent
//								nHistValue = 0;
//						}

						pDestImage[0] = nHistValue * Colors[nColorID][0];
						pDestImage[1] = nHistValue * Colors[nColorID][1];
						pDestImage[2] = nHistValue * Colors[nColorID][2];
					}
					else
					{
						pDestImage[0] = 0;
						pDestImage[1] = 0;
						pDestImage[2] = 0;
					}

					pDepth++;
					pLabels++;
					pDestImage+=3;
				}

				pDestImage += (texWidth - g_nXRes) *3;
			}
		}

		glBindTexture(GL_TEXTURE_2D, depthTexID);
		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);

		// Display the OpenGL texture map
		glColor4f(0.75,0.75,0.75,1);

		glEnable(GL_TEXTURE_2D);
		DrawTexture(dmd.XRes(),dmd.YRes(),0,0);
		glDisable(GL_TEXTURE_2D);

		char strLabel[20] = "";
		XnUserID aUsers[15];
		XnUInt16 nUsers;
		g_UserGenerator.GetUsers(aUsers, nUsers);
		for (int i = 0; i < nUsers; ++i)
		{
			XnPoint3D com;
			g_UserGenerator.GetCoM(aUsers[i], com);
			g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);

			glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);



			if (player == 0)
			{
				sprintf(strLabel, "%d", aUsers[i]);
				glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
				glRasterPos2i(com.X, com.Y);
				glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
			}

			/*else // if player != 0  ,  ecrire player en rouge sur le squelette
				if (aUsers[i] == player)
				{
					sprintf(strLabel, "Player");
					glColor4f(1,0,0,1);
					glRasterPos2i(com.X, com.Y);
					glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
				}*/

		}
	}

	DrawSkeleton(player);

}