handProject

ofApp.cpp

#include "ofApp.h"


//--------------------------------------------------------------
void ofApp::setup(){

	cam.setFixUpwards(false);
	cam.setCursorDraw(true);
	//cam.setPosition(+100, 800, -100);
	cam.rotate(90, +1, 0, 0);
	cam.setFarClip(10000);

	// Have the sample listener receive events from the controller
	cout << "I SETUPTED" << endl;
	listener.reset();
	controller.addListener(listener); //roughly equive to leap.open();
	
	ofSetVerticalSync(true);
	ofSetLogLevel(OF_LOG_WARNING);
	ofSetCylinderResolution (16, 1, 16);

	cam.setOrientation(ofPoint(0, 0, 0));//-55

	glEnable(GL_DEPTH_TEST);
    glEnable(GL_NORMALIZE);
	glEnable(GL_DEPTH);
	
	bDrawSimple = false;
	bDrawGrid = true; 
	calibrated = false;
	switchYandZ = false;

	lastTagNumber	= 0;
	numPoints		= 0;
	error = -1;

	// hack
	resolution = ofVec2f(1280.0f,720.0f);
	//resolutionWidth= 1280.0f;
	//resolutionHeight = 800.0f;
	throwRatioX= 1.6f;
	throwRatioY= 1.6f;
	lensOffsetX= 0.0f;
	lensOffsetY= -0.5f;
	translationX = 0.0f;
	translationY =0.0f;
	translationZ= 0.0f;
	rotationX= 0.0f;
	rotationY= 0.0f;
	rotationZ= 0.0f;

	position = cam.getPosition();
	throwRatio = 1.62f;
	lensOffset = ofVec2f(0.0f,0.5f);

	projector.setDefaultFar(10000.0f);
	projector.setDefaultNear(50.0f);
	projector.setWidth(resolution.x);
	projector.setHeight(resolution.y);

	this->viewThroughProjector = false;

	resetProjector() ;
	
	if( XML.loadFile("mySettings.xml") ){
		cout << "mySettings.xml loaded!" << endl;
	}else{
		cout << "unable to load mySettings.xml check data/ folder" << endl;
	}
}

//--------------------------------------------------------------
void ofApp::update(){
	// empty
}

//--------------------------------------------------------------
void ofApp::draw(){
	
	ofBackground(40,40,40);
	ofSetColor(255);
	if (calibrated)
	{
		ofDrawBitmapString("error " + ofToString(error) + " translation x " + ofToString(translationX) +
							" y " + ofToString(translationY) + " z " + ofToString(translationZ) +
							" rotation x " +  ofToString(rotationX) + " y " + ofToString(rotationY) +
							" z " + ofToString(rotationZ), 20, 20);		
	}
	ofDrawBitmapString("Press any key to flip display mode", 20, 35);
	ofDrawBitmapString("Press 'r' to restore camera", 20, 50);
	ofDrawBitmapString("Press 'c' to calibrate camera", 20, 65);
	ofDrawBitmapString("Press 'g' to toggle grid", 20, 80);
	ofDrawBitmapString("Press 'p' to toggle view through projector [" + ofToString(this->viewThroughProjector) + "]", 20, 95);
	ofDrawBitmapString("number of calibration point " + ofToString(numPoints), 20, 110);
	
	if (this->viewThroughProjector) {
		this->projector.beginAsCamera();
	} else {
		cam.begin();
	}
	
	// Draw a grid plane.
	if (bDrawGrid){
		ofPushMatrix();
			ofEnableSmoothing();
			ofRotate(90, 0, 0, 1);
			ofSetColor(160,160,160, 100);
			ofDrawGridPlane(200, 10, false);
		ofPopMatrix();
	}
	
	// Get the vector of Hands from ofxLeap.
	vector<Hand> hands = listener.getLeapHands();
	if (hands.size() > 0) {
		
		// For each hand,
		for (int h=0; h<hands.size(); h++){
			
			// Get the current hand
			Hand & hand = hands[h];
			if (hand.isValid())
			{
				drawTheFingersOfHand (hand);
				drawThePalmOfHand (hand);
				drawTheArmOfHand (hand);
			}
		}
	}

	//updateProjector();
	projector.draw();
	drawWorldPoints();

	if (this->viewThroughProjector) {
		this->projector.endAsCamera();
	} else {
		cam.end();
	}

	listener.markFrameAsOld();

	drawCrosshair();
	drawImagePoints();
	

	vector<ofRay>::iterator it;
	for (it = rays.begin(); it != rays.end(); it++)
	{
		it->draw();
	}

	ofDrawBitmapStringHighlight(ofToString(this->cam.getPosition()), 20, 125);
}

//--------------------------------------------------------------
void ofApp::drawTheFingersOfHand (Hand & hand){
	// For each finger in the Hand,
	FingerList fingers = hand.fingers();
	for (int f=0; f<fingers.count(); f++){
		
		// Get the current finger, and it's type (index, thumb, etc.);
		const Finger & finger = fingers[f];
		Finger::Type fingerType = finger.type();
		if (finger.isValid()){
		
			// For every bone (i.e. phalange) in the finger,
			for (int b=0; b<4; b++) {
				
				// Get each bone;
				Bone::Type boneType = static_cast<Bone::Type>(b);
				Bone bone = finger.bone(boneType);
				if (bone.isValid()){
					
					// Don't consider zero-length bones, such as the Thumb's metacarpal.
					float boneLength = bone.length();
					if (boneLength > 0){
						
						// The Leap returns data in millimeters.
						ofPoint bonePtC = getofPoint ( bone.center());
						ofPoint bonePt0 = getofPoint ( bone.prevJoint());
						ofPoint bonePt1 = getofPoint ( bone.nextJoint());
						float boneThickness = bone.width();
						
						if (bDrawSimple){
							// Draw a simple white skeleton.
							ofSetColor(255);
							ofLine(bonePt0, bonePt1);
							//ofDrawSphere(bonePt0, boneThickness * 0.15);
							//ofDrawSphere(bonePt1, boneThickness * 0.15);
							//ofDrawSphere(bonePtC, boneThickness * 0.05);
							
						} else {
							// Draw a colored cylinder with double-sphere caps. SOS
							setColorByFinger (fingerType, boneType);
							drawOrientedCylinder (bonePt0, bonePt1, boneThickness/2.0);
							ofDrawSphere(bonePt0, boneThickness/2.0);
							ofDrawSphere(bonePt1, boneThickness/2.0);
						}
						
					} // end if boneLength
				} // end if bone isValid()
			} // end for each bone
		} //end if finger isValid()
		
		if (bDrawSimple){
			// Draw the fingertip, which is a point within the last phalange.
			
			ofPoint fingerTipPt = getofPoint ( finger.tipPosition() );
			ofSetColor(255,0,0);
			ofDrawSphere(fingerTipPt, finger.width() * 0.15);
		}
		
	} // end for each finger
}



//--------------------------------------------------------------
void ofApp::drawThePalmOfHand (Hand & hand){
	// This draws the palm as a gray region.
	// Collect the palm vertices into an ofMesh.
	ofMesh palmMesh;
	int nVertices = 0;
	float averageBoneWidth = 0;
	
	// For each finger,
	FingerList fingers = hand.fingers();
	for (int f=0; f<fingers.count(); f++){
		
		// Get the current finger, and it's type (index, thumb, etc.);
		const Finger & finger = fingers[f];
		if (finger.isValid()){
			
			Finger::Type fingerType = finger.type();
			Bone bone;
			if (fingerType == Finger::TYPE_THUMB){
				bone = finger.bone(Bone::TYPE_PROXIMAL);
			} else {
				bone = finger.bone(Bone::TYPE_METACARPAL);
			}
			
			// If we've found the bones we want, add their vertices to the mesh.
			if (bone.isValid()){
				float boneLength = bone.length();
				if (boneLength > 0){
					
					ofPoint pt0 = getofPoint ( bone.prevJoint());
					ofPoint pt1 = getofPoint ( bone.nextJoint());
					
					palmMesh.addVertex(pt0);
					palmMesh.addVertex(pt1);
					
					averageBoneWidth += bone.width();
					nVertices += 2;
				}
			}
		}
	}
	averageBoneWidth /= (nVertices/2);
	
	// Render the palm as a triangle strip surface,
	// (optionally) bordered by cylinders.
	if (nVertices > 3){
		ofSetColor(128);
		
		// Draw the palm as a mesh of triangles.
		int nPalmMeshVertices = palmMesh.getNumVertices();
		for (int i=0; i<(nPalmMeshVertices-2); i++){
			palmMesh.addTriangle(i, i+1, i+2);
		}
		palmMesh.drawFaces();
		
		// Add optional cylinders.
		if (!bDrawSimple){
			float rad = averageBoneWidth / 2.0;
			
			if (nPalmMeshVertices == 10){
				for (int i=0; i<4; i++){
					ofVec3f p0 = palmMesh.getVertex( i   *2);
					ofVec3f p1 = palmMesh.getVertex((i+1)*2);
					drawOrientedCylinder (p0, p1, 10);
					ofDrawSphere(p0, rad);
					ofDrawSphere(p1, rad);
				}
				
				for (int i=0; i<4; i++){
					ofVec3f p0 = palmMesh.getVertex( i   *2 + 1);
					ofVec3f p1 = palmMesh.getVertex((i+1)*2 + 1);
					drawOrientedCylinder (p0, p1, 10);
					ofDrawSphere(p0, rad);
					ofDrawSphere(p1, rad);
				}
			}
		}	
	}
}



