Esempio n. 1
0
//--------------------------------------------------------------
void testApp::recordArmXML (Hand & hand){
	
	if (bRecordingThisFrame){
		Arm arm = hand.arm();
		if (arm.isValid()){
			
			float armWidth = arm.width();
			XML.setValue("AW", armWidth, lastTagNumber);
			
			ofPoint palmPt   = leap.getofPoint ( hand.palmPosition());
			ofPoint wristPt  = leap.getofPoint ( arm.wristPosition());
			ofPoint elbowPt  = leap.getofPoint ( arm.elbowPosition());
			ofPoint palmNorm = leap.getofPoint ( hand.palmNormal());
			
			int palmPtTagNum = XML.addTag("PM");
			XML.setValue("PM:X", palmPt.x, palmPtTagNum);
			XML.setValue("PM:Y", palmPt.y, palmPtTagNum);
			XML.setValue("PM:Z", palmPt.z, palmPtTagNum);
			
			int wristPtTagNum = XML.addTag("W");
			XML.setValue("W:X", wristPt.x, wristPtTagNum);
			XML.setValue("W:Y", wristPt.y, wristPtTagNum);
			XML.setValue("W:Z", wristPt.z, wristPtTagNum);
			
			int elbowPtTagNum = XML.addTag("E");
			XML.setValue("E:X", elbowPt.x, elbowPtTagNum);
			XML.setValue("E:Y", elbowPt.y, elbowPtTagNum);
			XML.setValue("E:Z", elbowPt.z, elbowPtTagNum);
			
			int palmNormTagNum = XML.addTag("PN");
			XML.setValue("PN:X", palmNorm.x, palmNormTagNum);
			XML.setValue("PN:Y", palmNorm.y, palmNormTagNum);
			XML.setValue("PN:Z", palmNorm.z, palmNormTagNum);
			
			
			//---------------
			// Export the hand basis matrix
			Leap::Matrix handMatrix = hand.basis();
			ofPoint handBasisX = leap.getofPoint( handMatrix.xBasis);
			ofPoint handBasisY = leap.getofPoint( handMatrix.yBasis);
			ofPoint handBasisZ = leap.getofPoint( handMatrix.zBasis);
			
			int handMatrixTagNum = XML.addTag("HM");
			if( XML.pushTag("HM", handMatrixTagNum) ){
				
				XML.setValue("XX", handBasisX.x, handMatrixTagNum);
				XML.setValue("XY", handBasisX.y, handMatrixTagNum);
				XML.setValue("XZ", handBasisX.z, handMatrixTagNum);
				
				XML.setValue("YX", handBasisY.x, handMatrixTagNum);
				XML.setValue("YY", handBasisY.y, handMatrixTagNum);
				XML.setValue("YZ", handBasisY.z, handMatrixTagNum);
				
				XML.setValue("ZX", handBasisZ.x, handMatrixTagNum);
				XML.setValue("ZY", handBasisZ.y, handMatrixTagNum);
				XML.setValue("ZZ", handBasisZ.z, handMatrixTagNum);
				
				XML.popTag(); // pop HM (Hand Matrix)
			}
			
			//---------------
			// Export the arm basis matrix
			Leap::Matrix armMatrix = arm.basis();
			ofPoint armBasisX = leap.getofPoint( armMatrix.xBasis);
			ofPoint armBasisY = leap.getofPoint( armMatrix.yBasis);
			ofPoint armBasisZ = leap.getofPoint( armMatrix.zBasis);
			
			int armMatrixTagNum = XML.addTag("AM");
			if( XML.pushTag("AM", armMatrixTagNum) ){
				
				XML.setValue("XX", armBasisX.x, armMatrixTagNum);
				XML.setValue("XY", armBasisX.y, armMatrixTagNum);
				XML.setValue("XZ", armBasisX.z, armMatrixTagNum);
				
				XML.setValue("YX", armBasisY.x, armMatrixTagNum);
				XML.setValue("YY", armBasisY.y, armMatrixTagNum);
				XML.setValue("YZ", armBasisY.z, armMatrixTagNum);
				
				XML.setValue("ZX", armBasisZ.x, armMatrixTagNum);
				XML.setValue("ZY", armBasisZ.y, armMatrixTagNum);
				XML.setValue("ZZ", armBasisZ.z, armMatrixTagNum);
				
				XML.popTag(); // pop AM (Arm Matrix)
			}
			
		}
	}
}
//--------------------------------------------------------------
void LeapVisualizer::drawArm (Hand & hand,ofxLeapMotion & leap){
	
	// Draw the wrist and elbow points.
	Arm arm = hand.arm();
	if (arm.isValid()){
		
		ofPoint handPt   = leap.getofPoint ( hand.palmPosition());
		ofPoint handNorm = leap.getofPoint ( hand.palmNormal());
		ofPoint wristPt  = leap.getofPoint ( arm.wristPosition());
		ofPoint elbowPt  = leap.getofPoint ( arm.elbowPosition());
		
		float basisLen = 50.0;
		
		
		if (bDrawSimple){
			ofSetColor(ofColor::white);
			ofDrawSphere(handPt,  8.0);
			ofDrawSphere(wristPt, 8.0);
			ofDrawSphere(elbowPt, 8.0);
			
			ofLine(handPt, wristPt);
			ofLine(wristPt, elbowPt);
			ofLine(handPt, handPt+ basisLen*handNorm);
			ofDrawSphere(handPt+ basisLen*handNorm, 2.0);
			
			// draw the rotation vectors of the hand.
			{
				Leap::Matrix handMatrix = hand.basis();
				ofPoint handBasisX = leap.getofPoint( handMatrix.xBasis);
				ofPoint handBasisY = leap.getofPoint( handMatrix.yBasis);
				ofPoint handBasisZ = leap.getofPoint( handMatrix.zBasis);
				
				glLineWidth(2.0);
				ofSetColor(ofColor::red  );	ofLine(handPt, handPt + basisLen*handBasisX);
				ofSetColor(ofColor::green);	ofLine(handPt, handPt + basisLen*handBasisY);
				ofSetColor(ofColor::blue );	ofLine(handPt, handPt + basisLen*handBasisZ);
				glLineWidth(1.0);
				
				// 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 = leap.getofPoint( armMatrix.xBasis);
				ofPoint armBasisY = leap.getofPoint( armMatrix.yBasis);
				ofPoint armBasisZ = leap.getofPoint( armMatrix.zBasis);
				
				glLineWidth(2.0);
				ofSetColor(ofColor::red  );	ofLine(wristPt, wristPt + basisLen*armBasisX);
				ofSetColor(ofColor::green);	ofLine(wristPt, wristPt + basisLen*armBasisY);
				ofSetColor(ofColor::blue );	ofLine(wristPt, wristPt + basisLen*armBasisZ);
				glLineWidth(1.0);
			}
			
		} 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 = leap.getofPoint( armMatrix.yBasis);
				float ax = armBasisY.x;
				float ay = armBasisY.y;
				float az = armBasisY.z;
				
				// Compute the longitudinal rotation of the arm.
				// Sheesh, I really need to learn 3D matrix math.
				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(ofColor::magenta);
					
					// 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
}