samer@0: #include "testApp.h"
samer@0: 
samer@0: #define IMGWIDTH  640
samer@0: #define IMGHEIGHT 480
samer@0: 
samer@0: static int Colors[6][3] = {
samer@0: 	255,   0,   0,
samer@0: 	  0, 200,   0,
samer@0: 	  0,   0, 255,
samer@0: 	200, 175,   0,
samer@0: 	  0, 200, 255,
samer@0: 	255,   0, 255
samer@0: };
samer@0: 
samer@0: 
samer@0: /* Not using these for the time being
samer@0: // Callback: A user was found
samer@0: void XN_CALLBACK_TYPE NewUserDetected(UserGenerator& rGenerator,XnUserID nID ,void* pCookie) {
samer@0: 	printf("New User %d\n", nID);
samer@0: 	send_osc_i(((testApp*)pCookie)->sender),"/birth",nID);
samer@0: }
samer@0: 
samer@0: // Callback: An existing user was lost
samer@0: void XN_CALLBACK_TYPE UserLostDetected(UserGenerator& rGenerator ,XnUserID nID ,void* pCookie) {
samer@0: 	printf("Lost user %d\n", nID);
samer@0: 	send_osc_i(((testApp*)pCookie)->sender),"/death",nID);
samer@0: }
samer@0: */
samer@0: 
samer@0: static void inline send_osc_i(ofxOscSender& sender, const char *path, int arg) {
samer@0: 	ofxOscMessage msg;
samer@0: 	msg.setAddress(path);
samer@0: 	msg.addIntArg(arg);
samer@0: 	sender.sendMessage(msg);
samer@0: }
samer@0: 
samer@0: static inline void check_rc(XnStatus rc, const char *tag){
samer@0: 	if (rc != XN_STATUS_OK) {
samer@0: 		printf("FAILURE: %s - %s\n",tag,xnGetStatusString(rc));
samer@0: 		OF_EXIT_APP(0);
samer@0: 	}
samer@0: }
samer@0: 
samer@0: static inline int check_nonfatal(XnStatus rc, const char *tag){
samer@0: 	if (rc != XN_STATUS_OK) {
samer@0: 		printf("WARNING: %s - %s\n",tag,xnGetStatusString(rc));
samer@0: 		return 1;
samer@0: 	} else return 0;
samer@0: }
samer@0: 
samer@0: //--------------------------------------------------------------
samer@0: 
samer@0: class int3d { 
samer@0: public:
samer@0: 	int X, Y, Z; 
samer@0: 	inline void set(int x, int y, int z) { X=x; Y=y; Z=z; }
samer@0: 	inline void add(int x, int y, int z) { X+=x; Y+=y; Z+=z; }
samer@0: 	inline void div(float d, XnVector3D *p) { p->X=X/d; p->Y=Y/d; p->Z=Z/d; }
samer@0: 	inline void cpy(XnVector3D *p) { p->X=X; p->Y=Y; p->Z=Z; }
samer@0: };
samer@0: 
samer@0: static void project_into_plane(XnPlane3D& plane, XnVector3D& r, XnVector3D *q) {
samer@0: 	float l = (r.X-plane.ptPoint.X)*plane.vNormal.X 
samer@0: 			+ (r.Y-plane.ptPoint.Y)*plane.vNormal.Y
samer@0: 			+ (r.Z-plane.ptPoint.Z)*plane.vNormal.Z;
samer@0: 	
samer@0: 	q->X = r.X - l*plane.vNormal.X;
samer@0: 	q->Y = r.Y - l*plane.vNormal.Y;
samer@0: 	q->Z = r.Z - l*plane.vNormal.Z;
samer@0: }
samer@0: 
samer@0: class UserStats {
samer@0: public:
samer@0: 	struct int3d left, right, top, bottom, sum;
samer@0: 	int numPixels;
samer@0: 	
samer@0: 	UserStats() { 
samer@0: 		left.X=IMGWIDTH; 
samer@0: 		top.Y=IMGHEIGHT; 
samer@0: 		right.X=bottom.Y=0;
samer@0: 		sum.X=sum.Y=sum.Z=0;
samer@0: 		numPixels=0;
samer@0: 	}
samer@0: 
samer@0: 	inline int  area() { return numPixels; }
samer@0: 	inline void centroid(XnVector3D *p) { sum.div(numPixels,p); }
samer@0: 	inline void accum(int x, int y, int z) {
samer@0: 		if (x<left.X)   left.set(x,y,z);
samer@0: 		if (x>right.X)  right.set(x,y,z);
samer@0: 		if (y<top.Y)    top.set(x,y,z);
samer@0: 		if (y>bottom.Y) bottom.set(x,y,z);
samer@0: 		sum.add(x,y,z);
samer@0: 		numPixels++;
samer@0: 	}
samer@0: 	
samer@0: };
samer@0: 
samer@0: 
samer@0: //--------------------------------------------------------------
samer@0: 
samer@0: testApp::testApp(const char *host, int port, int FPS) {
samer@0: 	ofSetupOpenGL(&window, IMGWIDTH,IMGHEIGHT, OF_WINDOW);
samer@8: 	ofSetWindowTitle("Melody Triangle Tracker");
samer@0: 	ofSetFrameRate(FPS);
samer@0: 
samer@0: 	// initialise OSC sender
samer@0: 	printf("OSC target: %s:%d.\n",host,port);
samer@0: 	printf("Will run at %d frames per second.\n",FPS);
samer@0: 	sender.setup(host, port);
samer@0: 	draw_image=2;
samer@0: }
samer@0: 
samer@0: static XnMapOutputMode fps_map_mode(int fps) {
samer@0: 	XnMapOutputMode mapMode;
samer@0: 	
samer@0: 	mapMode.nXRes = IMGWIDTH;
samer@0: 	mapMode.nYRes = IMGHEIGHT;
samer@0: 	mapMode.nFPS  = fps;
samer@0: 	return mapMode;
samer@0: }
samer@0: 	
samer@0: void testApp::setup() {
samer@0: 
samer@0: 	font.loadFont("frabk.ttf", 12);
samer@0: 	check_rc(context.Init(),"initialise context");
samer@0: 
samer@0: 	{
samer@0: 		XnLicense license;
samer@0: 		strcpy(license.strVendor,"PrimeSense");
samer@0: 		strcpy(license.strKey,"0KOIk2JeIBYClPWVnMoRKn5cdY4=");
samer@0: 		check_rc(context.AddLicense(license),"add license");
samer@0: 	}
samer@0: 	
samer@0: 	check_rc(depthGenerator.Create(context),"create depth generator");
samer@0: 	check_rc(depthGenerator.SetMapOutputMode(fps_map_mode(30)),"config depth generator");
samer@0: 	check_rc(sceneAnalyzer.Create(context),"create scene analyzer");
samer@0: 	check_rc(sceneAnalyzer.SetMapOutputMode(fps_map_mode(30)),"config scene analyzer");
samer@0: 	check_rc(userGenerator.Create(context),"create user generator");
samer@0: 
samer@0: 	//XnCallbackHandle user_cb_handle;
samer@0: 	//userGenerator.RegisterUserCallbacks(NewUserDetected,UserLostDetected,this,user_cb_handle);
samer@0: 	
samer@0: 	max_depth = depthGenerator.GetDeviceMaxDepth();
samer@0: 	printf("Depth generator max depth=%d.\n",max_depth);
samer@0: 	
samer@0:     imagePixels = new unsigned char[IMGWIDTH*IMGHEIGHT];
samer@0: 	userImage.allocate(IMGWIDTH, IMGHEIGHT, OF_IMAGE_GRAYSCALE);
samer@0: 	for(int i=0;i<MAX_USERS;i++) users[i].present=false;
samer@0: 	ofBackground(255, 255, 255);
samer@0: 
samer@0: 	printf("Ready to start generating.\n");
samer@0: 	check_rc( context.StartGeneratingAll(), "start generating");
samer@0: }
samer@0: 
samer@0: 
samer@0: //--------------------------------------------------------------
samer@0: void testApp::update() {
samer@0: 	if (check_nonfatal(context.WaitOneUpdateAll(depthGenerator),"update")) return;
samer@0: 	
samer@0: 	timestamp = depthGenerator.GetTimestamp();
samer@0: 	got_floor = (sceneAnalyzer.GetFloor(floor_pie)==XN_STATUS_OK);
samer@0: 	if (got_floor) { // project camera onto floor
samer@0: 		XnVector3D origin = {0,0,0};
samer@0: 		project_into_plane(floor_pie, origin, &floor_offset);
samer@0: 	}
samer@0: 	
samer@0: 	const XnDepthPixel *depthMap = depthGenerator.GetDepthMap();
samer@0: 	
samer@0: 	// maybe transfer depth map to image
samer@0: 	if (draw_image==2)
samer@0: 		for (int i=0; i<IMGWIDTH*IMGHEIGHT; i++)
samer@0: 			imagePixels[i] = (255*depthMap[i])/max_depth;
samer@0: 		
samer@0: 	// get pixels for all users, for each compute centroid and bounding rectangle
samer@0: 	{	
samer@0: 		UserStats stats[MAX_USERS];
samer@0: 		
samer@0: 		userGenerator.GetUserPixels(0, sceneMetaData); // 0 for all users
samer@0: 		unsigned short *userRawPixels = (unsigned short*)sceneMetaData.Data();
samer@0: 		
samer@0: 		if (draw_image==1) {
samer@0: 			for (int i=0, y=0; y<IMGHEIGHT; y++) {
samer@0: 				for (int x=0; x<IMGWIDTH; x++, i++) {
samer@0: 					int id=userRawPixels[i];
samer@8: 					if (id>0 && id<=MAX_USERS) {
samer@0: 						stats[id-1].accum(x,y,depthMap[i]);
samer@0: 						imagePixels[i]=(128/MAX_USERS)*id;
samer@0: 					} else imagePixels[i] = 255;
samer@0: 				}
samer@0: 			}
samer@0: 		} else {
samer@0: 			for (int i=0, y=0; y<IMGHEIGHT; y++) {
samer@0: 				for (int x=0; x<IMGWIDTH; x++, i++) {
samer@0: 					int id=userRawPixels[i];
samer@8: 					if (id>0 && id<=MAX_USERS) {
samer@8: 						stats[id-1].accum(x,y,depthMap[i]);
samer@8: 					}
samer@0: 				}
samer@0: 			}
samer@0: 		}
samer@0: 
samer@0: 		for (int id0=0; id0<MAX_USERS; id0++) {
samer@0: 			if (stats[id0].area()>0) {
samer@0: 				if (!users[id0].present) {
samer@0: 					send_osc_i(sender,"/birth",id0+1);
samer@8: 					send_osc_i(sender,"/randinit",id0+1);
hekeus@5: 					send_osc_i(sender,"/start",id0+1);
samer@0: 					users[id0].present=true;
samer@0: 				}
samer@0: 				update_user(id0+1,stats[id0],&users[id0]);
samer@0: 			} else {
samer@0: 				if (users[id0].present) {
samer@0: 					send_osc_i(sender,"/death",id0+1);
samer@0: 					users[id0].present=false;
samer@0: 				}
samer@0: 			}
samer@0: 		}
samer@0: 				
samer@0: 	}
samer@0: 	if (draw_image) {
samer@0: 		userImage.setFromPixels(imagePixels, IMGWIDTH, IMGHEIGHT, OF_IMAGE_GRAYSCALE);
samer@0: 	}
samer@0: }
samer@0: 
samer@0: void testApp::update_user(int id, UserStats& stats, UserData *user) {
samer@0: 	XnVector3D    proj[3], real[3];
samer@0: 	int           *coors=user->plot_coors;
samer@0: 	
samer@0: 	stats.centroid(&proj[0]);	// centroid in device coordinate
samer@0: 	stats.left.cpy(&proj[1]);	// left edge in device coordinates
samer@0: 	stats.right.cpy(&proj[2]);	// right edge in device coordinates
samer@0: 	
samer@0: 	// save values required for drawing later
samer@0: 	coors[0]=floor(proj[0].X); 
samer@0: 	coors[1]=floor(proj[0].Y);
samer@0: 	coors[2]=floor(proj[0].Z);
samer@0: 	coors[3]=stats.left.X;		
samer@0: 	coors[4]=stats.right.X;
samer@0: 	coors[5]=stats.top.Y;		
samer@0: 	coors[6]=stats.bottom.Y;
samer@0: 	
samer@0: 	if (got_floor) {
samer@0: 		ofxOscMessage m;
samer@0: 		XnVector3D    on_floor;
samer@0: 		
samer@0: 		// convert to camera-centric world coordinates
samer@0: 		depthGenerator.ConvertProjectiveToRealWorld(3,proj,real);
samer@0: 		
samer@0: 		// projection of centroid onto floor...
samer@0: 		project_into_plane(floor_pie,real[0],&on_floor);
samer@0: 		
samer@0: 		// ...relative to projection of camera on floor
samer@0: 		on_floor.X -= floor_offset.X;
samer@0: 		on_floor.Y -= floor_offset.Y;
samer@0: 		on_floor.Z -= floor_offset.Z;
samer@0: 		
samer@0: 		m.setAddress("/track");
samer@0: 		m.addIntArg(id);
samer@0: 		
samer@0: 		m.addFloatArg(on_floor.X);
samer@0: 		// m.addFloatArg(on_floor.Y); // not interesting
samer@0: 		m.addFloatArg(on_floor.Z);
samer@0: 		
samer@0: 		{	// distances to edges of bounding box
samer@0: 			// corrected for perspective
samer@0: 			float z=coors[2]; // depth
samer@0: 			m.addIntArg(z*(coors[0]-coors[3]));
samer@0: 			m.addIntArg(z*(coors[4]-coors[0]));
samer@0: 			m.addIntArg(z*(coors[1]-coors[5]));
samer@0: 			m.addIntArg(z*(coors[6]-coors[1]));
samer@0: 			// area, corrected for perspective
samer@0: 			m.addIntArg(z*sqrt(stats.area()));
samer@0: 		}
samer@0: 		
samer@0: 		sender.sendMessage(m);
samer@0: 	}
samer@0: }
samer@0: 
samer@0: //--------------------------------------------------------------
samer@0: static inline void draw_rect(float x1, float x2, float y1, float y2){
samer@0: 	ofLine(x1,y1,x1,y2);
samer@0: 	ofLine(x1,y1,x2,y1);
samer@0: 	ofLine(x1,y2,x2,y2);
samer@0: 	ofLine(x2,y1,x2,y2);
samer@0: }
samer@0: 
samer@0: 
samer@0: void testApp::draw(){
samer@0: 	char buf[64];
samer@0: 	ofSetColor(255,255,255);
samer@8: 	ofScale(ofGetWidth()/(float)IMGWIDTH,ofGetHeight()/(float)IMGHEIGHT);
samer@0: 	
samer@0:     if (draw_image) userImage.draw(0, 0, IMGWIDTH, IMGHEIGHT);
samer@0: 	// ofSetColor(0);
samer@0: 	// ofLine(IMGWIDTH,0,IMGWIDTH,IMGHEIGHT);
samer@0: 
samer@0: 	if (draw_time) {
samer@0: 		ofSetColor(180,0,0);
samer@0: 		sprintf(buf,"%llu",timestamp);
samer@0: 		font.drawString(buf,0,IMGHEIGHT-8);
samer@0: 	}
samer@0: 	
samer@0: 	for(int k=0; k<MAX_USERS; k++) {
samer@0: 		if (users[k].present) {
samer@0: 			int *coors=users[k].plot_coors; // pointer to coors for this user
samer@0: 			int *color=Colors[k%6];
samer@0: 			
samer@0: 			ofSetColor(color[0],color[1],color[2]);
samer@0: 			ofRect(coors[0],coors[1],10,10);
samer@0: 			font.drawString(ofToString(k+1), coors[0]-10, coors[1]-10);
samer@0: 			draw_rect(coors[3],coors[4],coors[5],coors[6]);
samer@0: 			// ofRect(coors[0]+IMGWIDTH, (IMGHEIGHT*coors[2])/max_depth, 10, 10);
samer@0: 		}
samer@0: 	}
samer@8: 	if (!got_floor) {
samer@0: 		ofSetColor(255,0,0);
samer@0: 		ofRect(16,16,16,16);
samer@0: 	}
samer@0: }
samer@0: 
samer@0: void testApp::keyPressed(int key){
samer@0: 	switch (key) {
samer@0: 		case 'd': draw_image = (draw_image+1)%3; break;
samer@0: 		case 'f':
samer@0: 			printf("floor plane: (%f,%f,%f), <%f,%f,%f>\n",
samer@0: 				floor_pie.ptPoint.X, floor_pie.ptPoint.Y, floor_pie.ptPoint.Z,
samer@0: 				floor_pie.vNormal.X, floor_pie.vNormal.Y, floor_pie.vNormal.Z);
samer@0: 			break;
samer@0: 		case 't': draw_time = !draw_time; break;
samer@8: 		case 'F': ofToggleFullscreen(); break;
samer@0: 	}
samer@0: }
samer@0: 
samer@0: void testApp::keyReleased(int key) {}
samer@0: void testApp::mouseMoved(int x, int y ) {}
samer@0: void testApp::mouseDragged(int x, int y, int button) {}
samer@0: void testApp::mousePressed(int x, int y, int button) {}
samer@0: void testApp::mouseReleased(int x, int y, int button) {}
samer@0: void testApp::windowResized(int w, int h) {}
samer@0: