constraints/CharacterSolver/HIK2014Solver/orcharactersolver_hik_manipulator.cxx

constraints/CharacterSolver/HIK2014Solver/orcharactersolver_hik_manipulator.cxx
/***************************************************************************************
Autodesk(R) Open Reality(R) Samples
(C) 2012 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 "fbsdk/fbsdk.h"
#include "orcharactersolver_hik.h"
#include "filterset2fbcharacter.h"
#include <math.h>
#define ORCHARACTERMANIPULATORCTRLSET__CLASS ORCHARACTERMANIPULATORCTRLSET__CLASSNAME
//--- implementation and registration
FBCharacterManipulatorCtrlSetImplementation ( ORCHARACTERMANIPULATORCTRLSET__CLASS );
/************************************************
* Creation function.
************************************************/
bool ORCharacterManipulatorCtrlSet::FBCreate()
{
mHIKCharacter = NULL;
CleanForManipulation();
HIKHostPropertiesInit(mHIKCharacterHost);
return true;
}
/************************************************
* Destruction function.
************************************************/
void ORCharacterManipulatorCtrlSet::FBDestroy()
{
DeallocateState();
FBCharacterManipulatorCtrlSet::FBDestroy();
}
//TRANSLATION
FBManipulationGetResult ORCharacterManipulatorCtrlSet::TranslationIsEditable( bool &pEditable, FBReferencialType pRefType )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
pEditable = true;
}
else
{
}
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::TranslationSet( FBTVector &pT, FBReferencialType pRefType, FBSetType pSetType, FBSetWhat pSetWhat )
{
FBCharacter *lCharacter = GetCharacter();
if ( lCharacter->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
FBEffectorId lEffectorIndex = GetCorrespondingEffector();
int lHIKId = FBEffectorIdToHIKId(lEffectorIndex);
// translation manipulation is only allowed on IK or FK that have corresponding IK effector (logic in GetCorrespondingEffector).
if(lHIKId != -1)
{
HIKSetTranslationActive(mCurrentState.mEffectorSetState, lHIKId, 1.0);
if(IsReachOverride())
{
// need to set it first, because IsDescendantPulling is using this value
HIKSetPull( mCurrentState.mEffectorSetState, lHIKId, 0.0 );
if(!IsDescendantPulling( lEffectorIndex ))
{
HIKSetPull( mCurrentState.mEffectorSetState, lHIKId, 1.0 );
}
}
if( pRefType == FBRefTypeGlobal )
{
HIKGetEffectorStateTQSdv(mCurrentState.mEffectorSetState, lHIKId, lT.mValue, lQ.mValue, lS.mValue );
lT = pT;
Translation_ApplyPivot(lEffectorIndex, lHIKId, lT, lQ, lS);
HIKSetEffectorStateTQSdv(mCurrentState.mEffectorSetState, lHIKId, lT.mValue, lQ.mValue, lS.mValue );
AskSync();
}
else if(pRefType == FBRefTypeLocal)
{
FBModel *lModel = lCharacter->GetEffectorModel(lEffectorIndex, GetEffectorSet());
// Convert to Global Coordinate
FBTVector lGlobalTranslation;
Translation_LocalToGlobal(lGlobalTranslation, lModel, pT);
TranslationSet(lGlobalTranslation, FBRefTypeGlobal, pSetType, pSetWhat);
}
else
{
// not supported.
}
lResult = kFBManipulationSetFail;// we don't want the system to Set the Value, we did it ourself
}
}
}
return lResult;
}
FBManipulationGetResult ORCharacterManipulatorCtrlSet::TranslationGet( FBTVector &pT, FBReferencialType pRefType, FBManipPivot pPivot )
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
if( IsManipulated_FK() ) // if it's an effector, the system will return the right value
{
FBEffectorId lEffectorIndex = GetCorrespondingEffector(); // Check if there is a corresponding Effector
if(lEffectorIndex != kFBInvalidEffectorId)
{
FBEvaluateInfo* lEvaluateInfo = FBGetDisplayInfo();
if(pRefType == FBRefTypeGlobal)
{
GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(lEffectorIndex, GetEffectorSet())->GetVector(*(FBVector3d*)&pT, kModelTranslation, true, lEvaluateInfo);
}
else if(pRefType == FBRefTypeLocal)
{
GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(lEffectorIndex, GetEffectorSet())->GetVector(*(FBVector3d*)&pT, kModelTranslation, false, lEvaluateInfo);
}
}
}
}
}
return lResult;
}
FBManipulationStartResult ORCharacterManipulatorCtrlSet::TranslationStartManipulation( FBReferencialType pRefType, const FBTime* pTime )
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
ManipulationStart( FBManip_EditorTranslation, pTime );
}
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::TranslationStopManipulation ()
{
CleanForManipulation();
}
//ROTATION
FBManipulationGetResult ORCharacterManipulatorCtrlSet::RotationIsEditable( bool &pEditable, FBReferencialType pRefType )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
pEditable = true;
}
else
{
}
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::RotationSet( FBMatrix &pRM, FBReferencialType pRefType, FBSetType pSetType, bool pOnlyRoot )
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
FBEffectorId lEffectorIndex = GetCorrespondingEffector();
// we manipulate IK directly or via FK that have corresponding IK
if( lEffectorIndex != kFBInvalidEffectorId )
{
if( pRefType == FBRefTypeGlobal )
{
lResult = RotationSetEffectorGlobal(lEffectorIndex, pRM);
}
else if ( pRefType == FBRefTypeLocal )
{
lResult = RotationSetEffectorLocal(lEffectorIndex, pRM);
}
else
{
// not supported.
}
}
// pure FK manipulation
else
{
FBBodyNodeId lBodyNodeIndex = GetManipulatedBodyNode();
// yes, this can happen, because it's characterized. In TranslationSet we don't have to test it, because reference doesn't have effector.
if ( lBodyNodeIndex == kFBReferenceNodeId )
{
}
else
{
OR_HIK_ASSERT(IsManipulated_FK()); //only if there is an error, we will get here with none FK model
FBModel* lModel = GetManipulatedNode();
if ( pRefType == FBRefTypeGlobal )
{
lResult = RotationSetFKGlobal(lModel, pRM);
}
else if ( pRefType == FBRefTypeLocal )
{
lResult = RotationSetFKLocal(lModel, pRM);
}
else
{
// not supported.
}
}
}
AskSync();
}
}
return lResult;
}
FBManipulationGetResult ORCharacterManipulatorCtrlSet::RotationGet( FBMatrix &pRM, FBReferencialType pRefType )
{
FBCharacter* lCharacter = GetCharacter();
if( lCharacter->IsCtrlSetReady() )
{
if( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
FBEffectorId lEffectorIndex;
// For rotation, we redirect if it's for the orientation of the Translation Widget
if( GetManipulatorMode() == FBManip_EditorTranslation )
{
lEffectorIndex = GetCorrespondingEffector(); // Get Corresponding Effector if any
}
else
{
lEffectorIndex = kFBInvalidEffectorId;
}
if( lEffectorIndex != kFBInvalidEffectorId )
{
FBModel *lModel = lCharacter->GetEffectorModel(lEffectorIndex, GetEffectorSet());
if ( lModel )
{
FBEvaluateInfo* lEvaluateInfo = FBGetDisplayInfo();
if(pRefType == FBRefTypeGlobal)
{
lModel->GetMatrix(pRM, kModelRotation, true, lEvaluateInfo);
// Apply post Rotation if needed
AddPostRotation(lModel, pRM);
}
else if(pRefType == FBRefTypeLocal)
{
lModel->GetMatrix(pRM, kModelRotation, false, lEvaluateInfo);
}
else if( pRefType == FBRefTypeParent && lModel->Parent != NULL )
{
lModel->Parent->GetMatrix(pRM, kModelRotation, true, lEvaluateInfo);
// Apply post Rotation if needed
AddPostRotation(lModel->Parent, pRM);
}
else
{
//Unsupported RefType !
}
}
}
}
}
return lRet;
}
FBManipulationStartResult ORCharacterManipulatorCtrlSet::RotationStartManipulation( FBReferencialType pRefType )
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
ManipulationStart( FBManip_EditorRotation );
}
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::RotationStopManipulation ()
{
CleanForManipulation();
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::RotationSetEffectorGlobal( FBEffectorId &pEffectorIndex, const FBMatrix &pRM )
{
int lHIKId = FBEffectorIdToHIKId( pEffectorIndex );
if(lHIKId != -1)
{
if( NeedRotationSync(pEffectorIndex,120.0) )
{
ManipulationStart( FBManip_EditorRotation );
}
HIKSetRotationActive(mCurrentState.mEffectorSetState, lHIKId, 1.0);
FBMatrix lResultRotation(pRM);
HIKGetEffectorStateTQSdv(mCurrentState.mEffectorSetState, lHIKId, lGT.mValue, lGQ.mValue, lGS.mValue );
FBModel* lModel = GetManipulatedNode(); //we should use real manipulated model to do the conversion, not corresponding IK
RemovePostRotation(lModel, lResultRotation);
if(IsManipulated_FK())
{
//have to convert FK global rotation to IK before applying it to IK
FBVector4d lGT_Temp;
FBQuaternion lGQ_FK;
FBQuaternion lGQ_IK;
FBVector4d lGS_Temp;
FBMatrix lGM_FK;
FBMatrix lGM_IK;
FBMatrix lGM_Offset;
HIKNodeId lNodeId = GetManipulatedHIKNodeId();
HIKGetNodeNormalizedStateTQSdv(mHIKCharacter, mInitialState.mState, lNodeId, lGT_Temp.mValue, lGQ_FK.mValue, lGS_Temp.mValue);
HIKGetEffectorStateTQSdv(mInitialState.mEffectorSetState, lHIKId, lGT_Temp.mValue, lGQ_IK.mValue, lGS_Temp.mValue);
// kxl: we should have more quaternion math functions - quaternion inverse!
FBQuaternionToMatrix(lGM_FK, lGQ_FK);
FBQuaternionToMatrix(lGM_IK, lGQ_IK);
FBGetLocalMatrix(lGM_Offset, lGM_FK, lGM_IK);
FBGetGlobalMatrix(lResultRotation, lResultRotation, lGM_Offset);
}
FBMatrixToQuaternion(lGQ, lResultRotation);
Rotation_ApplyPivot(pEffectorIndex, lHIKId, lGT, lGQ, lGS);
HIKSetEffectorStateTQSdv(mCurrentState.mEffectorSetState, lHIKId, lGT.mValue, lGQ.mValue, lGS.mValue );
}
return kFBManipulationSetFail; // We don't want the System to Set the Value, we did it ourself
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::RotationSetEffectorLocal( FBEffectorId &pEffectorIndex, const FBMatrix &pRM )
{
FBMatrix lGlobalRotation;
FBModel* lModel = GetManipulatedNode(); //we should use real manipulated model to do the conversion, not corresponding IK
if(IsManipulated_FK())
{
HIKNodeId lNodeId = GetManipulatedHIKNodeId();
OR_HIK_ASSERT(lNodeId != LastNodeId);
FBMatrix lPGRM;
int const lParentNodeId = HIKGetParentNodeId(mHIKCharacter, lNodeId);
if(lParentNodeId != -1)
{
FBVector4d lPGT;
FBVector4d lPGS;
HIKGetNodeNormalizedStateTQSdv( mHIKCharacter, mInitialState.mState, lParentNodeId, lPGT.mValue, lPGQ.mValue, lPGS.mValue );
FBQuaternionToMatrix(lPGRM, lPGQ);
}
AddPostRotation(lModel, lPGRM);
FBMatrixMult(lGlobalRotation, lPGRM, pRM);
}
else
{
if ( lModel->Parent )
{
lModel->Parent->GetMatrix( lGlobalRotation, kModelRotation, true );
}
AddPostRotation(lModel, lGlobalRotation);
FBMatrixMult(lGlobalRotation, lGlobalRotation, pRM);
}
RotationSetEffectorGlobal(pEffectorIndex, lGlobalRotation);
return kFBManipulationSetFail; // We don't want the System to Set the Value, we did it ourself
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::RotationSetFKGlobal( FBModel *pModel, FBMatrix &pRM )
{
HIKNodeId lNodeId = GetManipulatedHIKNodeId();
OR_HIK_ASSERT(lNodeId != LastNodeId);
FBMatrix lRM(pRM);
RemovePostRotation(pModel, lRM);
#if 0
// kxl: this can be optimize - we convert to local, because we want to rotate all FK bones in hierarchy
// current implementation force recompute of everything - we just need to set global data and recompute down from there
int const lParentNodeId = HIKGetParentNodeId(mHIKCharacter, lNodeId);
if(lParentNodeId != -1)
{
FBVector4d lGTVector, lGSVector;
FBMatrix lMatrix;
HIKGetNodeNormalizedStateTQSdv( mHIKCharacter, mCurrentState.mState, lParentNodeId, lGTVector.mValue, lGQ.mValue, lGSVector.mValue );
FBQuaternionToMatrix(lMatrix, lGQ);
FBMatrixInverse(lMatrix, lMatrix);
FBMatrixMult(lRM, lMatrix, lRM); // Inverse Parent x Global Matrix
}
GetNormalizedLocalState(mCurrentState.mDataSet);
{
mCurrentState.mDataSet[lNodeId].mQfv[0] = lGQ[0];
mCurrentState.mDataSet[lNodeId].mQfv[1] = lGQ[1];
mCurrentState.mDataSet[lNodeId].mQfv[2] = lGQ[2];
mCurrentState.mDataSet[lNodeId].mQfv[3] = lGQ[3];
}
SetNormalizedLocalState(mCurrentState.mDataSet);
#else
SetNormalizedNodeStateForHierarchy(mHIKCharacter, mCurrentState.mState, mCurrentState.mDataSet, lNodeId, NULL, &lGQ, NULL, true);
#endif
return kFBManipulationSetFail; // We don't want the System to Set the Value, we did it ourself
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::RotationSetFKLocal( FBModel *pModel, FBMatrix &pRM )
{
HIKNodeId lNodeId = GetManipulatedHIKNodeId();
OR_HIK_ASSERT(lNodeId != LastNodeId);
FBRVector lDof;
FBMatrix lLRM;
pModel->MatrixToRotation(lDof, pRM);
pModel->DofToLRM(lLRM, lDof);
#if 0
GetNormalizedLocalState(mCurrentState.mDataSet);
{
mCurrentState.mDataSet[lNodeId].mQfv[0] = lLQ[0];
mCurrentState.mDataSet[lNodeId].mQfv[1] = lLQ[1];
mCurrentState.mDataSet[lNodeId].mQfv[2] = lLQ[2];
mCurrentState.mDataSet[lNodeId].mQfv[3] = lLQ[3];
}
SetNormalizedLocalState(mCurrentState.mDataSet);
#else
SetNormalizedNodeStateForHierarchy(mHIKCharacter, mCurrentState.mState, mCurrentState.mDataSet, lNodeId, NULL, &lLQ, NULL, false);
#endif
return kFBManipulationSetFail; // We don't want the System to Set the Value, we did it ourself
}
void ORCharacterManipulatorCtrlSet::Translation_LocalToGlobal(FBTVector &pGT, FBModel *pModel, const FBTVector &pLT, FBModelTransformationType pTransformType)
{
OR_HIK_ASSERT(pModel);
// kxl: I'm not convinced that this is the best way to do this
FBMatrix lParentGlobal;
if ( pModel->Parent )
{
pModel->Parent->GetMatrix( lParentGlobal, pTransformType, true );
}
FBVectorMatrixMult(pGT, lParentGlobal, pLT);
}
//SCALING
FBManipulationGetResult ORCharacterManipulatorCtrlSet::ScalingIsEditable( bool &pEditable )
{
}
FBManipulationGetResult ORCharacterManipulatorCtrlSet::ScalingGet( FBSVector &pS, FBReferencialType pRefType )
{
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::ScalingSet( FBSVector &pS, FBReferencialType pRefType, FBSetType pSetType, FBSetWhat pSetWhat )
{
}
FBManipulationStartResult ORCharacterManipulatorCtrlSet::ScalingStartManipulation( FBReferencialType pRefType )
{
}
void ORCharacterManipulatorCtrlSet::ScalingStopManipulation ()
{
CleanForManipulation();
}
void ORCharacterManipulatorCtrlSet::AllocateState(FBCharacter* pCharacter)
{
ORCharacterSolver_HIK* lSolver = GetORCharacterSolver_HIK(pCharacter);
if(lSolver && lSolver->TargetCharacter)
{
// there is a problem, we don't call Allocate/Deallocate of state in pair!
if(mHIKCharacter != NULL)
{
HIKCharacterDestroy(mHIKCharacter,&free);
mHIKCharacter = NULL;
}
HIKCharacterHostFromFBCharacterSolver(mHIKCharacterHost, lSolver, NULL, true);
HIKCharacterFromFBCharacterSolver(mHIKCharacterHost, mHIKCharacter, lSolver);
HIKHostPropertiesFromCharacter(mHIKCharacterHost,pCharacter);
lSolver->RegisterExtraProperties(mHIKCharacterHost);
FBControlSet* lControlSet = pCharacter->GetCurrentControlSet();
if(lControlSet != NULL)
{
HIKControlRigHostFromFBCharacterSolver(mHIKControlRigHost, lSolver, lControlSet, NULL);
bool lFKIK = lControlSet->ControlSetType == kFBControlSetTypeFKIK;
mHIKCurrentControlRigHostEvaluator.Init(mHIKCharacter,&mHIKCharacterHost,&mHIKControlRigHost, &malloc, lFKIK);
mHIKInitialControlRigHostEvaluator.Init(mHIKCharacter,&mHIKCharacterHost,&mHIKControlRigHost, &malloc, lFKIK);
mCurrentState.Init(mHIKCharacter,NULL,&malloc);
mInitialState.Init(mHIKCharacter,NULL,&malloc);
}
}
}
void ORCharacterManipulatorCtrlSet::DeallocateState()
{
if(mHIKCharacter != NULL)
{
HIKCharacterDestroy(mHIKCharacter,&free);
mHIKCharacter = NULL;
}
mHIKCurrentControlRigHostEvaluator.Clear(&free);
mHIKInitialControlRigHostEvaluator.Clear(&free);
mCurrentState.Clear(&free);
mInitialState.Clear(&free);
}
bool ORCharacterManipulatorCtrlSet::IsDescendantPulling(FBEffectorId pEffectorId)
{
int lHIKId = FBEffectorIdToHIKId( pEffectorId );
bool lRet = HIKGetPull(mCurrentState.mEffectorSetState, lHIKId) >= 0.5;
int lCount = GetDescendantEffectorCount(pEffectorId);
for(int lIter = 0; !lRet && lIter < lCount; lIter++)
{
FBEffectorId lChildEffectorId = GetDescendantEffector(pEffectorId, lIter);
lRet = IsDescendantPulling(lChildEffectorId);
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::SetResist(float pResist)
{
for(int lIter = HipsEffectorId; lIter < LastEffectorId; lIter++)
{
HIKSetResist(mCurrentState.mEffectorSetState, lIter, pResist);
}
}
void ORCharacterManipulatorCtrlSet::SetBlendFact(float pFact)
{
for(int lIter = HipsEffectorId; lIter < LastEffectorId; lIter++)
{
HIKSetRotationActive(mCurrentState.mEffectorSetState, lIter, pFact);
HIKSetTranslationActive(mCurrentState.mEffectorSetState, lIter, pFact);
}
}
int ORCharacterManipulatorCtrlSet::GetManipulationSolvingStep()
{
FBCharacter *lCharacter = GetCharacter();
int lSolvingStep = HIKSolvingStepAll;
//Adjust the solving according to the Active body part
if( IsBodyPart() || IsFullBodyNoPull() )
{
lSolvingStep &= ~HIKSolvingStepBodyPull;
}
if( lCharacter && lCharacter->ContactBehaviour != kFBParamContactAlwaysSync )
{
lSolvingStep &= ~HIKSolvingStepContact;
}
//check for additionnal properties...
ORCharacterSolver_HIK* lConstraint = GetORCharacterSolver_HIK(lCharacter);
if(lConstraint)
{
//set sns
if(lConstraint->LegSNS)
lSolvingStep |= LegFilter;
else
lSolvingStep &= ~LegFilter;
if(lConstraint->ArmSNS)
lSolvingStep |= ArmFilter;
else
lSolvingStep &= ~ArmFilter;
}
return lSolvingStep;
}
void ORCharacterManipulatorCtrlSet::SyncCharacter(FBEvaluateInfo *pEvalInfo)
{
ApplySync(pEvalInfo);
}
void ORCharacterManipulatorCtrlSet::AskSync( bool pForce /*= false*/ )
{
if(pForce)
{
ApplySync(FBGetDisplayInfo(), true);
}
else
{
mCurrentState.mDirtyFlag = true;
}
}
void ORCharacterManipulatorCtrlSet::ApplySync(FBEvaluateInfo *pEvalInfo, bool pForce /*= false*/ )
{
if(mCurrentState.mDirtyFlag || pForce)
{
// Set solving step according to settings and solve mCurrentState
// are we in relative extraction mode - normal solve
if(HIKGetPropertyValue(mCurrentState.mPropertySetState, HIKRollExtractionMode) == 0)
{
mHIKCurrentControlRigHostEvaluator.SetSolvingStep( GetManipulationSolvingStep() );
}
// absolute extraction mode - we don't solve roll extraction, it will be only on skeleton
// this is done to avoid accumulation of extraction at each manipulation/key
else
{
mHIKCurrentControlRigHostEvaluator.SetSolvingStep( GetManipulationSolvingStep() & ~HIKSolvingStepRollExtraction );
}
mHIKCurrentControlRigHostEvaluator.Solve(&mCurrentState);
// Solve Back on Effector
mHIKCurrentControlRigHostEvaluator.EffectorStateFromControlSet(&mCurrentState);
// Write FK/IK Rig
mHIKCurrentControlRigHostEvaluator.WriteRigCandidate(pEvalInfo,&mCurrentState, true);
// Bring back initial FK and IK state (copy mInitialState to mCurrentState).
RestoreInitialState();
mCurrentState.mDirtyFlag = false;
}
}
void ORCharacterManipulatorCtrlSet::RestoreInitialState()
{
HIKCharacterStateCopy( mCurrentState.mState, mInitialState.mState );
HIKEffectorStateCopy( mCurrentState.mEffectorSetState, mInitialState.mEffectorSetState );
}
void ORCharacterManipulatorCtrlSet::Translation_ApplyPivot( FBEffectorId &pEffectorId, int pHIKId, FBTVector &pGT, const FBQuaternion &pGQ, const FBVector4d &pGS )
{
OR_HIK_ASSERT(pEffectorId != kFBInvalidEffectorId);
if(IsIKPivotUse() == false) return;
FBTVector lIKPivot;
FBModelMarker* lPivotEffector = dynamic_cast<FBModelMarker*>(GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(pEffectorId, GetEffectorSet()));
lPivotEffector->IKPivot.GetData(lIKPivot.mValue, sizeof(lIKPivot));
if( lIKPivot.mValue[0] != 0.0 || lIKPivot.mValue[1] != 0.0 || lIKPivot.mValue[2] != 0.0 )
{
// we can't let rotation change during this manipulation (to perfectly follow manipulation axis)
HIKSetRotationActive(mCurrentState.mEffectorSetState, pHIKId, 1.0);
FBRVector lGR;
FBMatrix lGX;
FBTRSToMatrix(lGX, pGT, lGR, *(FBSVector*)&pGS);
FBVectorMatrixMult(pGT, lGX, lIKPivot);
}
}
void ORCharacterManipulatorCtrlSet::Rotation_ApplyPivot( FBEffectorId &pEffectorId, int pHIKId, FBTVector &pGT, const FBQuaternion &pGQ, const FBVector4d &pGS )
{
OR_HIK_ASSERT(pEffectorId != kFBInvalidEffectorId);
OR_HIK_ASSERT(pHIKId != -1);
if(IsIKPivotUse() == false) return;
FBTVector lIKPivot;
FBModelMarker* lPivotEffector = dynamic_cast<FBModelMarker*>(GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(pEffectorId, GetEffectorSet()));
lPivotEffector->IKPivot.GetData(lIKPivot.mValue, sizeof(lIKPivot));
if( lIKPivot.mValue[0] != 0.0 || lIKPivot.mValue[1] != 0.0 || lIKPivot.mValue[2] != 0.0 )
{
// we can't let translation change during this manipulation (to perfectly follow manipulation axis)
HIKSetTranslationActive(mCurrentState.mEffectorSetState, pHIKId, 1.0);
// kxl - I'm not convinced that we have to do all those computations.
// temp, reused
FBRVector lTempGR;
FBMatrix lTempGX;
// initial GT
FBTVector lInitialGT;
FBQuaternion lInitialGQ;
FBVector4d lInitialGS;
HIKGetEffectorStateTQSdv(mInitialState.mEffectorSetState, pHIKId, lInitialGT.mValue, lInitialGQ.mValue, lInitialGS.mValue );
lIKPivot.mValue[0] = -1.0 * lIKPivot.mValue[0];
lIKPivot.mValue[1] = -1.0 * lIKPivot.mValue[1];
lIKPivot.mValue[2] = -1.0 * lIKPivot.mValue[2];
// taken from Translation_ApplyPivot, but don't call directly! (it makes checks again and set HIKSetRotationActive which should not be done from this point!)
FBQuaternionToRotation(lTempGR, lInitialGQ);
FBTRSToMatrix(lTempGX, lInitialGT, lTempGR, *(FBSVector*)&lInitialGS);
FBVectorMatrixMult(lInitialGT, lTempGX, lIKPivot);
// current GT
FBQuaternionToRotation(lTempGR, pGQ);
FBTRSToMatrix(lTempGX, FBTVector(), lTempGR, *(FBSVector*)&pGS);
FBVectorMatrixMult(pGT, lTempGX, lIKPivot);
// new one
FBSub(pGT,lInitialGT,pGT);
}
}
void ORCharacterManipulatorCtrlSet::ManipulationStart(FBManipMode pManipMode, const FBTime *pTime)
{
// kxl: I left old code on purpose...we should clean and remove the need of extra mHIKCurrentControlRigHostEvaluator!!!!
#if 1
// Sync Rig on Skeleton First
ORCharacterSolver_HIK* lConstraintSolver = GetORCharacterSolver_HIK();
FBEvaluateInfo *lDispEvalInfo = FBGetDisplayInfo();
FBEvaluateInfo *lEvalInfo = lDispEvalInfo;
bool lBackEvalUsed = false;
if(lConstraintSolver)
{
if ( pTime )
{
lEvalInfo = lConstraintSolver->BackgroundEvaluateInfoBegin( lDispEvalInfo );
lEvalInfo->Goto( *pTime );
lBackEvalUsed = true;
}
mHIKCharacterHost.ReadCharacterAndPropertiesForSolve( mHIKCharacter, mCurrentState.mPropertySetState, lEvalInfo);
// if we are in relative roll extraction, solve should perform roll extraction
// otherwise, we don't solve roll (in absolute mode)
const bool lRelativeRollExtraction = (HIKGetPropertyValue(mCurrentState.mPropertySetState, HIKRollExtractionMode) == 0);
// temporarily disable blending with auxiliary effectors in following solving process. It removes unnecessary influence from
// auxiliary effectors for MOBU-8422 ( refer to its JIRA page for more info)
lConstraintSolver->SetBlendAuxiliaryWithEffector(false);
// solving with constraint to detect and support double solve
lConstraintSolver->ControlRigInputEvaluateAndWrite(lEvalInfo, &mCurrentState, true, lConstraintSolver->GetDoubleSolve(), false, lRelativeRollExtraction ? 0 : HIKSolvingStepRollExtraction);
// restore blending with auxiliary effectors
lConstraintSolver->SetBlendAuxiliaryWithEffector(true);
// effectors should be sync before we start manipulating (we should use ControlRigHostEvaluator from lConstraintSolver!!! )
mHIKCurrentControlRigHostEvaluator.EffectorStateFromControlSet( &mCurrentState );
}
else
{
OR_HIK_ASSERT(lConstraintSolver);
}
HIKCharacterStateCopy( mInitialState.mState, mCurrentState.mState );
HIKEffectorStateCopy( mInitialState.mEffectorSetState, mCurrentState.mEffectorSetState );
#else
FBEvaluateInfo *lEvalInfo = FBGetDisplayInfo();
// Sync Rig on Skeleton First
ORCharacterSolver_HIK* lConstraint = GetORCharacterSolver_HIK();
FBEvaluateInfo* lEvaluation = lConstraint ? lConstraint->BackgroundEvaluateInfoBegin(lEvalInfo) : lEvalInfo;
// read candidate and solve rig
mHIKCurrentControlRigHostEvaluator.Read(lEvaluation,&mCurrentState);
// are we in relative extraction mode
if(HIKGetPropertyValue(mCurrentState.mPropertySetState, HIKRollExtractionMode) == 0)
{
mHIKCurrentControlRigHostEvaluator.SetSolvingStep(HIKSolvingStepAll);
}
// absolute extraction mode - we don't solve roll extraction, it will be only on skeleton
// this is done to avoid accumulation of extraction at each manipulation/key
else
{
mHIKCurrentControlRigHostEvaluator.SetSolvingStep(HIKSolvingStepAll & ~HIKSolvingStepRollExtraction);
}
mHIKCurrentControlRigHostEvaluator.Solve(&mCurrentState);
mHIKCurrentControlRigHostEvaluator.EffectorStateFromControlSet(&mCurrentState);
//copy what we restore
HIKCharacterStateCopy( mInitialState.mState, mCurrentState.mState );
HIKEffectorStateCopy( mInitialState.mEffectorSetState, mCurrentState.mEffectorSetState );
if(lConstraint)
{
lConstraint->BackgroundEvaluateInfoEnd(lEvaluation);
}
#endif
// Add Transforms to Undos before doing any change
if (!mHIKUndoSetup) {
UndoSetup(GetCharacter());
mHIKUndoSetup = true;
}
// this will set the manipulation Parameters
PrepareSetForManip(pManipMode, lEvalInfo);
if (lBackEvalUsed)
{
lConstraintSolver->BackgroundEvaluateInfoEnd(lEvalInfo);
}
}
void ORCharacterManipulatorCtrlSet::PrepareSetForManip(FBManipMode pManipMode, FBEvaluateInfo *pEvalInfo)
{
FBCharacter *lCharacter = GetCharacter();
const FBEffectorId lManipulatedEffectorId = GetCorrespondingEffector();
const FBBodyPartId lManipulatedBodyPart = FBGetEffectorBodyPart( lManipulatedEffectorId );
bool lActiveBodyPart[kFBLastCtrlSetPartIndex];
lCharacter->GetActiveBodyPart(lActiveBodyPart);
bool lIsFullBody = IsFullBody();
bool lIsReachOverrride = IsReachOverride();
bool lIsReleasePinPressed = IsReleasePinPressed();
// as MB solver, active body part adjustment should only work under Full Body mode
if ( lIsFullBody )
{
lActiveBodyPart[kFBCtrlSetPartHips] |= 0;
lActiveBodyPart[kFBCtrlSetPartChest] |= lActiveBodyPart[kFBCtrlSetPartHips];
lActiveBodyPart[kFBCtrlSetPartLeftArm] |= lActiveBodyPart[kFBCtrlSetPartChest];
lActiveBodyPart[kFBCtrlSetPartRightArm] |= lActiveBodyPart[kFBCtrlSetPartChest];
lActiveBodyPart[kFBCtrlSetPartLeftLeg] |= lActiveBodyPart[kFBCtrlSetPartHips];
lActiveBodyPart[kFBCtrlSetPartRightLeg] |= lActiveBodyPart[kFBCtrlSetPartHips];
lActiveBodyPart[kFBCtrlSetPartHead] |= lActiveBodyPart[kFBCtrlSetPartChest];
lActiveBodyPart[kFBCtrlSetPartLeftHand] |= lActiveBodyPart[kFBCtrlSetPartLeftArm];
lActiveBodyPart[kFBCtrlSetPartRightHand] |= lActiveBodyPart[kFBCtrlSetPartRightArm];
lActiveBodyPart[kFBCtrlSetPartLeftFoot] |= lActiveBodyPart[kFBCtrlSetPartLeftLeg];
lActiveBodyPart[kFBCtrlSetPartRightFoot] |= lActiveBodyPart[kFBCtrlSetPartRightLeg];
}
FBBodyPartId lBodyPart;
float lReachR, lReachT;
// Adjust Reach according to Pinning and BodyPart active
for(int lIter = kFBHipsEffectorId; lIter < kFBLastEffectorId; lIter++)
{
int lHIKId = FBEffectorIdToHIKId( (FBEffectorId)lIter );
if (lHIKId != -1)
{
lBodyPart = FBGetEffectorBodyPart( (FBEffectorId) lIter );
lReachR = mHIKCurrentControlRigHostEvaluator.mHIKControlRigHost->GetEffector(lIter, 0).ReadReachR( pEvalInfo );
lReachT = mHIKCurrentControlRigHostEvaluator.mHIKControlRigHost->GetEffector(lIter, 0).ReadReachT( pEvalInfo );
if ( lIsFullBody )
{
if ( lIsReachOverrride )
{
lReachR = 0;
lReachT = 0;
}
if ( lIsReachOverrride && !lIsReleasePinPressed )
{
lReachR = (lCharacter->IsRotationPin((FBEffectorId)lIter)) ? 1.0 : lReachR;
lReachT = (lCharacter->IsTranslationPin((FBEffectorId)lIter)) ? 1.0 : lReachT;
}
}
//body part
else if ( lActiveBodyPart[lBodyPart] && ( lIsReachOverrride || lIsReleasePinPressed ) )
{
if ( lIsReleasePinPressed )
{
lReachT = 0.0;
lReachR = 0.0;
}
else
{
lReachT = lCharacter->IsTranslationPin((FBEffectorId)lIter) ? 1 : 0;
lReachR = lCharacter->IsRotationPin((FBEffectorId)lIter) ? 1 : 0;
}
}
HIKSetRotationActive(mCurrentState.mEffectorSetState, lHIKId, lReachR);
HIKSetTranslationActive(mCurrentState.mEffectorSetState, lHIKId, lReachT);
}
}
// Adjust Resist
if( IsStiffnessOverride() )
{
SetResist(0.0);
HIKSetResist(mCurrentState.mEffectorSetState, HipsEffectorId, HIKGetRotationActive(mCurrentState.mEffectorSetState, HipsEffectorId) );
HIKSetResist(mCurrentState.mEffectorSetState, ChestEndEffectorId, HIKGetRotationActive(mCurrentState.mEffectorSetState, ChestEndEffectorId) );
if ( lIsFullBody )
{
if( HIKGetPropertyValue(mCurrentState.mPropertySetState, HIKPostureId) == 0 ) // biped
{
HIKSetResist(mCurrentState.mEffectorSetState, LeftShoulderEffectorId, 0.35f);
HIKSetResist(mCurrentState.mEffectorSetState, RightShoulderEffectorId, 0.35f);
}
HIKSetResist(mCurrentState.mEffectorSetState, HeadEffectorId, 1.0); // This needs to be here; it was here but removed at some point. MOBU-7102 needs this.
}
}
if( lIsReachOverrride )
{
if ( !lIsReleasePinPressed )
{
float lValue;
for(int lIter = HipsEffectorId; lIter < LastEffectorId; lIter++)
{
lBodyPart = FBGetEffectorBodyPart( (FBEffectorId) lIter );
if(lIter <= RightHandEffectorId && ( lIsFullBody || lActiveBodyPart[lBodyPart]) )
{
// In BodyPart, we respect the pinning only on the active Part
lValue = (lCharacter->IsTranslationPin((FBEffectorId)lIter)) ? 1 : 0;
HIKSetPull( mCurrentState.mEffectorSetState, lIter, lValue );
}
else
{
HIKSetPull( mCurrentState.mEffectorSetState, lIter, 0.0 );
}
}
}
if ( lIsFullBody ) // Full Body
{
// Left/Right
HIKSetPull( mCurrentState.mEffectorSetState, RightWristEffectorId, 1.0 );
HIKSetPull( mCurrentState.mEffectorSetState, LeftWristEffectorId, 1.0 );
}
}
// Some small adjustment to have the same behavior as Motionbuilder.
mManipulatedEffectorIdArray[mManipulatedEffectorsCount] = lManipulatedEffectorId;
mManipulatedEffectorsCount++;
for( int i=0; i< mManipulatedEffectorsCount; i++ )
{
int lId = mManipulatedEffectorIdArray[i];
if( lId == kFBHipsEffectorId && !lCharacter->IsRotationPin(kFBHipsEffectorId))
{
if( HIKGetResist(mCurrentState.mEffectorSetState, HipsEffectorId) != 1.0 )
HIKSetResist( mCurrentState.mEffectorSetState, HipsEffectorId, 1.0 ); // Smoother Rotation on Hips
}
else if ( lId == kFBLeftShoulderEffectorId ) // Keep Arm Rotations
{
if(HIKGetRotationActive( mCurrentState.mEffectorSetState, LeftShoulderEffectorId ) != 1.0 )
HIKSetRotationActive( mCurrentState.mEffectorSetState, LeftShoulderEffectorId, 1.0);
}
else if ( lId == kFBRightShoulderEffectorId ) // Keep Arm Rotations
{
if(HIKGetRotationActive( mCurrentState.mEffectorSetState, RightShoulderEffectorId ) != 1.0 )
HIKSetRotationActive( mCurrentState.mEffectorSetState, RightShoulderEffectorId, 1.0);
}
}
// better feed back
HIKSetPropertyValue(mCurrentState.mPropertySetState, HIKPullIterationCount, 30);
}
void ORCharacterManipulatorCtrlSet::CleanForManipulation()
{
// restore undo setup for next manipulation
mHIKUndoSetup = false;
// restore mManipulatedEffectorsCount
mManipulatedEffectorsCount = 0;
memset(mManipulatedEffectorIdArray,0,sizeof(float)*LastEffectorId);
}
bool ORCharacterManipulatorCtrlSet::NeedRotationSync(FBEffectorId pEffectorIndex,double pAngleTol)
{
static const double K_180_PI = 57.295779513082320876798154814105170332405472466565; // 180/pi
bool lRet = false;
FBCharacter *lCharacter = GetCharacter();
if(lCharacter != NULL)
{
FBControlSet *lControlSet = lCharacter->GetCurrentControlSet();
if( lControlSet->ControlSetType == kFBControlSetTypeFKIK)
{
if( pEffectorIndex == kFBHeadEffectorId ||
pEffectorIndex == kFBLeftHandThumbEffectorId ||
pEffectorIndex == kFBLeftHandIndexEffectorId ||
pEffectorIndex == kFBLeftHandMiddleEffectorId ||
pEffectorIndex == kFBLeftHandRingEffectorId ||
pEffectorIndex == kFBLeftHandPinkyEffectorId ||
pEffectorIndex == kFBLeftHandExtraFingerEffectorId ||
pEffectorIndex == kFBRightHandThumbEffectorId ||
pEffectorIndex == kFBRightHandIndexEffectorId ||
pEffectorIndex == kFBRightHandMiddleEffectorId ||
pEffectorIndex == kFBRightHandRingEffectorId ||
pEffectorIndex == kFBRightHandPinkyEffectorId ||
pEffectorIndex == kFBRightHandExtraFingerEffectorId ||
pEffectorIndex == kFBLeftFootThumbEffectorId ||
pEffectorIndex == kFBLeftFootIndexEffectorId ||
pEffectorIndex == kFBLeftFootMiddleEffectorId ||
pEffectorIndex == kFBLeftFootRingEffectorId ||
pEffectorIndex == kFBLeftFootPinkyEffectorId ||
pEffectorIndex == kFBLeftFootExtraFingerEffectorId ||
pEffectorIndex == kFBRightFootThumbEffectorId ||
pEffectorIndex == kFBRightFootIndexEffectorId ||
pEffectorIndex == kFBRightFootMiddleEffectorId ||
pEffectorIndex == kFBRightFootRingEffectorId ||
pEffectorIndex == kFBRightFootPinkyEffectorId ||
{
FBMatrix lInitialGlobalTransformation, lInvCurrentGlobalRot, lInitialGlobalRotM, lDeltaRotM;
FBQuaternion lDeltaRotQ;
FBRVector lRotV;
int lHIKId = FBEffectorIdToHIKId( pEffectorIndex );
if (lHIKId != -1)
{
FBModel *lModel = lCharacter->GetEffectorModel( pEffectorIndex );
lModel->GetMatrix(lInvCurrentGlobalRot, kModelInverse_Rotation, true, FBGetDisplayInfo() );
HIKGetEffectorStatedv( mInitialState.mEffectorSetState, lHIKId, (double*)lInitialGlobalTransformation);
FBMatrixToRotation( lRotV, lInitialGlobalTransformation);
FBRotationToMatrix ( lInitialGlobalRotM, lRotV);
FBMatrixMult(lDeltaRotM, lInitialGlobalRotM, lInvCurrentGlobalRot);
FBMatrixToQuaternion( lDeltaRotQ, lDeltaRotM );
lRet = 2.0 * acos(lDeltaRotQ[3]) * K_180_PI > pAngleTol;
}
}
}
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::AddPostRotation(FBModel *pModel, FBMatrix &pMatrix)
{
if( pModel && pModel->RotationActive && !pModel->RotationSpaceForLimitOnly )
{
FBRVector lPostRotationV( pModel->PostRotation );
FBMatrix lPostMult;
FBRotationToMatrix(lPostMult, lPostRotationV);
FBMatrixMult(pMatrix, pMatrix, lPostMult);
}
}
void ORCharacterManipulatorCtrlSet::RemovePostRotation(FBModel *pModel, FBMatrix &pMatrix)
{
if( pModel && pModel->RotationActive && !pModel->RotationSpaceForLimitOnly )
{
FBRVector lPostRotationV( pModel->PostRotation );
FBMatrix lPostMultInv;
FBRotationToMatrix(lPostMultInv, lPostRotationV);
FBMatrixInverse(lPostMultInv,lPostMultInv);
FBMatrixMult(pMatrix, pMatrix, lPostMultInv);
}
}
void ORCharacterManipulatorCtrlSet::GetNormalizedLocalState( KHIKNodeState pDataSet[LastNodeId] ) const
{
int lNodeIdDesc[LastNodeId + 1];
HIKDataDescription lDescription = {
HIKDataDescription::HIKLocalSpace,
offsetof(KHIKNodeState, mTfv),
offsetof(KHIKNodeState, mQfv),
offsetof(KHIKNodeState, mSfv),
sizeof(KHIKNodeState),
lNodeIdDesc
};
HIKFillCharacterDescription(mHIKCharacter, lNodeIdDesc);
HIKGetNormalizedCharacterStateTransformTQS( mHIKCurrentControlRigHostEvaluator.mHIKCharacter, mCurrentState.mState, &lDescription, pDataSet);
}
void ORCharacterManipulatorCtrlSet::SetNormalizedLocalState( KHIKNodeState pDataSet[LastNodeId] )
{
int lNodeIdDesc[LastNodeId + 1];
HIKDataDescription lDescription = {
HIKDataDescription::HIKLocalSpace,
offsetof(KHIKNodeState, mTfv),
offsetof(KHIKNodeState, mQfv),
offsetof(KHIKNodeState, mSfv),
sizeof(KHIKNodeState),
lNodeIdDesc
};
HIKFillCharacterDescription(mHIKCharacter, lNodeIdDesc);
HIKSetNormalizedCharacterStateTransformTQS(mHIKCharacter, mCurrentState.mState, &lDescription, pDataSet);
}
ORCharacterSolver_HIK* ORCharacterManipulatorCtrlSet::GetORCharacterSolver_HIK(FBCharacter* pCharacter)
{
FBCharacterSolver* lExtSolver;
if(pCharacter)
lExtSolver = pCharacter->GetExternalSolver();
else if(GetCharacter())
lExtSolver = GetCharacter()->GetExternalSolver();
else
return NULL;
if(lExtSolver)
{
return dynamic_cast<ORCharacterSolver_HIK*>(lExtSolver);
}
return NULL;
}
HIKNodeId ORCharacterManipulatorCtrlSet::GetManipulatedHIKNodeId()
{
FBBodyNodeId lBodyNodeIndex = GetManipulatedBodyNode();
if( lBodyNodeIndex != kFBInvalidNodeId )
{
return FBBodyNodeIdToHIKNodeId(lBodyNodeIndex);
}
else
{
int lExtraFKIndex = GetManipulatedNodeExtraFKIndex();
if ( lExtraFKIndex == LEFT_EXTRA_COLLAR)
{
}
else if ( lExtraFKIndex == RIGHT_EXTRA_COLLAR )
{
}
}
return LastNodeId;
}
int ORCharacterManipulatorCtrlSet::GetManipulatedNodeExtraFKIndex()
{
FBCharacterSolver* lSolver = GetCharacter() ? GetCharacter()->GetExternalSolver() : NULL;
if (lSolver)
{
FBModel* lModel = GetManipulatedNode();
const int lCount = lSolver->GetExtraFKCount();
for ( int lIter = 0; lIter < lCount; lIter++ )
{
if ( lModel == lSolver->GetExtraFKModelAt(lIter) )
{
return lIter;
}
}
}
return -1;
}
int ORCharacterManipulatorCtrlSet::GetManipulatedNodeExtraBoneIndex()
{
FBCharacterSolver * lSolver = GetCharacter() ? GetCharacter()->GetExternalSolver() : NULL;
if (lSolver)
{
FBModel* lModel = GetManipulatedNode();
const int lCount = lSolver->GetExtraBoneCount();
for ( int lIter = 0; lIter < lCount; lIter++ )
{
if ( lModel == lSolver->GetExtraBoneModelAt(lIter) )
{
return lIter;
}
}
}
return -1;
}
FBEffectorId ORCharacterManipulatorCtrlSet::GetCorrespondingEffector()
{
FBEffectorId lEffectorId = GetManipulatedEffectorId();
if ( lEffectorId == kFBInvalidEffectorId )
{
if ( GetManipulatorMode() == FBManip_EditorTranslation )
{
const int lFKIndex = GetManipulatedNodeExtraFKIndex();
if ( lFKIndex == LEFT_EXTRA_COLLAR )
{
}
else if ( lFKIndex == RIGHT_EXTRA_COLLAR )
{
}
}
}
return lEffectorId;
}
bool ORCharacterManipulatorCtrlSet::IsManipulated_FK()
{
FBBodyNodeId lBodyNode = GetManipulatedBodyNode();
if (lBodyNode == kFBInvalidNodeId)
{
return (GetManipulatedNodeExtraFKIndex() != -1);
}
else
{
return true;
}
}
bool ORCharacterManipulatorCtrlSet::IsManipulated_IK()
{
return !IsManipulated_FK() && (GetManipulatedEffectorId() != kFBInvalidEffectorId); //get manipulated effector does give valid index for FK models that are manipulating IK
}