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