double PoseIK::incorporateDamping( const math::vectorN& d, math::vectorN& dp ) { double dist = 0.0f; double c1, c2; unsigned int jj = 0; for( unsigned int j=0; j < NUM_JOINTS_IN_HUMAN; j++ ) { Joint* joint = skeleton->getHumanJoint( j ); if( joint ) { c1 = PoseIK::DampingCoeff[ j ]; c2 = 2.0 * c1; unsigned int dof = joint->getDOF(); switch( dof ) { case 6 : { dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++; dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++; dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++; dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++; dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++; dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++; break; } case 3 : { dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++; dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++; dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++; break; } case 1 : { dist += c1*d[jj]*d[jj]; dp[jj] -= c2*d[jj]; jj++; break; } default : break; } } } return dist; }
unsigned int Skeleton::calcDOF() { unsigned int total_dof = 0; std::vector< Joint* >::iterator itor_j = joint_list.begin(); while( itor_j != joint_list.end() ) { Joint* j = ( *itor_j ++ ); total_dof += j->getDOF(); } return total_dof; }
void PoseIK::scalingCoefficients( math::vectorN& x ) { double c = 0.0f; unsigned int jj = 0; for( unsigned int j=0; j < NUM_JOINTS_IN_HUMAN; j++ ) { Joint* joint = skeleton->getHumanJoint( j ); if( joint ) { unsigned int dof = joint->getDOF(); switch( dof ) { case 6 : { c = PoseIK::RigidityCoeff[ j ]; x[jj++] *= c; x[jj++] *= c; x[jj++] *= c; c = PoseIK::RigidityCoeff[ j ]; x[jj++] *= c; x[jj++] *= c; x[jj++] *= c; break; } case 3 : { c = PoseIK::RigidityCoeff[ j ]; x[jj++] *= c; x[jj++] *= c; x[jj++] *= c; break; } case 1 : { c = PoseIK::RigidityCoeff[ j ]; x[jj++] *= c; break; } default : break; } } } }
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; }