//--------------------------------------------------------------
void ofApp::drawTheArmOfHand (Hand & hand){
		
	// Draw the wrist and elbow points.
	Arm arm = hand.arm();
	if (arm.isValid()){
		
		ofPoint handPt   = getofPoint ( hand.palmPosition());
		ofPoint handNorm = getofPoint ( hand.palmNormal());
		ofPoint wristPt  = getofPoint ( arm.wristPosition());
		ofPoint elbowPt  = getofPoint ( arm.elbowPosition());
		
		float basisLen = 50.0;
		
		
		if (bDrawSimple){
			ofSetColor(ofColor::white);
			ofDrawSphere(handPt,  8);
			ofDrawSphere(wristPt, 8);
			ofDrawSphere(elbowPt, 8);
			
			ofLine(handPt, wristPt);
			ofLine(wristPt, elbowPt);
			ofLine(handPt, handPt+ basisLen*handNorm);

			
			
			// draw the rotation vectors of the hand.
			{
				Leap::Matrix handMatrix = hand.basis();
				ofPoint handBasisX = getofPoint( handMatrix.xBasis);
				ofPoint handBasisY = getofPoint( handMatrix.yBasis);
				ofPoint handBasisZ = getofPoint( handMatrix.zBasis);
				
				ofSetColor(ofColor::red  );	ofLine(handPt, handPt + basisLen*handBasisX);
				ofSetColor(ofColor::green);	ofLine(handPt, handPt + basisLen*handBasisY);
				ofSetColor(ofColor::blue );	ofLine(handPt, handPt + basisLen*handBasisZ);
				
				// draw the identity of the hand (left or right)
				string whichHandString = "RIGHT";
				if (hand.isLeft()){
					whichHandString = "LEFT";
				}
				// float handConfidence = hand.confidence();
				// whichHandString += " " + ofToString(handConfidence);
				
				ofSetColor(ofColor::white);
				ofDrawBitmapString(whichHandString, (handPt + (basisLen*1.2)*handBasisY));
				
				
			}
			
			// draw the rotation vectors of the arm.
			{
				Leap::Matrix armMatrix = arm.basis();
				ofPoint armBasisX = getofPoint( armMatrix.xBasis);
				ofPoint armBasisY = getofPoint( armMatrix.yBasis);
				ofPoint armBasisZ = getofPoint( armMatrix.zBasis);
				
				ofSetColor(ofColor::red  );	ofLine(wristPt, wristPt + basisLen*armBasisX);
				ofSetColor(ofColor::green);	ofLine(wristPt, wristPt + basisLen*armBasisY);
				ofSetColor(ofColor::blue );	ofLine(wristPt, wristPt + basisLen*armBasisZ);
			}
			
		} else {
		
			// Draw a cylinder between two points, properly oriented in space.
			float armWidth = arm.width();
			float dx = wristPt.x - elbowPt.x;
			float dy = wristPt.y - elbowPt.y;
			float dz = wristPt.z - elbowPt.z;
			float dh = sqrt(dx*dx + dy*dy + dz*dz);
			
			ofPushMatrix();
			{
				ofTranslate( (elbowPt.x+wristPt.x)/2, (elbowPt.y+wristPt.y)/2, (elbowPt.z+wristPt.z)/2 );
			
				float theta =   90 - RAD_TO_DEG * asin(dz/dh);
				float phi   =        RAD_TO_DEG * atan2(dy,dx);
				ofRotate(phi,   0,0,1);
				ofRotate(theta, 0,1,0);
				ofRotate(90,	1,0,0);
				
				// Get the arm Matrix, which provides its orthogonal basis vectors.
				Leap::Matrix armMatrix = arm.basis();
				ofPoint armBasisY = getofPoint( armMatrix.yBasis);
				float ax = armBasisY.x;
				float ay = armBasisY.y;
				float az = armBasisY.z;
				
				// Compute the longitudinal rotation of the arm
				ofNode armBasisYNode;
				armBasisYNode.setPosition(armBasisY);
				armBasisYNode.rotateAround(0-   phi, ofVec3f(0,0,1), ofVec3f(0,0,0));
				armBasisYNode.rotateAround(0- theta, ofVec3f(0,1,0), ofVec3f(0,0,0));
				armBasisYNode.rotateAround(0-    90, ofVec3f(1,0,0), ofVec3f(0,0,0));
				ofPoint newArmBasisY = armBasisYNode.getPosition();
				float armRotation = RAD_TO_DEG * atan2f(newArmBasisY.z, newArmBasisY.x);
			
				ofPushMatrix();
				{
					ofRotate(armRotation, 0,-1,0);
					float armThicknessRatio = 0.6;
					glScalef(armThicknessRatio, 1.0, 1.0);
					ofSetColor(255,0,255);
				
					// Oblate arm cylinder
					ofDrawCylinder (armWidth/2.0, dh);
					
					// Wrist endcap
					ofPushMatrix();
						ofTranslate(ofPoint(0, dh/2,0));
						glScalef(1.0, armThicknessRatio, 1.0);
						ofDrawSphere(armWidth/2.0);
					ofPopMatrix();
						
					// Elbow endcap
					ofPushMatrix();
						ofTranslate(ofPoint(0, -dh/2,0));
						glScalef(1.0, armThicknessRatio, 1.0);
						ofDrawSphere(armWidth/2.0);
					ofPopMatrix();
					
				} // Close popMatrix
				ofPopMatrix();
			} // Close popMatrix
			ofPopMatrix();
		} // Close if !drawSimple
	} // Close if arm isValid
}



