//--------------------------------------------------------------
void LeapVisualizer::drawFinger (const Finger & finger,ofxLeapMotion & leap){
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.
if (bone.length() > 0){
drawBone (finger, bone,leap);
} // end if boneLength
} // end if bone isValid()
} // end for each bone
if (bDrawSimple){
// Draw the fingertip, which is an extra point within the last phalange.
ofSetColor(ofColor::white);
ofPoint fingerTipPt = leap.getofPoint ( finger.tipPosition() );
ofDrawSphere(fingerTipPt, finger.width() * 0.05);
}
}
}
void display() {
glClear(GL_COLOR_BUFFER_BIT);
glColor3f(0.f,0.f,0.f);
drawBone(bones[0], true);
glFlush();
glutPostRedisplay();
}
void HumanBodyModelRenderer::draw(const FrameParameters& posture, const DblVector3D& displacement)
{
if( posture._boneCount <= 0 )
return;
glPushMatrix();
// draw the default pose
const float globalScaleFactor = _m->scale();
glScalef(globalScaleFactor, globalScaleFactor, globalScaleFactor);
glTranslatef(displacement.x(), displacement.y(), displacement.z());
BoneSegment* root = _m->_boneSegments[0];
glTranslatef(root->_pos.x(), root->_pos.y(), root->_pos.z());
glRotatef(root->_orient[0], 1, 0, 0);
glRotatef(root->_orient[1], 0, 1, 0);
glRotatef(root->_orient[2], 0, 0, 1);
const FrameParameters& pose = posture;
// apply the root pose
BoneSegmentParameter& boneParam = pose._boneParams[0];
// root must have 6 parameters, tx ty tz and rx ry rz
assert( boneParam._numParams == 6 );
glTranslatef(boneParam._params[0],
boneParam._params[1],
boneParam._params[2]);
glRotatef(boneParam._params[5], 0, 0, 1);
glRotatef(boneParam._params[4], 0, 1, 0);
glRotatef(boneParam._params[3], 1, 0, 0);
// render the model using DFS
drawBone( root, posture );
glRotatef(-root->_orient[2], 0, 0, 1);
glRotatef(-root->_orient[1], 0, 1, 0);
glRotatef(-root->_orient[0], 1, 0, 0);
glTranslatef(-root->_pos.x(), -root->_pos.y(), -root->_pos.z());
glPopMatrix();
}
/// <summary>
/// Draw skeleton.
/// </summary>
/// <param name="skeletonData">Skeleton coordinates</param>
/// <param name="imageRect">The rect which the color or depth stream image is streched to fit</param>
void UKinect::drawSkeleton(const NUI_SKELETON_DATA& skeletonData) {
// Torso
drawBone(skeletonData, NUI_SKELETON_POSITION_HEAD, NUI_SKELETON_POSITION_SHOULDER_CENTER);
drawBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SHOULDER_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SHOULDER_RIGHT);
drawBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SPINE);
drawBone(skeletonData, NUI_SKELETON_POSITION_SPINE, NUI_SKELETON_POSITION_HIP_CENTER);
drawBone(skeletonData, NUI_SKELETON_POSITION_HIP_CENTER, NUI_SKELETON_POSITION_HIP_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_HIP_CENTER, NUI_SKELETON_POSITION_HIP_RIGHT);
// Left arm
drawBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_LEFT, NUI_SKELETON_POSITION_ELBOW_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_ELBOW_LEFT, NUI_SKELETON_POSITION_WRIST_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_WRIST_LEFT, NUI_SKELETON_POSITION_HAND_LEFT);
// Right arm
drawBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_RIGHT, NUI_SKELETON_POSITION_ELBOW_RIGHT);
drawBone(skeletonData, NUI_SKELETON_POSITION_ELBOW_RIGHT, NUI_SKELETON_POSITION_WRIST_RIGHT);
drawBone(skeletonData, NUI_SKELETON_POSITION_WRIST_RIGHT, NUI_SKELETON_POSITION_HAND_RIGHT);
// Left leg
drawBone(skeletonData, NUI_SKELETON_POSITION_HIP_LEFT, NUI_SKELETON_POSITION_KNEE_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_KNEE_LEFT, NUI_SKELETON_POSITION_ANKLE_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_ANKLE_LEFT, NUI_SKELETON_POSITION_FOOT_LEFT);
// Right leg
drawBone(skeletonData, NUI_SKELETON_POSITION_HIP_RIGHT, NUI_SKELETON_POSITION_KNEE_RIGHT);
drawBone(skeletonData, NUI_SKELETON_POSITION_KNEE_RIGHT, NUI_SKELETON_POSITION_ANKLE_RIGHT);
drawBone(skeletonData, NUI_SKELETON_POSITION_ANKLE_RIGHT, NUI_SKELETON_POSITION_FOOT_RIGHT);
// Draw joints
for (int i = 0; i < NUI_SKELETON_POSITION_COUNT; i++) {
drawJoint(skeletonData, (NUI_SKELETON_POSITION_INDEX)i);
}
}
/* recursively draws each bone, then its children */
void drawBone(struct Bone *bone, bool isRoot) {
// // rotate entire structure
// float newAngle_z = bone->angle_z + rotDelta;
// if (newAngle_z >= bone->minAngle_z && newAngle_z <= bone->maxAngle_z) {
// bone->angle_z = newAngle_z;
// }
// // translate entire structure
// if(isRoot) {
// bone->x0 += transDelta;
// bone->y0 += transDelta;
// }
glPushMatrix();
glTranslatef(bone->x0, bone->y0, bone->z0);
glRotatef(bone->angle_x, 1.0, 0.0, 0.0);
glRotatef(bone->angle_y, 0.0, 1.0, 0.0);
glRotatef(bone->angle_z, 0.0, 0.0, 1.0);
// // makes bones as lines
// glBegin(GL_LINES);
// glVertex2f(0.0, 0.0);
// glVertex2f(bone->xdim, 0.0);
// glEnd();
// makes bones as cuboids
glColor3f(bone->color.r, bone->color.g, bone->color.b);
glBegin(GL_QUADS);
glVertex3f(0.0, -0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(0.0, 0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(bone->xdim, 0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(bone->xdim, -0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(0.0, -0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(0.0, 0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(bone->xdim, 0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(bone->xdim, -0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(0.0, 0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(0.0, 0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(bone->xdim, 0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(bone->xdim, 0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(0.0, -0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(0.0, -0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(bone->xdim, -0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(bone->xdim, -0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(0.0, 0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(0.0, 0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(0.0, -0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(0.0, -0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(bone->xdim, 0.5*bone->ydim, -0.5*bone->zdim);
glVertex3f(bone->xdim, 0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(bone->xdim, -0.5*bone->ydim, 0.5*bone->zdim);
glVertex3f(bone->xdim, -0.5*bone->ydim, -0.5*bone->zdim);
glEnd();
glTranslatef(bone->xdim, 0.0, 0.0);
for(int i = 0; i < bone->children.size(); i++) {
drawBone(bone->children[i], false);
}
glPopMatrix();
}
void HumanBodyModelRenderer::drawBone(BoneSegment *bone)
{
// draw the bone, then draw all its children recursively
// ignore the root node
glPushMatrix();
if( bone->_id > 0 )
{
glMultMatrixd(bone->_rot_parent_to_self.rawData());
if( 0 )
drawAxis();
double endX, endY, endZ;
endX = bone->_length * bone->_dir.x();
endY = bone->_length * bone->_dir.y();
endZ = bone->_length * bone->_dir.z();
glPushMatrix();
if( _frameIdx >= 0)
{
FrameParameters& pose = _m->_frames[_frameIdx];
// apply the root pose
BoneSegmentParameter& boneParam = pose._boneParams[bone->_id];
if( bone->_dof != boneParam._numParams )
throw "amc file contains error! cannot display animation!";
for(int i=bone->_dof - 1;i>=0;i--)
{
string dofname = bone->_dofName[i];
#if 1
if(dofname == "rx") glRotatef(boneParam._params[i], 1., 0., 0.);
if(dofname == "ry") glRotatef(boneParam._params[i], 0., 1, 0.);
if(dofname == "rz") glRotatef(boneParam._params[i], 0., 0., 1.);
#else
// for testing purpose
if(dofname == "rx") glMultMatrixd(makeXRotationMatrix_deg(boneParam._params[i]).transposed().rawData());
if(dofname == "ry") glMultMatrixd(makeYRotationMatrix_deg(boneParam._params[i]).transposed().rawData());
if(dofname == "rz") glMultMatrixd(makeZRotationMatrix_deg(boneParam._params[i]).transposed().rawData());
#endif
}
}
#if 0
glPushMatrix();
// draw the joint
glColor4f(1, 0, 0, 1);
GLfloat joint_diffuse[] = {1, 0.75, 0.65, 1.0};
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, joint_diffuse);
double radius = powf(bone->_length / 2.0, 0.25);
glutSolidSphere(radius, 20, 20);
// draw the bone
glColor4f(0.65, 0.75, 1.0, 1.0);
GLfloat bone_diffuse[] = {0.7, 1.0, 0.85, 1.0};
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, bone_diffuse);
drawTube(0, 0, 0, endX, endY, endZ, 0.75 * radius, 40, 40);
glPopMatrix();
#else
glCallList( _dplist[bone->_id] );
#endif
glTranslatef(endX, endY, endZ);
}
for(int i=0;i<bone->_numChildren;i++)
{
BoneSegment* child = _m->_boneSegments[bone->_children[i]];
drawBone( child );
}
glPopMatrix();
glPopMatrix();
}
void HumanBodyModelRenderer::draw(int frameIdx)
{
if( !_m )
{
cout << "no model loaded." << endl;
return;
}
_frameIdx = frameIdx;
// render the model
if( _frameIdx < 0 )
{
glPushMatrix();
// draw the default pose
const float globalScaleFactor = _m->scale();
glScalef(globalScaleFactor, globalScaleFactor, globalScaleFactor);
BoneSegment* root = _m->_boneSegments[0];
glTranslatef(root->_pos.x(), root->_pos.y(), root->_pos.z());
glRotatef(root->_orient[0], 1, 0, 0);
glRotatef(root->_orient[1], 0, 1, 0);
glRotatef(root->_orient[2], 0, 0, 1);
// render the model using DFS
drawBone( root );
glRotatef(-root->_orient[2], 0, 0, 1);
glRotatef(-root->_orient[1], 0, 1, 0);
glRotatef(-root->_orient[0], 1, 0, 0);
glTranslatef(-root->_pos.x(), -root->_pos.y(), -root->_pos.z());
glPopMatrix();
}
else
{
// find the pose parameters and render the pose
glPushMatrix();
// draw the default pose
const float globalScaleFactor = _m->scale();
glScalef(globalScaleFactor, globalScaleFactor, globalScaleFactor);
BoneSegment* root = _m->_boneSegments[0];
glTranslatef(root->_pos.x(), root->_pos.y(), root->_pos.z());
glRotatef(root->_orient[0], 1, 0, 0);
glRotatef(root->_orient[1], 0, 1, 0);
glRotatef(root->_orient[2], 0, 0, 1);
FrameParameters& pose = _m->_frames[frameIdx];
// apply the root pose
BoneSegmentParameter& boneParam = pose._boneParams[0];
// root must have 6 parameters, tx ty tz and rx ry rz
assert( boneParam._numParams == 6 );
glTranslatef(boneParam._params[0],
boneParam._params[1],
boneParam._params[2]);
glRotatef(boneParam._params[5], 0, 0, 1);
glRotatef(boneParam._params[4], 0, 1, 0);
glRotatef(boneParam._params[3], 1, 0, 0);
// render the model using DFS
drawBone( root );
glRotatef(-root->_orient[2], 0, 0, 1);
glRotatef(-root->_orient[1], 0, 1, 0);
glRotatef(-root->_orient[0], 1, 0, 0);
glTranslatef(-root->_pos.x(), -root->_pos.y(), -root->_pos.z());
glPopMatrix();
}
}
