constraints/constraintextraction/orconstraintextraction.cxx

/***************************************************************************************
 Autodesk(R) Open Reality(R) Samples
 
 (C) 2013 Autodesk, Inc. and/or its licensors
 All rights reserved.
 
 AUTODESK SOFTWARE LICENSE AGREEMENT
 Autodesk, Inc. licenses this Software to you only upon the condition that 
 you accept all of the terms contained in the Software License Agreement ("Agreement") 
 that is embedded in or that is delivered with this Software. By selecting 
 the "I ACCEPT" button at the end of the Agreement or by copying, installing, 
 uploading, accessing or using all or any portion of the Software you agree 
 to enter into the Agreement. A contract is then formed between Autodesk and 
 either you personally, if you acquire the Software for yourself, or the company 
 or other legal entity for which you are acquiring the software.
 
 AUTODESK, INC., MAKES NO WARRANTY, EITHER EXPRESS OR IMPLIED, INCLUDING BUT 
 NOT LIMITED TO ANY IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR 
 PURPOSE REGARDING THESE MATERIALS, AND MAKES SUCH MATERIALS AVAILABLE SOLELY ON AN 
 "AS-IS" BASIS.
 
 IN NO EVENT SHALL AUTODESK, INC., BE LIABLE TO ANYONE FOR SPECIAL, COLLATERAL, 
 INCIDENTAL, OR CONSEQUENTIAL DAMAGES IN CONNECTION WITH OR ARISING OUT OF PURCHASE 
 OR USE OF THESE MATERIALS. THE SOLE AND EXCLUSIVE LIABILITY TO AUTODESK, INC., 
 REGARDLESS OF THE FORM OF ACTION, SHALL NOT EXCEED THE PURCHASE PRICE OF THE 
 MATERIALS DESCRIBED HEREIN.
 
 Autodesk, Inc., reserves the right to revise and improve its products as it sees fit.
 
 Autodesk and Open Reality are registered trademarks or trademarks of Autodesk, Inc., 
 in the U.S.A. and/or other countries. All other brand names, product names, or 
 trademarks belong to their respective holders. 
 
 GOVERNMENT USE
 Use, duplication, or disclosure by the U.S. Government is subject to restrictions as 
 set forth in FAR 12.212 (Commercial Computer Software-Restricted Rights) and 
 DFAR 227.7202 (Rights in Technical Data and Computer Software), as applicable. 
 Manufacturer is Autodesk, Inc., 10 Duke Street, Montreal, Quebec, Canada, H3C 2L7.
***************************************************************************************/

//--- Class declarations
#include "orconstraintextraction.h"

//--- Registration defines
#define ORCONSTRAINTEXTRACTION__CLASS       ORCONSTRAINTEXTRACTION__CLASSNAME
#define ORCONSTRAINTEXTRACTION__NAME        "Extraction override"
#define ORCONSTRAINTEXTRACTION__LABEL       "Extraction override"
#define ORCONSTRAINTEXTRACTION__DESC        "Extraction override"

//--- implementation and registration
FBConstraintImplementation  (   ORCONSTRAINTEXTRACTION__CLASS       );
FBRegisterConstraint        (   ORCONSTRAINTEXTRACTION__NAME,
                                ORCONSTRAINTEXTRACTION__CLASS,
                                ORCONSTRAINTEXTRACTION__LABEL,
                                ORCONSTRAINTEXTRACTION__DESC,
                                FB_DEFAULT_SDK_ICON             );  // Icon filename (default=Open Reality icon)

/************************************************
 *  Creation function.
 ************************************************/
bool ORConstraintExtraction::FBCreate()
{
    FBPropertyPublish(this, Character, "Character", NULL, NULL);    
    Character.SetSingleConnect(true);
    Character.SetFilter(FBCharacter::GetInternalClassId());

    mNodes.Add(new Node(kFBLeftHipNodeId, kFBLeftAnkleNodeId));
    mNodes.Add(new Node(kFBRightHipNodeId, kFBRightAnkleNodeId));
    mNodes.Add(new Node(kFBLeftShoulderNodeId, kFBLeftWristNodeId));
    mNodes.Add(new Node(kFBRightShoulderNodeId, kFBRightWristNodeId));

    mReferenceGroup = ReferenceGroupAdd( "Goals", 0 );

    HasLayout = true;

    return true;
}

/************************************************
 *  Destruction function.
 ************************************************/
void ORConstraintExtraction::FBDestroy()
{
    while(mNodes.GetCount())
    {
        delete mNodes.GetLast();
        mNodes.RemoveLast();
    }
}

/************************************************
 *  Setup all of the animation nodes.
 ************************************************/
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); 
            }
        }
    }
}

/************************************************
 *  Removed all of the animation nodes.
 ************************************************/