//--------------------------------------------------------------
void ofApp::setColorByFinger (Finger::Type fingerType, Bone::Type boneType){
	// Set the current color, according to the type of this finger.
	// Thumb is red, Index is green, etc.
	switch (fingerType){
		case Finger::TYPE_THUMB:
			ofSetColor(ofColor::red);
			break;
		case Finger::TYPE_INDEX:
			ofSetColor(ofColor::green);
			break;
		case Finger::TYPE_MIDDLE:
			ofSetColor(ofColor::blue);
			break;
		case Finger::TYPE_RING:
			ofSetColor(ofColor::yellow);
			break;
		case Finger::TYPE_PINKY:
			ofSetColor(ofColor::cyan);
			break;
		default:
			ofSetColor(ofColor::gray);
			break;
	}
	
	// For the bones inside the palm, set the color to gray.
	bool bSetInternalBonesToGray = true;
	if (bSetInternalBonesToGray){
		if ( (boneType == Bone::TYPE_METACARPAL) ||
			((boneType == Bone::TYPE_PROXIMAL)   && (fingerType == Finger::TYPE_THUMB))) {
			ofSetColor(128);
		}
	}
}



//--------------------------------------------------------------
void ofApp::drawOrientedCylinder (ofPoint pt0, ofPoint pt1, float radius){
	// Draw a cylinder between two points, properly oriented in space.
	float dx = pt1.x - pt0.x;
	float dy = pt1.y - pt0.y;
	float dz = pt1.z - pt0.z;
	float dh = sqrt(dx*dx + dy*dy + dz*dz);
	
	ofPushMatrix();
		ofTranslate( (pt0.x+pt1.x)/2, (pt0.y+pt1.y)/2, (pt0.z+pt1.z)/2 );
		
		ofQuaternion q;
		q.makeRotate (ofPoint(0, -1, 0), ofPoint(dx,dy,dz) );
		ofMatrix4x4 m;
		q.get(m);
		glMultMatrixf(m.getPtr());
		
		ofDrawCylinder (radius, dh);
	ofPopMatrix();

}

