C++ Skeleton::bone方法代码示例

//------------------------------------------------------------------------------
//!
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...
}