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