//--------------------------------------------------------------
ofPoint ofApp::getIndexFingertip()
{
    vector <Hand> hands = listener.getLeapHands();
    Hand hand;
	if (hands.size() > 0)
    {
		// For each hand,
		for (int h=0; h<hands.size(); h++){
			
			// Get the current hand
			hand = hands[h];
			if (hand.isValid())
            {
                break;
			}
		}
	}
	
	// For each finger in the Hand,
	FingerList fingers = hand.fingers();
	for (int f=0; f<fingers.count(); f++)
    {
		// Get the current finger, and it's type (index, thumb, etc.);
		const Finger & finger = fingers[f];
		Finger::Type fingerType = finger.type();
		if (finger.isValid() && fingerType == Finger::TYPE_INDEX)
        {
            return getofPoint( finger.tipPosition() );
		}
	}
    cout << "FINGER NOT FOUND" << endl;
    return ofPoint(-1.0f,-1.0f,-1.0f);
}

void ofApp::drawCrosshair()
{
	ofPushMatrix();
		ofSetColor(ofColor::white);
		ofSetLineWidth(5.0f);
		ofLine(ofPoint(mouseX,0),ofPoint(mouseX,ofGetScreenHeight()));
		ofLine(ofPoint(0,mouseY),ofPoint(ofGetScreenWidth(),mouseY));
	ofPopMatrix();
}

void ofApp::drawWorldPoints()
{
	ofPushMatrix();
		ofSetColor(ofColor::red);
		for(int i = 0; i < CalibVectorWorld.size(); i++)
		{
			//cout << "DRAW WORLD " << i << endl;
			ofDrawSphere(CalibVectorWorld[i], 5.0f);
		}
	ofPopMatrix();
}

void ofApp::drawImagePoints()
{
	auto count = this->CalibVectorImage.size();
	vector<cv::Point2f> evaluatedImagePoints(count);

	if (this->calibrated) {
		cv::projectPoints(ofxCv::toCv(this->CalibVectorWorld), this->rotation, this->translation, this->camera, this->distortion, evaluatedImagePoints);
	}

	ofPushMatrix();
		ofSetColor(ofColor::white);
		for(int i = 0; i < CalibVectorImage.size(); i++)
		{
			//cout << "DRAW SPHERE x " << CalibVectorImage[i].x << " y " << CalibVectorImage[i].y << i << endl;
			ofDrawSphere(CalibVectorImage[i], 5.0f);

			if (this->calibrated) {
				ofLine(CalibVectorImage[i], ofxCv::toOf(evaluatedImagePoints[i]));
			}
		}
	ofPopMatrix();
}


ofPoint ofApp::getofPoint(Vector v)
{
	return ofPoint(v.x, v.y, v.z); 
}

