nearly working trap door

1 files changed, 171 insertions, 128 deletions

diff --git a/gaunt01/src/testApp.cpp b/gaunt01/src/testApp.cpp
index 3b81cdd..87a51c4 100644
--- a/gaunt01/src/testApp.cpp
+++ b/gaunt01/src/testApp.cpp
@@ -11,15 +11,23 @@ void testApp::setup(){
 	
 	loadSettings("settings.xml");
 
-	#ifdef _USE_LIVE_VIDEO
+	vidPlayer.loadMovie("camoutput.mov"); //footage/ camera needs to be the same res as opencv planes and output
+	vidPlayer.setLoopState(OF_LOOP_NORMAL);
         vidGrabber.setVerbose(true);
-        vidGrabber.initGrabber(320,240);
-	#else
-        vidPlayer.loadMovie("camoutput.wmv"); //footage/ camera needs to be the same res as opencv planes and output
-        vidPlayer.play();
-	#endif
+        if (vidGrabber.initGrabber(640,480)) {
+		hasCamera=true;
+		useCamera=true;
+	}
+	else 
+	{
+		hasCamera=false;
+		useCamera=false;
+		vidPlayer.play();
+	}
+
 
 	colorImg.allocate(640,480);
+	colorImg.setUseTexture(true);
 	grayImage.allocate(640,480);
 	grayBg.allocate(640,480);
 	grayDiff.allocate(640,480);
@@ -37,11 +45,53 @@ void testApp::setup(){
 	cam=ofCamera();
 	cam.setPosition(ofGetWidth()/2,ofGetHeight()/2,-ofGetWidth());
 	cam.lookAt(ofVec3f(ofGetWidth()/2,ofGetHeight()/2,0),ofVec3f(0, -1, 0));
-	cam.setFov(42.0); //39.85); //53.13);
+	cam.setFov(41.0); //39.85); //53.13);
 	
-	trapDoor=trapdoor(ofRectangle(0,ofGetHeight()/4,ofGetHeight(),ofGetHeight()),ofVec2f(20,20));
+	updatePlane();
 	
+	mode=PLAY;
 	
+}
+
+ofVec2f testApp::screen2plane(ofVec2f screenpos){
+	ofVec3f p;
+	ray=projector.castPixel(screenpos.x,screenpos.y); 
+	bool hit = plane.intersect(ray,p);
+	return ofVec2f(p.x,pow(pow(p.y,2)+pow(p.z,2),0.5f));
+}
+ofVec3f testApp::plane2world(ofVec2f planepos){
+	return ofVec3f(planepos.x,planepos.y,0);
+}
+
+void testApp::updatePlane(){
+	plane.setNormal(ofVec3f(0,sin(cam_angle*0.01745329),-cos(cam_angle*0.01745329)));
+	trapDoor=trapdoor(screen2plane(ofVec2f(ofGetWidth(),0)),screen2plane(ofVec2f(ofGetWidth(),ofGetHeight())),ofVec2f(20,20));
+	//create ground mesh with hole for trapdoor
+	ground=ofMesh();
+	ground.addVertex(plane2world(screen2plane(ofVec2f(0,0))));
+	ground.addTexCoord(ofVec2f(0,0));
+	ground.addVertex(plane2world(screen2plane(ofVec2f(ofGetWidth(),0))));
+	ground.addTexCoord(ofVec2f(1,0));
+	ground.addVertex(plane2world(screen2plane(ofVec2f(ofGetWidth(),ofGetHeight()))));
+	ground.addTexCoord(ofVec2f(1,1));
+	ground.addVertex(plane2world(screen2plane(ofVec2f(0,ofGetHeight()))));
+	ground.addTexCoord(ofVec2f(0,1));
+	
+	ground.addTriangle(0,2,1);
+	ground.addTriangle(0,3,2);
+	
+	/*
+	vector<ofVec2f> corners=trapDoor.getCorners();
+	for (int i=0;i<corners.size();i++) {
+		ground.addVertex(plane2world(corners[0]));
+		ground.addTexCoord(trapDoor.bounds2UV(corners[0]));
+	}
+	//join a quad for each side
+	for (int i=0;i<corners.size();i++) {
+		ground.addTriangle(i,i+4,i+1);
+		ground.addTriangle(i+4,(i+5)%ground.getNumVertices(),i+1);
+	}
+	*/
 	
 }
 
@@ -51,21 +101,21 @@ void testApp::update(){
 
     bool bNewFrame = false;
 
-	#ifdef _USE_LIVE_VIDEO
-       vidGrabber.grabFrame();
-	   bNewFrame = vidGrabber.isFrameNew();
-    #else
-        vidPlayer.idleMovie();
-        bNewFrame = vidPlayer.isFrameNew();
-	#endif
+	if (useCamera) {
+	       vidGrabber.grabFrame();
+		bNewFrame = vidGrabber.isFrameNew();
+	}else {
+		vidPlayer.idleMovie();
+		bNewFrame = vidPlayer.isFrameNew();
+	}
 
 	if (bNewFrame){
 
-		#ifdef _USE_LIVE_VIDEO
-            colorImg.setFromPixels(vidGrabber.getPixels(), 320,240);
-	    #else
+	if (useCamera) {
+            colorImg.setFromPixels(vidGrabber.getPixels(), 640,480);
+	}else {
             colorImg.setFromPixels(vidPlayer.getPixels(), 640,480);
-        #endif
+        }
 
         grayImage = colorImg;
 		if (bLearnBakground == true){
@@ -88,115 +138,103 @@ void testApp::update(){
 
 //--------------------------------------------------------------
 void testApp::draw(){
-
-	/*
-	// draw the incoming, the grayscale, the bg and the thresholded difference
-	ofSetHexColor(0xffffff);
-	colorImg.draw(20,20);
-	grayImage.draw(360,20);
-	grayBg.draw(20,280);
-	grayDiff.draw(360,280);
-
-	// then draw the contours:
- */
-	ofFill();
-	ofSetHexColor(0x333333);
-	//ofRect(360,540,320,240);
-	ofSetHexColor(0xa0a0a0);
-
-	// we could draw the whole contour finder
-	//contourFinder.draw(360,540);
-	
-	//keep running background average- how do you use a custom background
-
-
-	colorImg.draw(0,0,ofGetWidth(),ofGetHeight());
-
 	cam.begin();
-	ofPushMatrix();
-
-	ofRotate(cam_angle,1,0,0);
-	//cam.Rotate(cam_angle,1,0,0);
-
-	for (float i=0;i<=ofGetWidth();i+=ofGetWidth()/10) {
-        glBegin(GL_LINES);
-            glVertex3f(i,0,0);
-            glVertex3f(i,ofGetWidth(),0);
-        glEnd();
-        glBegin(GL_LINES);
-            glVertex3f(0,i,0);
-            glVertex3f(ofGetWidth(),i,0);
-        glEnd();
-	}
-
-	ofPopMatrix();
 	
-
-	//ofSphere(pos.x,pos.y,pos.z,5);
-	
-	vector<ofVec3f> newPlayers;
-	float movethresh=10;
-
-	for (int i = 0; i < contourFinder.nBlobs; i++){
-		//do some bounds checking, size threshold and overlap removal
-		//in order to translate blobs into players
-		//attempt to track players - maintain ID no
-		//attempt to base blobs
-		//TODO attempt to estimate bounds shape
-		//project all of this into the 3D space
-		ofRectangle r=contourFinder.blobs[i].boundingRect;
-		
-		ofVec2f blobBase=ofVec2f(r.x+(r.width/2),r.y+r.height-(r.width/2));
-		
-		contourFinder.blobs[i].draw(0,0);
-		ofPoint p=contourFinder.blobs[i].centroid;
-
-		ofSetHexColor(0xffff00);
-		
-		char numStr[16];
-		sprintf(numStr, "%i", i);
-		ofDrawBitmapString(numStr, r.x, r.y);
-		
-		ofVec3f pp;
-		ray=projector.castPixel(blobBase.x,blobBase.y); //(ofGetHeight()-y));
-		bool hit = plane.intersect(ray,pp);	
-
+	switch(mode) {
+		case PLAY:
+			glDisable(GL_DEPTH_TEST);
+			//must be a better way than this
+			colorImg.draw(0,0,ofGetWidth(),ofGetHeight());
+			ofFill();
+			ofSetHexColor(0x000000);
+			ofRect(0,0,ofGetWidth(),ofGetHeight());
+			ofSetHexColor(0xffffff);
+			ofPushMatrix();
+				ofRotate(cam_angle,1,0,0);
+				bindTexture(colorImg);
+				ground.draw();
+				unbindTexture(colorImg);
+				trapDoor.checkUpdate(players);
+				trapDoor.draw();
+			ofPopMatrix();
+		break;
+		case CALIBRATE:
+			ofFill();
+			ofSetHexColor(0x333333);
+			ofSetHexColor(0xa0a0a0);
+			// we could draw the whole contour finder
+			//contourFinder.draw(360,540);
+			//keep running background average- how do you use a custom background
+			ofSetHexColor(0xffffff);
+			colorImg.draw(0,0,ofGetWidth(),ofGetHeight());
 		
-		for (int i=0;i<players.size();i++) {
-			if (abs(players[i].x-pp.x)<10&&abs(players[i].z-pp.z)) {
-				//identified a player
-				//this is flawed: tracking boxes in a skewed axis
-				//how to get UV coords on the plane
-				//maybe rotate the camera instead of the plane
-				
+			ofPushMatrix();
+				ofRotate(cam_angle,1,0,0);
+				for (float i=0;i<=ofGetWidth();i+=ofGetWidth()/10) {
+				glBegin(GL_LINES);
+				    glVertex3f(i,0,0);
+				    glVertex3f(i,ofGetWidth(),0);
+				glEnd();
+				glBegin(GL_LINES);
+				    glVertex3f(0,i,0);
+				    glVertex3f(ofGetWidth(),i,0);
+				glEnd();
+				}
+			ofPopMatrix();
+
+			vector<ofVec3f> newPlayers;
+			float movethresh=10;
+			for (int i = 0; i < contourFinder.nBlobs; i++){
+				//do some bounds checking, size threshold and overlap removal
+				//in order to translate blobs into players
+				//attempt to track players - maintain ID no
+				//attempt to base blobs
+				//TODO attempt to estimate bounds shape
+				//project all of this into the 3D space
+				ofRectangle r=contourFinder.blobs[i].boundingRect;
+				ofVec2f blobBase=ofVec2f(r.x+(r.width/2),r.y+r.height-(r.width/2));
+				contourFinder.blobs[i].draw(0,0);
+				ofPoint p=contourFinder.blobs[i].centroid;
+				ofSetHexColor(0xffff00);
+				char numStr[16];
+				sprintf(numStr, "%i", i);
+				ofDrawBitmapString(numStr, r.x, r.y);
+				ofVec3f pp;
+				ray=projector.castPixel(blobBase.x,blobBase.y); //(ofGetHeight()-y));
+				bool hit = plane.intersect(ray,pp);	
+				for (int i=0;i<players.size();i++) {
+					if (abs(players[i].x-pp.x)<10&&abs(players[i].z-pp.z)) {
+						//identified a player
+						//this is flawed: tracking boxes in a skewed axis
+						//how to get UV coords on the plane
+						//maybe rotate the camera instead of the plane
+						
+					}
+				}
+				ofPushMatrix();
+				ofTranslate(pp);
+				ofRotate(cam_angle,1,0,0);
+				ofBox(0,-10,0,20);
+				//for now, draw in this axis
+				ofPopMatrix();
 			}
-		}
-	
-		ofPushMatrix();
-		ofTranslate(pp);
-		ofRotate(cam_angle,1,0,0);
-		ofBox(0,-10,0,20);
-		//for now, draw in this axis
-		ofPopMatrix();
+			
+			trapDoor.checkUpdate(players);
+			ofPushMatrix();
+				ofRotate(cam_angle,1,0,0);
+				trapDoor.draw();
+			ofPopMatrix();
+			
+			
+			// finally, a report:
+
+			ofSetHexColor(0xffffff);
+			char reportStr[1024];
+			sprintf(reportStr, "threshold %i\nnum blobs found %i, fps: %f", threshold, contourFinder.nBlobs, ofGetFrameRate());
+			ofDrawBitmapString(reportStr, 10, ofGetHeight()-40);
+		break;
 	}
-	
-	trapDoor.checkUpdate(players);
-	ofPushMatrix();
-	ofRotate(cam_angle,1,0,0);
-	//for now, draw in this axis
-	trapDoor.draw();
-	ofPopMatrix();
-	    
 	cam.end();
-
-
-
-	// finally, a report:
-
-	ofSetHexColor(0xffffff);
-	char reportStr[1024];
-	sprintf(reportStr, "threshold %i\nnum blobs found %i, fps: %f", threshold, contourFinder.nBlobs, ofGetFrameRate());
-	ofDrawBitmapString(reportStr, 10, ofGetHeight()-40);
 }
 
 //--------------------------------------------------------------
@@ -216,15 +254,22 @@ void testApp::keyPressed(int key){
 			break;
 		case 'a':
 			cam_angle+=1;
+			updatePlane();
 			break;
 		case 'z':
 			cam_angle-=1;
+			updatePlane();
 			break;
 		case 's':
 			saveSettings("settings.xml");
 			break;
+		case '1':
+			mode=PLAY;
+			break;
+		case '2':
+			mode=CALIBRATE;
+			break;
 	}
-	plane.setNormal(ofVec3f(0,sin(cam_angle*0.01745329),-cos(cam_angle*0.01745329)));
 }
 
 //--------------------------------------------------------------
@@ -249,11 +294,9 @@ void testApp::mousePressed(int x, int y, int button){
 
 //--------------------------------------------------------------
 void testApp::mouseReleased(int x, int y, int button){
-	ray=projector.castPixel(x,y); //+(ofGetHeight()/2)); //(ofGetHeight()-y));
-	bool hit = plane.intersect(ray,pos);
-	//pos=ofVec3f(pos.x*2,pos.y*2,pos.z*2);
-	if (hit) printf("ray:%i,%i hit plane:%f,%f,%f\n",x,y,pos.x,pos.y,pos.z);
-	else printf("ray:%i,%i missed plane\n",x,y);
+	pos=ofVec2f(x,y);
+	//if (hit) printf("ray:%i,%i hit plane:%f,%f,%f\n",x,y,pos.x,pos.y,pos.z);
+	//else printf("ray:%i,%i missed plane\n",x,y);
 
 }