//--------------------------------------------------------------
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
}