void ORConstraintExtraction::RemoveAllAnimationNodes()
{
    for(int i = 0; i < mNodes.GetCount(); i++)
    {
        mNodes[i]->Clear(); 
    }
}

/************************************************
 *  Removed all of the animation nodes.
 ************************************************/
bool ORConstraintExtraction::PlugNotify(FBConnectionAction pAction,FBPlug* pThis,int pIndex,FBPlug* pPlug,FBConnectionType pConnectionType,FBPlug* pNewPlug)
{
    if( FBIS(pPlug, FBCharacter) )
    {
        FBCharacter* lCharacter = (FBCharacter*)pPlug;
        if(pThis->GetHIObject() == Character.GetHIObject())
        {
            switch (pAction)
            {
            case kFBRequestConnectSrc:
                return lCharacter->GetCharacterize() && lCharacter->GetCharacterMarkerSet() != NULL;
                Active = false;
                ReferenceRemoveAll();
                break;

            case kFBConnectedSrc:
                for(int i = 0; i < mNodes.GetCount(); i++)
                {
                    mNodes[i]->ReferenceAdd(lCharacter, this, mReferenceGroup); 
                }
                break;

            case kFBConnectSrc:
            case kFBDisconnectSrc:
                Active = false;
                ReferenceRemoveAll();

                break;
            }
        }
    }

    return true;
}

/************************************************
 *  Add goal reference to the constraint, so we will receive SetupAllAnimationNodes call.
 ************************************************/
void ORConstraintExtraction::Node::ReferenceAdd(FBCharacter* pCharacter, FBConstraint* pConstraint, int pGroup)
{
    FBModel* lStartGoalModel = pCharacter->GetGoalModel(mStartNode);
    FBModel* lEndGoalModel = pCharacter->GetGoalModel(mEndNode);

    if(lStartGoalModel && lEndGoalModel)
    {
        pConstraint->ReferenceAdd(pGroup,lStartGoalModel);
        pConstraint->ReferenceAdd(pGroup,lEndGoalModel);
    }
}

/************************************************
 *  Setup all node information useful for solve.
 ************************************************/
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 characterization stance pose to calculate max 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);
        // used to read directly from connector to avoid caching the data on the model (this way we can write back the data)
        // other solution, little slower, would be to use background evaluation, but then this solver should be build on
        // top of FBConstraintSolver.
        mReadT = lEndGoalModel->Translation.GetAnimationNode();
        // create input connector to end effector translation (global data)
        mWriteT = pConstraint->AnimationNodeInCreate    ( pIndex+1, lEndGoalModel, ANIMATIONNODE_TYPE_TRANSLATION );
    }
}

/************************************************
 *  Solve node (very simply logic that lock node length to characterized one).
 *  This can be use to create smooth, anti snapping constraint for elbows and knees
 ************************************************/
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);
        // we can't read from effector model, because this will cache the data and writing back won't have any effect
        //lEnd->GetVector(*(FBVector3d*)(lT_B.mValue), kModelTranslation, true, pEvaluateInfo);
        // we have to read from connector directly...knowing that data over there is always 
        // written global by Character Marker Set solver!
        mReadT->ReadData(lT_B.mValue, pEvaluateInfo);

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

        // very simply test that can be expand to more sophisticated logic
        if(lCurrentLength > mLength)
        {
            FBMult(lV,lV,mLength/lCurrentLength);

            FBAdd(lT_B, lT_A, lV);

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

/************************************************
 *  Real-Time Engine Evaluation
 ************************************************/
bool ORConstraintExtraction::AnimationNodeNotify(FBAnimationNode* pConnector,FBEvaluateInfo* pEvaluateInfo,FBConstraintInfo* pConstraintInfo)
{
    // if there is any character connected (just in case, since we use it blindly after)
    if(Character.GetSrcCount() > 0)
    {
        FBCharacter* lCharacter = (FBCharacter*)Character.GetSrc(0);            

        // solve all constraints
        for(int i = 0; i < mNodes.GetCount(); i++)
        {
            mNodes[i]->Solve(lCharacter, this, pEvaluateInfo); 
        }
    }
    // disable all those animation nodes that we didn't write to.
    // this is optimization, that will stop evaluation engine
    // from calling AnimationNodeNotify again in the same evaluation id
    AnimationNodesOutDisableIfNotWritten(pEvaluateInfo);

    return true;
}

/************************************************
 *  FBX storage of constraint parameters.
 ************************************************/
bool ORConstraintExtraction::FbxStore( FBFbxObject* pFbxObject, kFbxObjectStore pStoreWhat )
{
    return true;
}

/************************************************
 *  FBX retrieval of constraint parameters.
 ************************************************/
bool ORConstraintExtraction::FbxRetrieve( FBFbxObject* pFbxObject, kFbxObjectStore pStoreWhat )
{
    return true;
}