void ofApp::correctCamera()
{
	//we have to intitialise a basic camera matrix for it to start with (this will get changed by the function call calibrateCamera
	vector<cv::Point2f> imagePoints;
	vector<cv::Point3f> worldPoints;

	if (setCalibVectors())
	{
		for(int i = 0; i < CalibVectorImage.size(); i++) 
		{
			//Point2f toCv(ofVec2f vec);
			//Point3f toCv(ofVec3f vec);
			//projectorPoint.x = ofMap(projectorPoint.x, -1.0f, +1.0f, 0.0f, resolution.x);
			//projectorPoint.y = ofMap(projectorPoint.y, +1.0f, -1.0f, 0.0f, resolution.y);
			imagePoints.push_back(ofxCv::toCv(CalibVectorImage[i]));
			worldPoints.push_back(ofxCv::toCv(CalibVectorWorld[i]));
		}
	}
	else
	{
		cout << "not enough control points" << endl;
		return;
	}


	//initialise matrices
	cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
	cameraMatrix.at<double>(0,0) = resolution.x * 1.62f; // default at 1.4 : 1.0f throw ratio
	cameraMatrix.at<double>(1,1) = resolution.y * 1.62f;
	cameraMatrix.at<double>(0,2) = resolution.x / 2.0f;
	cameraMatrix.at<double>(1,2) = resolution.y * 0.90f; // default at 40% lens offset
	cameraMatrix.at<double>(2,2) = 1;
	cv::Mat distortionCoefficients = cv::Mat::zeros(5, 1, CV_64F);

	vector<cv::Mat> rotations, translations;

	int flags = CV_CALIB_FIX_K1 | CV_CALIB_FIX_K2 | CV_CALIB_FIX_K3 | CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6 | 
		CV_CALIB_ZERO_TANGENT_DIST | CV_CALIB_USE_INTRINSIC_GUESS;
	//if (this->fixAspectRatio) {
		//flags |= CV_CALIB_FIX_ASPECT_RATIO;
	//}
	//cout << " worldPoints " << <<
	error = cv::calibrateCamera(	vector<vector<cv::Point3f>>(1, worldPoints), 
										vector<vector<cv::Point2f>>(1, imagePoints), 
										cv::Size(resolution.x, resolution.y), 
										cameraMatrix, 
										distortionCoefficients, 
										rotations, 
										translations, 
										flags);
	cout << " cameraMatrix " << cameraMatrix << endl;

	/*vector<cv::Point3f> newWorldPoints;
	vector<cv::Point2f> newImagePoints;
	cout << "world point size" << worldPoints.size() << endl;
	for (int i = 0; i < worldPoints.size(); i++)
	{
		newWorldPoints.push_back(cv::Point3f(worldPoints[i].x,worldPoints[i].y,worldPoints[i].z));
		newImagePoints.push_back(cv::Point2f(imagePoints[i].x,imagePoints[i].y));
	}
	cv::solvePnP(	newWorldPoints, 
					newImagePoints, 
					cameraMatrix,
					distortionCoefficients,
					rotations, 
					translations);*/
	//cout << "error calibration rate" << error << endl;
	calibrated = true;
	/*lastTagNumber	= XML.addTag("ERROR");
	if( XML.pushTag("ERROR", lastTagNumber) )
	{
		XML.setValue("ERROR", error, lastTagNumber);
	}
	XML.popTag();
	XML.saveFile("mySettings.xml");*/
	setExtrinsics(rotations[0], translations[0]);
	setIntrinsics(cameraMatrix);

	this->camera = cameraMatrix;
	this->distortion = distortionCoefficients;
	this->rotation = rotations[0];
	this->translation = translations[0];
}

void ofApp::setIntrinsics(cv::Mat cameraMatrix) 
{
	float fovx = cameraMatrix.at<double>(0, 0);
	float fovy = cameraMatrix.at<double>(1, 1);
	float ppx = cameraMatrix.at<double>(0, 2);
	float ppy = cameraMatrix.at<double>(1, 2);

	throwRatioX = fovx / resolution.x;
	throwRatioY = fovy / resolution.y;

	lensOffsetX = (ppx /  resolution.x) - 0.5f; // not sure if this is + or -ve (if wrong, then both this and ofxCvMin::Helpers::makeProjectionMatrix should be switched
	lensOffsetY = (ppy /  resolution.y) - 0.5f;

	//const auto newProjection = ofxCv::makeProjectionMatrix(cameraMatrix, cv::Size(projector.getWidth(), projector.getHeight()));
	const auto newProjection = ofxCv::makeProjectionMatrix(cameraMatrix, cv::Size(resolution.x, resolution.y) );
	projector.setProjection(newProjection);
}

//----------
void ofApp::setExtrinsics(cv::Mat rotation, cv::Mat translation) 
{
	const auto rotationMatrix = ofxCv::makeMatrix(rotation, cv::Mat::zeros(3, 1, CV_64F));
	const auto rotationEuler = rotationMatrix.getRotate().getEuler();

	translationX = translation.at<double>(0);
	translationY = translation.at<double>(1);
	translationZ = translation.at<double>(2);

	rotationX = rotationEuler.x;
	rotationY = rotationEuler.y;
	rotationZ = rotationEuler.z;

	//cam.setGlobalOrientation();
	//updateProjector();
	//projector.setTransformMatrix((ofxCv::makeMatrix(rotation, translation)).getInverse());//.getInverse()
	projector.setView(ofxCv::makeMatrix(rotation, translation));
	//updateProjector();
}

