//------------------------------------------------------------------------------
//!
void
adjustLimb(
Skeleton::Instance& skelInst,
const Puppeteer::PositionalConstraint& constraint,
const Vec3f& dRoot
)
{
Skeleton* skeleton = skelInst.skeleton();
int cID = constraint.id();
const Skeleton::Limb& limb = skeleton->limbFromBone( cID );
int sID = limb.boneID();
int eID = limb.endEffectorID();
// 1. Compute mid angle.
float b1Len = skeleton->bone( sID ).length();
float b2Len = skeleton->bone( eID ).length();
// Current angle.
Vec3f cdir = constraint.currentPos()-skelInst.globalPosition(sID);
float clenSqr = cdir.sqrLength();
float ccosAngle = (b1Len*b1Len + b2Len*b2Len - clenSqr) / (2*b1Len*b2Len);
ccosAngle = CGM::clamp( ccosAngle, -1.0f, 1.0f );
float cangle = CGM::acos( ccosAngle );
// New angle.
Vec3f dir = constraint.endPos()-(skelInst.globalPosition(sID)+dRoot);
float lenSqr = dir.sqrLength();
float cosAngle = (b1Len*b1Len + b2Len*b2Len - lenSqr) / (2*b1Len*b2Len);
cosAngle = CGM::clamp( cosAngle, -1.0f, 1.0f );
float angle = CGM::acos( cosAngle );
float diffAngle = cangle-angle;
Quatf orient;
switch( skeleton->bone( eID ).dof() )
{
case Skeleton::RX: orient = Quatf::eulerX( diffAngle ); break;
case Skeleton::RY: orient = Quatf::eulerY( diffAngle ); break;
case Skeleton::RZ: orient = Quatf::eulerZ( diffAngle ); break;
default: orient = Quatf::eulerX( diffAngle );
}
orient = orient * skelInst.localOrientation( eID );
// 2. Compute start angle.
// 2.1 target position in local space of first limb bone.
Vec3f target = skelInst.globalReferential(sID).globalToLocal() * (constraint.endPos()-dRoot);
// 2.2 Compute new current end effector position in limb space.
Vec3f localPos = skelInst.localReferential(eID).position();
Vec3f curPos = Reff( orient, localPos ).toMatrix() * skeleton->bone(eID).endPoint();
// 2.3 Compute rotation quaternion.
Quatf rot = Quatf::twoVecs( curPos, target );
rot = rot * skelInst.localOrientation(sID);
// 3. Blend with slerp/nlerp.
//skelInst.localOrientationNoUpdate( sID, skelInst.localOrientation(sID).slerp( rot, constraint.weight() ) );
//skelInst.localOrientationNoUpdate( eID, skelInst.localOrientation(eID).slerp( orient, constraint.weight() ) );
skelInst.localOrientationNoUpdate( sID, skelInst.localOrientation(sID).nlerp( rot, constraint.weight() ) );
skelInst.localOrientationNoUpdate( eID, skelInst.localOrientation(eID).nlerp( orient, constraint.weight() ) );
}
//------------------------------------------------------------------------------
//!
void
Puppeteer::resolveConstraints(
Skeleton::Instance& skelInst,
const Vector< PositionalConstraint >& constraints
)
{
if( constraints.empty() ) return;
Skeleton* skeleton = skelInst.skeleton();
// Compute global root position.
Vec3f root = skelInst.globalPosition(0);
// 1. Transform constraints into end effector constraints.
for( uint i = 0; i < constraints.size(); ++i )
{
int cID = constraints[i].id();
Skeleton::Limb& limb = skeleton->limbFromBone( cID );
int eID = limb.endEffectorID();
Vec3f epos = skelInst.globalReferential( eID ).toMatrix() * skeleton->bone( eID ).endPoint();
constraints[i]._currentPos = epos;
constraints[i]._endEffectorPos = constraints[i]._position;
if( cID != eID )
{
Vec3f cpos = skelInst.globalReferential( cID ).toMatrix() * skeleton->bone( cID ).endPoint();
constraints[i]._endEffectorPos -= cpos-epos;
}
Vec3f delta = root - skelInst.globalPosition( limb.boneID() );
constraints[i]._sphere.center( constraints[i]._endEffectorPos + delta );
constraints[i]._sphere.radius( limb.reachRadius()/constraints[i].weight() );
}
// 2. Compute an approximate root position.
Vec3f dRoot(0.0f);
float weights = 0.0f;
float maxWeight = 0.0f;
for( uint i = 0; i < constraints.size(); ++i )
{
dRoot += constraints[i].weight() * (constraints[i]._endEffectorPos-constraints[i]._currentPos);
weights += constraints[i].weight();
if( constraints[i].weight() > maxWeight )
{
maxWeight = constraints[i].weight();
}
}
dRoot *= (maxWeight/weights)*0.8f;
root += dRoot;
// 3. Adjust root position to satisfy all constraints.
adjustRoot( constraints, root, dRoot );
// 4. Adjust limbs positions.
for( uint i = 0; i < constraints.size(); ++i )
{
adjustLimb( skelInst, constraints[i], dRoot );
}
// 5. Adjust skeleton instance.
skelInst.offset().position() += dRoot;
skelInst.updateGlobalTransforms();
// TODO: Ajust end of limbs...
}
