double PoseIK::energyFunc( const math::vectorN& d ) { target_pose->copy( source_pose ); target_pose->addDisplacement( skeleton, d ); // double dist = 0.0f; vector dv, dq, dc; quater q; JointConstraint* joint_constraint = 0; for( unsigned int i=0; i < NUM_JOINTS_IN_HUMAN; i++ ) { Joint* joint = skeleton->getHumanJoint( i ); if( !joint ) continue; bool is_constrained = pose_constraint->getConstraint( i, &joint_constraint ); if( !is_constrained ) continue; unsigned int joint_index = joint->getIndex(); unsigned int joint_dof = joint->getDOF(); math::transq jT = target_pose->getGlobalTransform( skeleton, joint_index ); math::transq cT = joint_constraint->getTransform(); switch( joint_constraint->getType() ) { case JointConstraint::Type::POSITION : { dv = jT.translation - cT.translation; dist += dv % dv; } break; case JointConstraint::Type::ORIENTATION : { dv = difference( jT.rotation, cT.rotation ); dist += dv % dv; } break; case JointConstraint::Type::TRANSQ : { dv = jT.translation - cT.translation; dist += dv % dv; dv = difference( jT.rotation, cT.rotation ); dist += dv % dv; } break; default : { assert( false ); } break; } } if( IsDampingEnabled ) { unsigned int jj=0; for( unsigned int j=0; j < NUM_JOINTS_IN_HUMAN; j++ ) { Joint* joint = skeleton->getHumanJoint( j ); if( joint ) { double c = DampingCoeff[ j+1 ]; unsigned int joint_dof = joint->getDOF(); switch( joint_dof ) { case 6 : { dist += c*d[jj]*d[jj]; jj++; dist += c*d[jj]*d[jj]; jj++; dist += c*d[jj]*d[jj]; jj++; dist += c*d[jj]*d[jj]; jj++; dist += c*d[jj]*d[jj]; jj++; dist += c*d[jj]*d[jj]; jj++; } break; case 3 : { dist += c*d[jj]*d[jj]; jj++; dist += c*d[jj]*d[jj]; jj++; dist += c*d[jj]*d[jj]; jj++; } break; case 1 : { dist += c*d[jj]*d[jj]; jj++; } break; default : break; } } } } return dist; }
double PoseIK::computeJacobian() { double dist = 0.0f; transq t; vector dv, dq, dc; vector endTrans; quater endRot; unsigned int i, j; vector w1, w2, w3; num_dof = skeleton->calcDOF(); num_equations = MAX( pose_constraint->getDOC(), num_dof ); num_unknowns = num_dof; J.setSize( num_equations, num_unknowns ); b.setSize( num_equations ); for( i=0; i < num_equations; i++ ) { b[i] = 0.0; for( j=0; j < num_unknowns; j++ ) { J[i][j] = 0.0; } } unsigned int ii = 0; JointConstraint* joint_constraint = 0; // for( i=0; i < NUM_JOINTS_IN_HUMAN; i++ ) { bool is_constrained = pose_constraint->getConstraint( i, &joint_constraint ); if( !is_constrained ) { continue; } Joint* joint = skeleton->getHumanJoint( i ); unsigned int joint_id = joint->getIndex(); unsigned int constraint_type = joint_constraint->getType(); t = target_pose->getGlobalTransform( skeleton, joint_id ); endTrans = t.translation; endRot = t.rotation; unsigned int temp = ii; switch( constraint_type ) { case JointConstraint::Type::POSITION : { dv = joint_constraint->getTransform().translation - endTrans; dist += dv % dv; b[ii++] = dv.x(); b[ii++] = dv.y(); b[ii++] = dv.z(); } break; case JointConstraint::Type::ORIENTATION : { dq = math::difference( joint_constraint->getTransform().rotation, endRot ); dist += dq % dq; b[ii++] = dq.x(); b[ii++] = dq.y(); b[ii++] = dq.z(); } break; case JointConstraint::Type::TRANSQ : { dv = joint_constraint->getTransform().translation - endTrans; dist += dv % dv; b[ii++] = dv.x(); b[ii++] = dv.y(); b[ii++] = dv.z(); dq = math::difference( joint_constraint->getTransform().rotation, endRot ); dist += dq % dq; b[ii++] = dq.x(); b[ii++] = dq.y(); b[ii++] = dq.z(); } break; default : break; } // calc derivatives of all ancestors of this constrained joint unsigned int jj = 0; for( j=0; j < NUM_JOINTS_IN_HUMAN; j++ ) { Joint* other_joint = skeleton->getHumanJoint( j ); if( !other_joint ) { continue; } unsigned int other_id = other_joint->getIndex(); unsigned int other_dof = other_joint->getDOF(); if( skeleton->isAncestor( other_id, joint_id ) ) { unsigned int iii = temp; switch( other_dof ) { case 6 : { switch( joint_constraint->getType() ) { case JointConstraint::Type::POSITION : { J[iii ][jj ] = 1; J[iii+1][jj+1] = 1; J[iii+2][jj+2] = 1; t = target_pose->getGlobalTransform( skeleton, other_id ); dv = endTrans - t.translation; w1 = vector(1,0,0) * t * dv; w2 = vector(0,1,0) * t * dv; w3 = vector(0,0,1) * t * dv; J[iii ][jj+3] = w1[0]; J[iii ][jj+4] = w2[0]; J[iii ][jj+5] = w3[0]; J[iii+1][jj+3] = w1[1]; J[iii+1][jj+4] = w2[1]; J[iii+1][jj+5] = w3[1]; J[iii+2][jj+3] = w1[2]; J[iii+2][jj+4] = w2[2]; J[iii+2][jj+5] = w3[2]; iii += 3; break; } case JointConstraint::Type::ORIENTATION : { J[iii ][jj+3] = 1; J[iii+1][jj+4] = 1; J[iii+2][jj+5] = 1; break; } case JointConstraint::Type::TRANSQ : { J[iii ][jj ] = 1; J[iii+1][jj+1] = 1; J[iii+2][jj+2] = 1; t = target_pose->getGlobalTransform( skeleton, other_id ); dv = endTrans - t.translation; w1 = vector(1,0,0) * t * dv; w2 = vector(0,1,0) * t * dv; w3 = vector(0,0,1) * t * dv; J[iii ][jj+3] = w1[0]; J[iii ][jj+4] = w2[0]; J[iii ][jj+5] = w3[0]; J[iii+1][jj+3] = w1[1]; J[iii+1][jj+4] = w2[1]; J[iii+1][jj+5] = w3[1]; J[iii+2][jj+3] = w1[2]; J[iii+2][jj+4] = w2[2]; J[iii+2][jj+5] = w3[2]; iii += 3; J[iii ][jj+3] = 1; J[iii+1][jj+4] = 1; J[iii+2][jj+5] = 1; break; } default : break; } } break; case 3 : { switch( joint_constraint->getType() ) { case JointConstraint::Type::POSITION : { t = target_pose->getGlobalTransform( skeleton, other_id ); dv = endTrans - t.translation; w1 = vector(1,0,0) * t * dv; w2 = vector(0,1,0) * t * dv; w3 = vector(0,0,1) * t * dv; J[iii ][jj ] = w1[0]; J[iii ][jj+1] = w2[0]; J[iii ][jj+2] = w3[0]; J[iii+1][jj ] = w1[1]; J[iii+1][jj+1] = w2[1]; J[iii+1][jj+2] = w3[1]; J[iii+2][jj ] = w1[2]; J[iii+2][jj+1] = w2[2]; J[iii+2][jj+2] = w3[2]; iii += 3; break; } case JointConstraint::Type::ORIENTATION : { J[iii ][jj ] = 1; J[iii+1][jj+1] = 1; J[iii+2][jj+2] = 1; break; } case JointConstraint::Type::TRANSQ : { t = target_pose->getGlobalTransform( skeleton, other_id ); dv = endTrans - t.translation; w1 = vector(1,0,0) * t * dv; w2 = vector(0,1,0) * t * dv; w3 = vector(0,0,1) * t * dv; J[iii ][jj ] = w1[0]; J[iii ][jj+1] = w2[0]; J[iii ][jj+2] = w3[0]; J[iii+1][jj ] = w1[1]; J[iii+1][jj+1] = w2[1]; J[iii+1][jj+2] = w3[1]; J[iii+2][jj ] = w1[2]; J[iii+2][jj+1] = w2[2]; J[iii+2][jj+2] = w3[2]; iii += 3; J[iii ][jj ] = 1; J[iii+1][jj+1] = 1; J[iii+2][jj+2] = 1; break; } default : break; } } break; case 1 : { switch( joint_constraint->getType() ) { case JointConstraint::Type::POSITION : { t = target_pose->getGlobalTransform( skeleton, other_id ); dv = vector(1,0,0) * t * ( endTrans - t.translation ); J[iii ][jj] = dv[0]; J[iii+1][jj] = dv[1]; J[iii+2][jj] = dv[2]; iii += 3; break; } case JointConstraint::Type::ORIENTATION : { J[iii ][jj] = 1; J[iii+1][jj] = 0; J[iii+2][jj] = 0; break; } case JointConstraint::Type::TRANSQ : { t = target_pose->getGlobalTransform( skeleton, other_id ); dv = vector(1,0,0) * t * ( endTrans - t.translation ); J[iii ][jj] = dv[0]; J[iii+1][jj] = dv[1]; J[iii+2][jj] = dv[2]; iii += 3; J[iii ][jj] = 1; J[iii+1][jj] = 0; J[iii+2][jj] = 0; break; } default : break; } } break; } } jj += other_dof; } } return dist; }
void segmentMotion() { int num_frames = motion_data.getNumFrames(); if( num_frames > 0 ) { bool* is_lf_contact = new bool[ num_frames ]; bool* is_rf_contact = new bool[ num_frames ]; // Joint* lf = motion_data.getHumanJoint( Human::LEFT_FOOT ); Joint* rf = motion_data.getHumanJoint( Human::RIGHT_FOOT ); Joint* lt = motion_data.getHumanJoint( Human::LEFT_TOE ); Joint* rt = motion_data.getHumanJoint( Human::RIGHT_TOE ); double hl = 10, sl = 0.5; int f; for( f=0; f < num_frames-1; f++ ) { math::position lf_p = motion_data.getPosition( f, lf->getIndex() ); math::position rf_p = motion_data.getPosition( f, rf->getIndex() ); math::vector lf_v0 = motion_data.getLinearVelocity( f, lf->getIndex() ); math::vector rf_v0 = motion_data.getLinearVelocity( f, rf->getIndex() ); math::vector lf_v1 = motion_data.getLinearVelocity( f+1, lf->getIndex() ); math::vector rf_v1 = motion_data.getLinearVelocity( f+1, rf->getIndex() ); //if( lf_a.length() < sl ) if( lf_p.y() < hl && lf_v0.y() < 0 && lf_v1.y() > 0 ) { is_lf_contact[ f ] = true; } else { is_lf_contact[ f ] = false; } //if( rf_a.length() < sl ) if( rf_p.y() < hl && rf_v0.y() < 0 && rf_v1.y() > 0 ) { is_rf_contact[ f ] = true; } else { is_rf_contact[ f ] = false; } } // unsigned int prev_f = 0; unsigned int num_segments = 0; for( f=1; f < num_frames; f++ ) { if( ( is_rf_contact[ f-1 ] != is_rf_contact[ f ] || is_lf_contact[ f-1 ] != is_lf_contact[ f ] ) && f-prev_f >= MIN_SEGMENT_LENGTH ) { segment_list.push_back( new MotionSegment( prev_f, f-1 ) ); std::cout << "segment[ " << num_segments++ << " ]: " << f-prev_f << " frames ( " << prev_f << "~" << f-1 << " )\n"; prev_f = f; } } } }