bool ofApp::setCalibVectors()
{
	if (numPoints == 0)
	{
		numPoints = XML.getNumTags("CALIB_READ");
	}
	if(numPoints > 12)
	{
		
		CalibVectorImage.clear();
		CalibVectorWorld.clear();
		for(int i = 0; i < numPoints; i++)
		{
			XML.pushTag("CALIB_READ", i);
			//the last argument of getValue can be used to specify
			//which tag out of multiple tags you are refering to.
			float MX = XML.getValue("MOUSE:X", 0, 0);
			float MY = XML.getValue("MOUSE:Y", 0, 0);
			cout << "MX " << MX << " MY " << MY << endl; 
			CalibVectorImage.push_back(ofVec2f(MX,MY));

			float FX = XML.getValue("FINGER:X", 0, 0);
			float FY = XML.getValue("FINGER:Y", 0, 0);
			float FZ = XML.getValue("FINGER:Z", 0, 0);
			if (switchYandZ)
			{
				CalibVectorWorld.push_back(ofVec3f(FX,FZ,FY));
			}
			else
			{
				CalibVectorWorld.push_back(ofVec3f(FX,FY,FZ));
			}
			XML.popTag();
		}
		

		return true;
	}
	else
	{
		return false;
	}
}

void ofApp::resetProjector() 
{
	//position = ofVec3f(1.0f,1.0f,1.0f);//cam.getPosition();
	throwRatio = 1.62f;
	lensOffset = ofVec2f(0.0f,0.5f);

	ofQuaternion rotation;
	auto rotationQuat = ofQuaternion(rotationX, ofVec3f(1, 0, 0), rotationZ, ofVec3f(0, 0, 1), rotationY, ofVec3f(0, 1, 0));
	ofMatrix4x4 pose = ofMatrix4x4(rotationQuat);
	pose(3,0) = translationX;
	pose(3,1) = translationY;
	pose(3,2) = translationZ;
	projector.setView(pose);

	ofMatrix4x4 projection;
	projection(0,0) = throwRatioX;
	projection(1,1) = -throwRatioY;
	projection(2,3) = 1.0f;
	projection(3,3) = 0.0f;
	projection.postMultTranslate(-lensOffsetX, -lensOffsetY, 0.0f);
	projector.setProjection(projection);

	projector.setWidth(resolution.x);
	projector.setHeight(resolution.y);
}

void ofApp::updateProjector()	
{
	position = ofVec3f(1.0f,1.0f,1.0f);//cam.getPosition();

	//projector.setWidth(resolution.x);
	//projector.setHeight(resolution.y);
	//projector.setProjection(throwRatio, lensOffset);
	//projector.setPosition(position);
	//pixels.clear();
	//for (int i = 0; i < resolution.x; i += 32)
	//{
		//for(int j = 0; j < resolution.y; j += 32)
		//{
			//pixels.push_back(ofVec2f(i, j));
		//}
	//}
	//projector.castPixels(pixels, rays);
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key){
	if (key == 'r'){
		// Reset the camera if the user presses 'c'.
		cam.reset();
	} else if (key == 'g'){
		// flip whether or not we're drawing a grid.
		bDrawGrid = !bDrawGrid;
	} else if (key == 'c')
    {
		correctCamera();
	} else if (key == 'z')
	{
		switchYandZ = !switchYandZ;
		correctCamera();
	} else if (key == 'f') {
		ofToggleFullscreen();
	} else if (key == 'p') {
		this->viewThroughProjector = !this->viewThroughProjector;
	} else if (key == OF_KEY_LEFT) {
		this->projector.pan(-1.0f);
	} else if (key == OF_KEY_RIGHT) {
		this->projector.pan(+1.0f);
	} else {
		// When the user presses a key, flip the rendering mode.
		bDrawSimple = !bDrawSimple;
	}
}

//--------------------------------------------------------------
void ofApp::keyReleased(int key){

}

