Adjusting the Extraction

void ORConstraintExtraction::SetupAllAnimationNodes()
{
  if(Character.GetSrcCount() > 0)
  {
    FBCharacter* lCharacter = (FBCharacter*)Character.GetSrc(0);
    FBCharacterMarkerSet* lMarkerSet = lCharacter ? lCharacter->GetCharacterMarkerSet():NULL;

    if(lMarkerSet)
    {
      for(int i = 0; i < mNodes.GetCount(); i++)
      {
        mNodes[i]->Setup(lCharacter, this, i); 
      }
    }
  }
}
 
void ORConstraintExtraction::Node::Setup(FBCharacter* pCharacter, FBConstraint* pConstraint, int pIndex)
{
  FBModel* lStartGoalModel = pCharacter->GetGoalModel(mStartNode);
  FBModel* lEndGoalModel = pCharacter->GetGoalModel(mEndNode);

  if(lStartGoalModel && lEndGoalModel)
  {
    FBTVector lT_A,lT_B,lV;
    
    // Get the characterization stance pose to calculate the maximum length.
    pCharacter->GetTOffset(mStartNode, &lT_A);
    pCharacter->GetTOffset(mEndNode, &lT_B);

    FBSub(lV, lT_B, lT_A);
    // Calculate the maximum allowed length per body part.
    mLength = FBLength(lV);
 
    mReadT = lEndGoalModel->Translation.GetAnimationNode();
    // Create the input connector to end the effector translation (global data)
    mWriteT = pConstraint->AnimationNodeInCreate  (pIndex+1, lEndGoalModel, ANIMATIONNODE_TYPE_TRANSLATION);
  }
}

void ORConstraintExtraction::Node::Solve(FBCharacter* pCharacter, FBConstraint* pConstraint, FBEvaluateInfo* pEvaluateInfo)
{
  if(mReadT == NULL || mWriteT == NULL) return;

  FBModel* lStart = pCharacter->GetGoalModel(mStartNode);
  FBModel* lEnd = pCharacter->GetGoalModel(mEndNode);

  if(lStart && lEnd)
  {
    FBTVector lT_A,lT_B,lV;

    lStart->GetVector(*(FBVector3d*)(lT_A.mValue), kModelTranslation, true, pEvaluateInfo);
    // Read from the connector directly, knowing that data over there is always 
    // written globally by the Character Marker Set solver.
    mReadT->ReadData(lT_B.mValue, pEvaluateInfo);

    FBSub(lV, lT_B, lT_A);
    double lCurrentLength = FBLength(lV);

    // Very simple test that can be expanded to more sophisticated logic.
    if(lCurrentLength > mLength)
    {
      FBMult(lV,lV,mLength/lCurrentLength);

      FBAdd(lT_B, lT_A, lV);

      // Write the clamp global translation.
      mWriteT->WriteData(lT_B.mValue, pEvaluateInfo);
    }
  }
}

bool ORConstraintExtraction::AnimationNodeNotify(FBAnimationNode* pConnector,
     FBEvaluateInfo* pEvaluateInfo,FBConstraintInfo* pConstraintInfo)
{
  // If any character is connected (Just in case, as it is used blindly afterwards)
  if(Character.GetSrcCount() > 0)
  {
    FBCharacter* lCharacter = (FBCharacter*)Character.GetSrc(0);

    // Solve all the constraints.
    for(int i = 0; i < mNodes.GetCount(); i++)
    {
      mNodes[i]->Solve(lCharacter, this, pEvaluateInfo); 
    }
  }
  // Disable all the animation nodes that you did not write to.
  // This optimization stops the evaluation engine
  // from calling AnimationNodeNotify again in the same evaluation ID
  AnimationNodesOutDisableIfNotWritten(pEvaluateInfo);

  return true;
}