//--------------------------------------------------------------
void ofApp::mouseMoved(int x, int y ){

}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button)
{

}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button)
{
	if (button == OF_MOUSE_BUTTON_MIDDLE)
	{
		mouseCoord.set(mouseX,mouseY);
		fingerCoord.set(getIndexFingertip());
		if (fingerCoord == ofVec3f(-1.0f,-1.0f,-1.0f))
		{
			cout << "NOTHING RECORDED" << endl;
			return;
		}
		cout << "mouseCoord x " << mouseCoord.x << " y "<< mouseCoord.y << " fingerCoord x " << fingerCoord.x << " y " << fingerCoord.y << " z " << fingerCoord.z <<  endl;
		numPoints++;
		lastTagNumber	= XML.addTag("CALIB_READ");
		if( XML.pushTag("CALIB_READ", lastTagNumber) )
		{
			//now we will add a pt tag - with two
			//children - X and Y
			int tagNum = XML.addTag("MOUSE");
			XML.setValue("MOUSE:X", mouseCoord.x, tagNum);
			XML.setValue("MOUSE:Y", mouseCoord.y, tagNum);
			
			tagNum = XML.addTag("FINGER");
			XML.setValue("FINGER:X", fingerCoord.x, tagNum);
			XML.setValue("FINGER:Y", fingerCoord.y, tagNum);
			XML.setValue("FINGER:Z", fingerCoord.z, tagNum);
			XML.popTag();
		}
		XML.popTag();
		XML.saveFile("mySettings.xml");
	}
	correctCamera();
	//cout << "END OF SAVE" << endl;
}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void ofApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void ofApp::dragEvent(ofDragInfo dragInfo){ 

}

//--------------------------------------------------------------
void ofApp::exit(){
	cout << "I EXITED" << endl;
	listener.reset();
	controller.removeListener(listener);
}

ofApp.h

#pragma once

#include "ofMain.h"
//#include "ofxCv.h"
#include "ofxXmlSettings.h"
#include "Leap.h"
#include "LeapListener.h"
#include "ofxStrip.h"
#include "ofVectorMath.h"
#include "ofxGrabCam.h"
#include "ofxRay.h"
#include "ofxCvMin.h"

using namespace Leap;

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();
		void exit();

		//
		ofPoint getIndexFingertip();
		ofPoint getofPoint(Vector v);
		void drawCrosshair();
		void drawWorldPoints();
		void drawImagePoints();
		void correctCamera();
		void setExtrinsics(cv::Mat rotation, cv::Mat translation);
		void setIntrinsics(cv::Mat cameraMatrix);
		void resetProjector(); 
		bool setCalibVectors();
		void updateProjector();
		//

		void keyPressed(int key);
		void keyReleased(int key);
		void mouseMoved(int x, int y );
		void mouseDragged(int x, int y, int button);
		void mousePressed(int x, int y, int button);
		void mouseReleased(int x, int y, int button);
		void windowResized(int w, int h);
		void dragEvent(ofDragInfo dragInfo);
		void gotMessage(ofMessage msg);
	private:
		LeapListener listener;
		Controller controller;

		vector <int> fingersFound; 
		ofxGrabCam cam;
		//
		ofProjector projector;
		ofProjector projectorIdeal;
		vector<ofVec2f> pixels;
		vector<ofRay> rays;
		ofVec3f position;
		ofVec2f resolution;
		float throwRatio;
		ofVec2f lensOffset;
		//
	
		bool bDrawGrid; 
		bool bDrawSimple; 
		void drawOrientedCylinder (ofPoint pt0, ofPoint pt1, float radius);
		void setColorByFinger (Finger::Type fingerType, Bone::Type boneType);
		void drawTheFingersOfHand (Hand & hand);
		void drawThePalmOfHand (Hand & hand);
		void drawTheArmOfHand (Hand & hand);

		// xml calib
		ofPoint mouseCoord;
		ofPoint fingerCoord;
		ofxXmlSettings XML;
		int lastTagNumber;
		// calib
		float resolutionWidth, resolutionHeight;
		float throwRatioX, throwRatioY;
		float lensOffsetX, lensOffsetY;
		float translationX, translationY, translationZ;
		float rotationX, rotationY, rotationZ;

		vector<ofVec2f> CalibVectorImage;
		vector<ofVec3f> CalibVectorWorld;
		int numPoints;
		float error;
		bool calibrated;
		bool switchYandZ;

		cv::Mat camera, distortion;
		cv::Mat rotation, translation;
		bool viewThroughProjector;
};