constraints/CharacterSolver/HIK2013Solver/orcharactersolver_hik_manipulator.cxx

constraints/CharacterSolver/HIK2013Solver/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 );
#ifndef _WIN32
#define _aligned_malloc(size, alignment) memalign(alignment, size)
#define _aligned_free(p) free(p)
#endif
/************************************************
* Creation function.
************************************************/
bool ORCharacterManipulatorCtrlSet::FBCreate()
{
mHIKCharacter = NULL;
mManipulatedObjectCount = 0;
mCurrentObjectManipulatedCount = 0;
mDataSet = (KHIKNodeState*)_aligned_malloc( sizeof(KHIKNodeState) * LastNodeId, 16);
return true;
}
/************************************************
* Destruction function.
************************************************/
void ORCharacterManipulatorCtrlSet::FBDestroy()
{
DeallocateState();
_aligned_free(mDataSet);
mDataSet = NULL;
FBCharacterManipulatorCtrlSet::FBDestroy();
}
//TRANSLATION
FBManipulationGetResult ORCharacterManipulatorCtrlSet::TranslationIsEditable( bool &pEditable, FBReferencialType pRefType )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
pEditable = true;
}
return lRet;
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::TranslationSet( FBTVector &pT, FBReferencialType pRefType, FBSetType pSetType, FBSetWhat pSetWhat )
{
FBCharacter *lCharacter = GetCharacter();
if ( lCharacter->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
FBEffectorId lEffectorIndex = GetCorrespondingEffector();
if(lEffectorIndex != kFBInvalidEffectorId)
{
int lHIKId = HIKEffectorIdFromEffectorName( FBCharacterEffectorNameFromId( lEffectorIndex ) );
if(lHIKId != -1)
{
HIKSetTranslationActive(mCurrentState.mCREffectorSetState, lHIKId, 1.0);
if(IsReachOverride())
{
HIKSetPull( mCurrentState.mCREffectorSetState, lHIKId, 0.0 );
if(!IsDescendantPulling( lEffectorIndex ))
{
HIKSetPull( mCurrentState.mCREffectorSetState, lHIKId, 1.0 );
}
}
if( pRefType == FBRefTypeGlobal )
{
FBVector4d lGTVector;
FBQuaternion lInitialGR, lGRVector;
FBVector4d lGSVector;
FBTVector lTBackup(pT);
HIKGetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lGTVector.mValue, lInitialGR.mValue, lGSVector.mValue );
TranslationSetApplyPivot(lTBackup, pRefType);
HIKSetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lTBackup.mValue, lInitialGR.mValue, lGSVector.mValue );
ApplySync( FBGetDisplayInfo() );
mCurrentObjectManipulatedCount++;
if(mManipulatedObjectCount == mCurrentObjectManipulatedCount)
{
mCurrentObjectManipulatedCount = 0;
RestoreInitialState();
}
}
else if(pRefType == FBRefTypeLocal)
{
// Convert to Global Coordinate
FBEffectorSetID lEffectorSet = GetEffectorSet();
FBModel *lModel = lCharacter->GetEffectorModel(lEffectorIndex, lEffectorSet);
FBVector3d l3DLocalTrans, l3dGlobalTrans;
lModel->GetVector(l3DLocalTrans, kModelTranslation, false );
lModel->GetVector(l3dGlobalTrans, kModelTranslation, true );
FBTVector lLocalTranslation(l3DLocalTrans[0], l3DLocalTrans[1], l3DLocalTrans[2], 0);
FBTVector lGlobalTranslation(l3dGlobalTrans[0], l3dGlobalTrans[1], l3dGlobalTrans[2], 0);
FBTVector lDeltaTranslation;
FBSub(lDeltaTranslation, lLocalTranslation, pT);
FBAdd(lGlobalTranslation, lLocalTranslation, lDeltaTranslation);
TranslationSet(lGlobalTranslation, FBRefTypeGlobal, pSetType, pSetWhat);
}
else
{
//
}
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() )
{
FBBodyNodeId lBodyNode = GetManipulatedBodyNode();
int lExtraFKIndex = -1;
if (lBodyNode == kFBInvalidNodeId)
lExtraFKIndex = GetManipulatedNodeExtraFKIndex();
if( lBodyNode != kFBInvalidNodeId || lExtraFKIndex != -1) // 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)
{
FBVector3d lVector;
GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(lEffectorIndex, GetEffectorSet())->GetVector(lVector, kModelTranslation, true, lEvaluateInfo);
pT[0] = lVector[0];
pT[1] = lVector[1];
pT[2] = lVector[2];
}
else if(pRefType == FBRefTypeLocal)
{
FBVector3d lVector;
GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(lEffectorIndex, GetEffectorSet())->GetVector(lVector, kModelTranslation, false, lEvaluateInfo);
pT[0] = lVector[0];
pT[1] = lVector[1];
pT[2] = lVector[2];
}
}
}
}
}
return lResult;
}
FBManipulationStartResult ORCharacterManipulatorCtrlSet::TranslationStartManipulation( FBReferencialType pRefType, const FBTime* pTime )
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
ManipulationStart();
}
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::TranslationStopManipulation ()
{
}
//ROTATION
FBManipulationGetResult ORCharacterManipulatorCtrlSet::RotationIsEditable( bool &pEditable, FBReferencialType pRefType )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
pEditable = true;
}
return lRet;
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::RotationSet( FBMatrix &pRM, FBReferencialType pRefType, FBSetType pSetType, bool pOnlyRoot )
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
FBEffectorId lEffectorIndex = GetCorrespondingEffector();
if( lEffectorIndex != kFBInvalidEffectorId )
{
if( pRefType == FBRefTypeGlobal )
{
lResult = RotationSetEffectorGlobal(pRM);
}
else if ( pRefType == FBRefTypeLocal )
{
lResult = RotationSetEffectorLocal(pRM);
}
RotationSetApplyPivot(lEffectorIndex );
}
else
{
FBBodyNodeId lBodyNodeIndex = GetManipulatedBodyNode();
if ( lBodyNodeIndex == kFBReferenceNodeId )
{
}
else
{
int lFKIndex = -1;
if ( lBodyNodeIndex == kFBInvalidNodeId )
lFKIndex = GetManipulatedNodeExtraFKIndex();
if ( lBodyNodeIndex != kFBInvalidNodeId || lFKIndex != -1)
{
FBModel *lModel = NULL;
if ( lBodyNodeIndex != kFBInvalidNodeId )
lModel = GetCharacter()->GetCtrlRigModel(lBodyNodeIndex);
else
{
FBCharacterSolver* lExtSolver = GetCharacter()->GetExternalSolver();
if (lExtSolver)
lModel = lExtSolver->GetExtraFKModelAt(lFKIndex);
}
if ( pRefType == FBRefTypeGlobal )
{
lResult = RotationSetFKGlobal(lModel, pRM);
}
else if ( pRefType == FBRefTypeLocal )
{
RotationSetFKLocal(lModel, pRM);
}
}
}
}
ApplySync( FBGetDisplayInfo(), true, true );
mCurrentObjectManipulatedCount++;
if(mManipulatedObjectCount == mCurrentObjectManipulatedCount)
{
mCurrentObjectManipulatedCount = 0;
RestoreInitialState();
}
}
}
return lResult;
}
FBManipulationGetResult ORCharacterManipulatorCtrlSet::RotationGet( FBMatrix &pRM, FBReferencialType pRefType )
{
FBCharacter* lCharacter = GetCharacter();
if( GetCharacter()->IsCtrlSetReady() )
{
if( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
// 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
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, false, lEvaluateInfo);
}
else
{
//Unsupported RefType !
}
}
}
}
}
return lRet;
}
FBManipulationStartResult ORCharacterManipulatorCtrlSet::RotationStartManipulation( FBReferencialType pRefType )
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
ManipulationStart();
}
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::RotationStopManipulation ()
{
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::RotationSetEffectorGlobal( FBMatrix &pRM )
{
int lHIKId = -1;
FBEffectorId lEffectorIndex = GetCorrespondingEffector();
if ( lEffectorIndex != kFBInvalidEffectorId )
lHIKId = HIKEffectorIdFromEffectorName( FBCharacterEffectorNameFromId( lEffectorIndex ) );
if(lHIKId != -1)
{
FBEffectorSetID lEffectorSet = GetEffectorSet();
FBModel *lModel = GetCharacter()->GetEffectorModel(lEffectorIndex, lEffectorSet);
// IK and FK can have different offset. When manipulating FK we should remove FK model post rotation, not IK.
FBBodyNodeId lBodyNode = GetManipulatedBodyNode();
if ( lBodyNode != kFBInvalidNodeId )
lModel = GetCharacter()->GetCtrlRigModel(lBodyNode);
if( NeedRotationSync(lEffectorIndex,120.0) )
{
ManipulationStart();
}
HIKSetRotationActive(mCurrentState.mCREffectorSetState, lHIKId, 1.0);
FBMatrix lResultRotation(pRM);
FBVector4d lGTVector;
FBQuaternion lGRQuat;
FBVector4d lGSVector;
HIKGetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lGTVector.mValue, lGRQuat.mValue, lGSVector.mValue );
RemovePostRotation(lModel, lResultRotation);
FBRVector lGRVector;
FBMatrixToRotation(lGRVector, lResultRotation);
FBRotationToQuaternion(lGRQuat, lGRVector);
HIKSetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lGTVector.mValue, lGRQuat.mValue, lGSVector.mValue );
}
return kFBManipulationSetFail; // We don't want the System to Set the Value, we did it ourself
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::RotationSetEffectorLocal( FBMatrix &pRM )
{
int lHIKId = -1;
FBManipulationSetResult lResult = kFBManipulationSetFail; // We don't want the System to Set the Value, we did it ourself
FBEffectorId lEffectorIndex = GetCorrespondingEffector();
if ( lEffectorIndex != kFBInvalidEffectorId )
lHIKId = HIKEffectorIdFromEffectorName( FBCharacterEffectorNameFromId( lEffectorIndex ) );
if(lHIKId != -1)
{
FBEffectorSetID lEffectorSet = GetEffectorSet();
FBModel *lModel = NULL;
FBBodyNodeId lBodyNodeIndex = GetManipulatedBodyNode();
int lFKIndex = -1;
if ( lBodyNodeIndex == kFBInvalidNodeId )
lFKIndex = GetManipulatedNodeExtraFKIndex();
if ( lBodyNodeIndex != kFBInvalidNodeId || lFKIndex != -1)
{
if ( lBodyNodeIndex != kFBInvalidNodeId )
lModel = GetCharacter()->GetCtrlRigModel(lBodyNodeIndex);
else
{
FBCharacterSolver* lExtSolver = GetCharacter()->GetExternalSolver();
if (lExtSolver)
lModel = lExtSolver->GetExtraFKModelAt(lFKIndex);
}
}
else
lModel = GetCharacter()->GetEffectorModel(lEffectorIndex, lEffectorSet);
if(lModel)
{
FBMatrix lGlobalRotation;
if ( lModel->Parent )
{
FBMatrix lParentGlobalRotation;
lModel->Parent->GetMatrix( lParentGlobalRotation, kModelRotation, true );
AddPostRotation(lModel, lParentGlobalRotation);
FBMatrixMult(lGlobalRotation, lParentGlobalRotation, pRM);
}
else
{
AddPostRotation(lModel, lGlobalRotation);
FBMatrixMult(lGlobalRotation, lGlobalRotation, pRM);
}
lResult = RotationSetEffectorGlobal(lGlobalRotation);
}
}
return lResult;
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::RotationSetFKGlobal( FBModel *pModel, FBMatrix &pRM )
{
FBRVector lRVector;
FBVector4d lGTVector, lGSVector;
FBQuaternion lGRQuat;
FBMatrix lRM(pRM);
HIKNodeId lNodeId = LastNodeId;
FBBodyNodeId lBodyNodeIndex = GetManipulatedBodyNode();
if ( lBodyNodeIndex != kFBInvalidNodeId )
lNodeId = gFBBodyNodeToHIKNodeId[lBodyNodeIndex];
else
{
FBCharacterSolver *lCharacterSolver = GetCharacter()->GetExternalSolver();
if ( lCharacterSolver )
{
int lExtraFKIndex = GetManipulatedNodeExtraFKIndex();
if ( lExtraFKIndex == LEFT_EXTRA_COLLAR)
lNodeId = LeftCollarExtraNodeId;
else if ( lExtraFKIndex == RIGHT_EXTRA_COLLAR )
lNodeId = RightCollarExtraNodeId;
}
}
RemovePostRotation(pModel, lRM);
GetNormalizedLocalState(mDataSet);
int const lParentNodeId = HIKGetParentNodeId(mHIKCharacter, lNodeId);
if(lParentNodeId != -1)
{
FBMatrix lMatrix;
HIKGetNodeNormalizedStateTQSdv( mHIKCharacter, mCurrentState.mCRState, lParentNodeId, lGTVector.mValue, lGRQuat.mValue, lGSVector.mValue );
FBQuaternionToRotation(lRVector, lGRQuat);
FBRotationToMatrix(lMatrix, lRVector);
FBMatrixInverse(lMatrix, lMatrix);
FBMatrixMult(lRM, lMatrix, lRM); // Inverse Parent x Global Matrix
}
FBMatrixToRotation(lRVector, lRM);
FBRotationToQuaternion(lGRQuat, lRVector);
GetNormalizedLocalState(mDataSet);
mDataSet[lNodeId].mQfv[0] = lGRQuat[0]; mDataSet[lNodeId].mQfv[1] = lGRQuat[1]; mDataSet[lNodeId].mQfv[2] = lGRQuat[2]; mDataSet[lNodeId].mQfv[3] = lGRQuat[3];
SetNormalizedLocalState(mDataSet);
return kFBManipulationSetFail; // We don't want the System to Set the Value, we did it ourself
}
FBManipulationSetResult ORCharacterManipulatorCtrlSet::RotationSetFKLocal( FBModel *pModel, FBMatrix &pRM )
{
// Transform To Global and Let the Global Manipulation Update the Character
FBMatrix lGlobalRotation;
if ( pModel->Parent )
{
FBMatrix lParentGlobalRotation;
pModel->Parent->GetMatrix( lParentGlobalRotation, kModelRotation, true );
AddPostRotation(pModel, lParentGlobalRotation);
FBMatrixMult(lGlobalRotation, lParentGlobalRotation, pRM);
}
else
{
AddPostRotation(pModel, lGlobalRotation);
FBMatrixMult(lGlobalRotation, lGlobalRotation, pRM);
}
RotationSetFKGlobal(pModel, lGlobalRotation);
return kFBManipulationSetFail; // We don't want the System to Set the Value, we did it ourself
}
//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 ()
{}
void ORCharacterManipulatorCtrlSet::AllocateState(FBCharacter* pCharacter)
{
HIKHostPropertiesInit(mHIKCharacterHost);
HIKCharacterHostFromMBCharacterHierarchy(mHIKCharacterHost,pCharacter, NULL);
//check for additionnal properties...
FBCharacterSolver* lExtSolver = pCharacter->GetExternalSolver();
ORCharacterSolver_HIK* lConstraint = NULL;
FBModel* lLeft = NULL;
FBModel* lRight = NULL;
if(lExtSolver)
{
lConstraint = dynamic_cast<ORCharacterSolver_HIK*>(lExtSolver);
if(lConstraint)
{
lLeft = lConstraint->GetExtraBoneModelAt(LEFT_EXTRA_COLLAR);
lRight = lConstraint->GetExtraBoneModelAt(RIGHT_EXTRA_COLLAR);
//Add the 2 new clavicle bones
HIKCharacterHostFromHIKHostMB(mHIKCharacterHost, lLeft, lRight, NULL);
}
}
if (!mHIKCharacter)
mHIKCharacter = mHIKCharacterHost.CharacterCreate(&malloc);
HIKCharacterHostFromMBCharacterGeometry(mHIKCharacterHost,mHIKCharacter,pCharacter);
if(lConstraint && (lLeft || lRight) )
{
//Characterize the 2 new clavicle bones
FBVector3d lLeftT, lRightT;
FBRVector lLeftR, lRightR, lLeftRPOff, lRightRPOff;
FBSVector lLeftS, lRightS;
lConstraint->GetTransformOffset(lLeftT, lLeftR, lLeftS, 0);
lConstraint->GetParentRotationOffset(lLeftRPOff, 0);
lConstraint->GetTransformOffset(lRightT, lRightR, lRightS, 1);
lConstraint->GetParentRotationOffset(lRightRPOff, 1);
FBQuaternion lLeftQ, lRightQ;
FBQuaternion lLeftQPOff, lRightQPOff;
FBRotationToQuaternion(lLeftQ, lLeftR);
FBRotationToQuaternion(lRightQ, lRightR);
FBRotationToQuaternion(lLeftQPOff, lLeftRPOff);
FBRotationToQuaternion(lRightQPOff, lRightRPOff);
CharacterizeHIKCharacterHostFromHIKHostMB(mHIKCharacterHost,mHIKCharacter,
lLeftT, lLeftQ, lLeftS, lLeftQPOff,
lRightT, lRightQ, lRightS, lRightQPOff);
}
HIKCharacterizeGeometry(mHIKCharacter);
HIKInverseJoint(mHIKCharacter,HIKLeftKnee, pCharacter->InverseLeftKnee);
HIKInverseJoint(mHIKCharacter,HIKRightKnee, pCharacter->InverseRightKnee);
HIKInverseJoint(mHIKCharacter,HIKLeftElbow, pCharacter->InverseLeftElbow);
HIKInverseJoint(mHIKCharacter,HIKRightElbow, pCharacter->InverseRightElbow);
HIKHostPropertiesFromCharacter(mHIKCharacterHost,pCharacter);
lConstraint->RegisterExtraProperties(mHIKCharacterHost);
FBControlSet* lControlSet = pCharacter->GetCurrentControlSet();
if(lControlSet != NULL)
{
HIKControlRigHostFromMBControlRig(mHIKControlRigHost,lControlSet);
if(lConstraint && (lLeft || lRight) )
{
FBModel* lLeftShoulderFK = lConstraint->GetExtraFKModelAt(LEFT_EXTRA_COLLAR);
FBModel* lRightShoulderFK = lConstraint->GetExtraFKModelAt(RIGHT_EXTRA_COLLAR);
HIKExtraShoulderControlRigHost(mHIKControlRigHost, lLeftShoulderFK, lRightShoulderFK);
}
mHIKCurrentControlRigHostEvaluator.Init(mHIKCharacter,&mHIKCharacterHost,&mHIKControlRigHost, &malloc);
mHIKInitialControlRigHostEvaluator.Init(mHIKCharacter,&mHIKCharacterHost,&mHIKControlRigHost, &malloc);
mCurrentState.Init(mHIKCharacter,NULL,true,&malloc);
mInitialState.Init(mHIKCharacter,NULL,true,&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(int pEffectorId)
{
int lHIKId = HIKEffectorIdFromEffectorName( FBCharacterEffectorNameFromId( (FBEffectorId)pEffectorId ) );
bool lRet = HIKGetPull(mCurrentState.mCREffectorSetState, lHIKId) >= 0.5;
int lCount = GetDescendantEffectorCount((FBEffectorId)pEffectorId);
for(int lIter = 0; !lRet && lIter < lCount; lIter++)
{
FBEffectorId lChildEffectorId = GetDescendantEffector((FBEffectorId)pEffectorId, lIter);
lRet = IsDescendantPulling(lChildEffectorId);
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::SetResist(float pResist)
{
for(int lIter = HipsEffectorId; lIter < LastEffectorId; lIter++)
{
HIKSetResist(mCurrentState.mCREffectorSetState, lIter, pResist);
}
}
void ORCharacterManipulatorCtrlSet::SetBlendFact(float pFact)
{
for(int lIter = HipsEffectorId; lIter < LastEffectorId; lIter++)
{
HIKSetRotationActive(mCurrentState.mCREffectorSetState, lIter, pFact);
HIKSetTranslationActive(mCurrentState.mCREffectorSetState, lIter, pFact);
}
}
void ORCharacterManipulatorCtrlSet::SyncCharacter(FBEvaluateInfo *pEvalInfo)
{
//this is not properly called during manipulation, if you want to implement it, see how it was done in HIK 2014
}
void ORCharacterManipulatorCtrlSet::ApplySync(FBEvaluateInfo *pEvalInfo, bool pSolveBackOnRig /*= true*/, bool pWriteRig /*= true*/ )
{
FBCharacter *lCharacter = GetCharacter();
int lSolvingStep = HIKSolvingStepAll;
//Adjust the solving according to the Active body part
if( IsBodyPart()|| IsFullBodyNoPull() )
{
bool lActiveBodyPart[12];
lCharacter->GetActiveBodyPart(lActiveBodyPart);
for(int i = 1; i < kFBLastCtrlSetPartIndex; i++)
{
if(lActiveBodyPart[i])
{
lSolvingStep = lSolvingStep | GetSolvingStepBasedOnActivePart(i);
}
}
lSolvingStep &= ~HIKSolvingStepBodyPull;
}
if( lCharacter && lCharacter->ContactBehaviour != kFBParamContactAlwaysSync )
{
lSolvingStep &= ~HIKSolvingStepContact;
}
mHIKCurrentControlRigHostEvaluator.mSolvingStep = lSolvingStep;
//check for additionnal properties...
FBCharacterSolver* lExtSolver = lCharacter->GetExternalSolver();
if(lExtSolver)
{
ORCharacterSolver_HIK* lConstraint = dynamic_cast<ORCharacterSolver_HIK*>(lExtSolver);
if(lConstraint)
{
//set sns
int LegFilter = (HIKSolvingStepLeftLegSnS | HIKSolvingStepRightLegSnS);
if(lConstraint->LegSNS)
lSolvingStep |= LegFilter;
else
lSolvingStep &= ~LegFilter;
int ArmFilter = (HIKSolvingStepLeftArmSnS | HIKSolvingStepRightArmSnS);
if(lConstraint->ArmSNS)
lSolvingStep |= ArmFilter;
else
lSolvingStep &= ~ArmFilter;
mHIKCurrentControlRigHostEvaluator.SetSolvingStep( lSolvingStep );
}
}
mHIKCurrentControlRigHostEvaluator.Solve(&mCurrentState);
if (pSolveBackOnRig)
{
// Solve Back on Effector
mHIKCurrentControlRigHostEvaluator.EffectorStateFromControlSet(&mCurrentState);
}
if (pWriteRig)
{
// Write FK/IK Rig
mHIKCurrentControlRigHostEvaluator.WriteRigCandidate(pEvalInfo,&mCurrentState);
}
SyncEffectorPivot(pEvalInfo);
}
void ORCharacterManipulatorCtrlSet::SyncEffectorPivot(FBEvaluateInfo* pEvaluateInfo)
{
FBControlSet* lControlRig = GetCharacter()->GetCurrentControlSet();
for(int lIter = 0; lIter < kFBLastEffectorId; lIter++)
{
for(int lSetIter = 0; lSetIter < FBLastEffectorSetIndex; lSetIter++)
{
FBModelMarker* lPivotEffector = dynamic_cast<FBModelMarker*>(lControlRig->GetIKEffectorModel((FBEffectorId)lIter, lSetIter));
if(lPivotEffector && lPivotEffector->IKSync)
{
FBTVector lTemp4dVector;
lPivotEffector->IKPivot.GetData(lTemp4dVector, sizeof(lTemp4dVector));
FBVector3d lIKPivot;
lIKPivot.mValue[0] = lTemp4dVector.mValue[0];
lIKPivot.mValue[1] = lTemp4dVector.mValue[1];
lIKPivot.mValue[2] = lTemp4dVector.mValue[2];
FBVector4d lGTVector;
FBRVector lGRVector;
FBVector4d lGSVector;
FBQuaternion lCurrentGRQuat;
HIKGetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, (FBEffectorId)lIter, lGTVector.mValue, lCurrentGRQuat.mValue, lGSVector.mValue );
FBQuaternionToRotation(lGRVector, lCurrentGRQuat);
if( lIKPivot.mValue[0] != 0.0 ||
lIKPivot.mValue[1] != 0.0 ||
lIKPivot.mValue[2] != 0.0)
{
FBSVector lPivotGSVector;
lPivotEffector->GetVector(lPivotGSVector, kModelScaling, true, pEvaluateInfo);
lIKPivot.mValue[0] *= lPivotGSVector.mValue[0];
lIKPivot.mValue[1] *= lPivotGSVector.mValue[1];
lIKPivot.mValue[2] *= lPivotGSVector.mValue[2];
FBMatrix lGRM;
FBRotationToMatrix(lGRM, lGRVector);
double V0, V1, V2;
V0 = lIKPivot.mValue[0];
V1 = lIKPivot.mValue[1];
V2 = lIKPivot.mValue[2];
lIKPivot.mValue[0] = lGRM(0,0) * V0 + lGRM(1,0) * V1 + lGRM(2,0) * V2;
lIKPivot.mValue[1] = lGRM(0,1) * V0 + lGRM(1,1) * V1 + lGRM(2,1) * V2;
lIKPivot.mValue[2] = lGRM(0,2) * V0 + lGRM(1,2) * V1 + lGRM(2,2) * V2;
FBTVector lTempTVector;
lTempTVector.mValue[0] = lIKPivot.mValue[0];
lTempTVector.mValue[1] = lIKPivot.mValue[1];
lTempTVector.mValue[2] = lIKPivot.mValue[2];
FBSub(lTempTVector,lGTVector,lTempTVector);
lIKPivot.mValue[0] = lTempTVector.mValue[0];
lIKPivot.mValue[1] = lTempTVector.mValue[1];
lIKPivot.mValue[2] = lTempTVector.mValue[2];
}
else
{
lIKPivot.mValue[0] = lGTVector.mValue[0];
lIKPivot.mValue[1] = lGTVector.mValue[1];
lIKPivot.mValue[2] = lGTVector.mValue[2];
}
lPivotEffector->SetVector(lIKPivot, kModelTranslation, true, false, pEvaluateInfo);
lPivotEffector->SetVector(lGRVector, kModelRotation, true, false, pEvaluateInfo);
}
}
}
}
void ORCharacterManipulatorCtrlSet::RestoreInitialState()
{
// Copy Back Initial State
HIKEffectorSetFromCharacter( mHIKInitialControlRigHostEvaluator.mHIKCharacter, mCurrentState.mCREffectorSetState,
mInitialState.mCRState, mInitialState.mCRPropertySetState );
HIKCharacterStateCopy( mCurrentState.mCRState, mInitialState.mCRState );
}
void ORCharacterManipulatorCtrlSet::TranslationSetApplyPivot(FBTVector &pVector, FBReferencialType pRefType)
{
FBEffectorId lHIKId = GetCorrespondingEffector();
if( lHIKId != kFBInvalidEffectorId )
{
FBVector3d lIKPivot;
FBModelMarker* lPivotEffector = dynamic_cast<FBModelMarker*>(GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(lHIKId, GetEffectorSet()));
FBTVector lTemp;
lPivotEffector->IKPivot.GetData(lTemp, sizeof(lTemp));
lIKPivot.mValue[0] = lTemp.mValue[0];
lIKPivot.mValue[1] = lTemp.mValue[1];
lIKPivot.mValue[2] = lTemp.mValue[2];
if( lIKPivot.mValue[0] != 0.0 || lIKPivot.mValue[1] != 0.0 || lIKPivot.mValue[2] != 0.0 )
{
FBRVector lGR;
FBMatrix lGRM;
FBMatrix lGX;
FBVector4d lGTVector;
FBQuaternion lGRQuat;
FBVector4d lTempVector;
HIKGetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lGTVector.mValue, lGRQuat.mValue, lTempVector.mValue );
FBQuaternionToRotation(lGR, lGRQuat);
//We should be only in global coordinate here
//Set the default effector position
FBSVector lGSVector;
lPivotEffector->GetVector(lGSVector, kModelScaling);
lIKPivot.mValue[0] = (lIKPivot.mValue[0] * lGSVector.mValue[0]);
lIKPivot.mValue[1] = (lIKPivot.mValue[1] * lGSVector.mValue[1]);
lIKPivot.mValue[2] = (lIKPivot.mValue[2] * lGSVector.mValue[2]);
FBTRSToMatrix(lGX, pVector, lGR, FBSVector());
pVector[0] = lGX(0,0) * lIKPivot.mValue[0] + lGX(1,0) * lIKPivot.mValue[1] + lGX(2,0) * lIKPivot.mValue[2] + lGX(3,0);
pVector[1] = lGX(0,1) * lIKPivot.mValue[0] + lGX(1,1) * lIKPivot.mValue[1] + lGX(2,1) * lIKPivot.mValue[2] + lGX(3,1);
pVector[2] = lGX(0,2) * lIKPivot.mValue[0] + lGX(1,2) * lIKPivot.mValue[1] + lGX(2,2) * lIKPivot.mValue[2] + lGX(3,2);
}
}
}
void ORCharacterManipulatorCtrlSet::RotationSetApplyPivot( FBEffectorId pEffectorIndex )
{
if ( pEffectorIndex != kFBInvalidEffectorId )
{
if(IsIKPivotUse())
{
FBModelMarker* lPivotEffector = dynamic_cast<FBModelMarker*>(GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(pEffectorIndex, GetEffectorSet()));
FBTVector lTemp;
FBVector3d lIKPivot;
lPivotEffector->IKPivot.GetData(lTemp, sizeof(lTemp));
lIKPivot.mValue[0] = lTemp.mValue[0];
lIKPivot.mValue[1] = lTemp.mValue[1];
lIKPivot.mValue[2] = lTemp.mValue[2];
if(lIKPivot.mValue[0] != 0.0 || lIKPivot.mValue[1] != 0.0 || lIKPivot.mValue[2] != 0.0)
{
int lHIKId = HIKEffectorIdFromEffectorName( FBCharacterEffectorNameFromId( pEffectorIndex ) );
if(lHIKId != -1)
{
HIKSetTranslationActive(mCurrentState.mCREffectorSetState, pEffectorIndex, 1.0);
FBMatrix lInitialGX;
FBVector4d lInitialGT;
FBQuaternion lInitialGRQuat;
FBVector4d lInitialGS;
FBRVector lInitialGR;
HIKGetEffectorStateTQSdv(mInitialState.mCREffectorSetState, lHIKId, lInitialGT.mValue, lInitialGRQuat.mValue, lInitialGS.mValue );
FBQuaternionToRotation(lInitialGR, lInitialGRQuat);
FBSVector lGSVector;
lPivotEffector->GetVector(lGSVector, kModelScaling);
lIKPivot.mValue[0] = -1.0 * lIKPivot.mValue[0] * lGSVector.mValue[0];
lIKPivot.mValue[1] = -1.0 * lIKPivot.mValue[1] * lGSVector.mValue[1];
lIKPivot.mValue[2] = -1.0 * lIKPivot.mValue[2] * lGSVector.mValue[2];
FBTRSToMatrix(lInitialGX, lInitialGT, lInitialGR, FBSVector());
FBTVector lInitialPivot;
FBTVector lFinalPivot;
lInitialPivot[0] = lInitialGX(0,0) * lIKPivot.mValue[0] + lInitialGX(1,0) * lIKPivot.mValue[1] + lInitialGX(2,0) * lIKPivot.mValue[2] + lInitialGX(3,0);
lInitialPivot[1] = lInitialGX(0,1) * lIKPivot.mValue[0] + lInitialGX(1,1) * lIKPivot.mValue[1] + lInitialGX(2,1) * lIKPivot.mValue[2] + lInitialGX(3,1);
lInitialPivot[2] = lInitialGX(0,2) * lIKPivot.mValue[0] + lInitialGX(1,2) * lIKPivot.mValue[1] + lInitialGX(2,2) * lIKPivot.mValue[2] + lInitialGX(3,2);
FBVector4d lCurrentGT;
FBQuaternion lCurrentGRQuat;
FBVector4d lCurrentGS;
FBRVector lCurrentGR;
HIKGetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lCurrentGT.mValue, lCurrentGRQuat.mValue, lCurrentGS.mValue );
FBQuaternionToRotation(lCurrentGR, lCurrentGRQuat);
FBMatrix lCurrentGRM;
FBRotationToMatrix(lCurrentGRM, lCurrentGR);
lFinalPivot[0] = lCurrentGRM(0,0) * lIKPivot[0] + lCurrentGRM(1,0) * lIKPivot[1] + lCurrentGRM(2,0) * lIKPivot[2];
lFinalPivot[1] = lCurrentGRM(0,1) * lIKPivot[0] + lCurrentGRM(1,1) * lIKPivot[1] + lCurrentGRM(2,1) * lIKPivot[2];
lFinalPivot[2] = lCurrentGRM(0,2) * lIKPivot[0] + lCurrentGRM(1,2) * lIKPivot[1] + lCurrentGRM(2,2) * lIKPivot[2];
FBSub(lFinalPivot,lInitialPivot,lFinalPivot);
HIKSetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lFinalPivot.mValue, lCurrentGRQuat.mValue, lCurrentGS.mValue );
}
}
}
}
}
void ORCharacterManipulatorCtrlSet::ManipulationStart()
{
FBEvaluateInfo *lEvalInfo = FBGetDisplayInfo();
// Sync Rig on Skeleton First
mHIKInitialControlRigHostEvaluator.ReadSkeletonSolveOnRig( lEvalInfo, &mInitialState);
mHIKCurrentControlRigHostEvaluator.ReadSkeletonSolveOnRig( lEvalInfo, &mCurrentState);
// Add Transforms to Undos before doing any change
UndoSetup(GetCharacter());
// Write the Sync Rig
mHIKCurrentControlRigHostEvaluator.WriteRigCandidate(lEvalInfo, &mCurrentState);
// Calculate the numbers of object manipulated
UpdateManipulatedObjectCount();
// this will set the manipulation Parameters
PrepareSetForManip();
}
void ORCharacterManipulatorCtrlSet::UpdateManipulatedObjectCount()
{
FBCharacter *lCharacter = GetCharacter();
// Calculate the numbers of object manipulated
mManipulatedObjectCount = 0;
{
for(int j = FBEffectorSetDefault; j < FBLastEffectorSetIndex; j++) // Need to validate for Pivot and Auxiliary selected at the same time that an effector
{
FBModel *lModel = lCharacter->GetEffectorModel((FBEffectorId)i, (FBEffectorSetID)j);
if(lModel && lModel->Selected)
{
mManipulatedObjectCount++;
}
}
}
for (int lBodyNodeIndex = kFBHipsNodeId; lBodyNodeIndex < kFBLastNodeId_Old; lBodyNodeIndex++)
{
FBModel *lModel = lCharacter->GetCtrlRigModel( (FBBodyNodeId)lBodyNodeIndex );
if ( lModel && lModel->Selected )
{
mManipulatedObjectCount++;
}
}
FBCharacterSolver * lSolver = lCharacter->GetExternalSolver();
if (lSolver)
{
int lExtraFKIter, lExtraFKCount = lSolver->GetExtraFKCount();
for ( lExtraFKIter = 0; lExtraFKIter < lExtraFKCount; lExtraFKIter++ )
{
FBModel *lModel = lSolver->GetExtraFKModelAt(lExtraFKIter);
if ( lModel && lModel->Selected )
{
mManipulatedObjectCount++;
}
}
}
}
void ORCharacterManipulatorCtrlSet::PrepareSetForManip()
{
FBCharacter *lCharacter = GetCharacter();
FBBodyPartId lManipulatedBodyPart = FBGetEffectorBodyPart( GetCorrespondingEffector() );
bool lActiveBodyPart[kFBLastCtrlSetPartIndex];
lCharacter->GetActiveBodyPart(lActiveBodyPart);
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 = HIKEffectorIdFromEffectorName( FBCharacterEffectorNameFromId( (FBEffectorId)lIter ) );
if (lHIKId != -1)
{
lBodyPart = FBGetEffectorBodyPart( (FBEffectorId) lIter );
lReachR = mHIKCurrentControlRigHostEvaluator.mHIKControlRigHost->GetEffector(lIter, 0).ReadReachR( FBGetDisplayInfo() );
lReachT = mHIKCurrentControlRigHostEvaluator.mHIKControlRigHost->GetEffector(lIter, 0).ReadReachT( FBGetDisplayInfo() );
if ( IsFullBody() )
{
if ( IsReachOverride() )
{
lReachR = 0;
lReachT = 0;
}
if ( IsFullBody() || lManipulatedBodyPart==lBodyPart )
{
if ( IsReachOverride() && !IsReleasePinPressed() )
{
lReachR = (lCharacter->IsRotationPin((FBEffectorId)lIter)) ? 1.0 : lReachR;
lReachT = (lCharacter->IsTranslationPin((FBEffectorId)lIter)) ? 1.0 : lReachT;
}
}
}
else
{
//body part
if(IsReachOverride() || IsReleasePinPressed())
{
if ( lActiveBodyPart[lBodyPart] )
{
if ( IsReleasePinPressed() )
{
lReachT = 0.0;
lReachR = 0.0;
}
else
{
lReachT = lCharacter->IsTranslationPin((FBEffectorId)lIter) ? 1 : 0;
lReachR = lCharacter->IsRotationPin((FBEffectorId)lIter) ? 1 : 0;
}
}
}
}
HIKSetRotationActive(mCurrentState.mCREffectorSetState, lHIKId, lReachR);
HIKSetTranslationActive(mCurrentState.mCREffectorSetState, lHIKId, lReachT);
}
}
// Adjust Resist
if( IsStiffnessOverride() )
{
SetResist(0.0);
HIKSetResist(mCurrentState.mCREffectorSetState, HipsEffectorId, HIKGetRotationActive(mCurrentState.mCREffectorSetState, HipsEffectorId) );
HIKSetResist(mCurrentState.mCREffectorSetState, ChestEndEffectorId, HIKGetRotationActive(mCurrentState.mCREffectorSetState, ChestEndEffectorId) );
if ( IsFullBody() )
{
if( HIKGetPropertyValue(mCurrentState.mCRPropertySetState, HIKPostureId) == 0 ) // biped
{
HIKSetResist(mCurrentState.mCREffectorSetState, LeftShoulderEffectorId, 0.35f);
HIKSetResist(mCurrentState.mCREffectorSetState, RightShoulderEffectorId, 0.35f);
}
HIKSetResist(mCurrentState.mCREffectorSetState, HeadEffectorId, 1.0); // This needs to be here; it was here but removed at some point. MOBU-7102 needs this.
}
}
if( IsReachOverride() )
{
if ( !IsReleasePinPressed() )
{
float lValue;
for(int lIter = HipsEffectorId; lIter < LastEffectorId; lIter++)
{
lBodyPart = FBGetEffectorBodyPart( (FBEffectorId) lIter );
if(lIter <= RightHandEffectorId && ( IsFullBody() || lActiveBodyPart[lBodyPart]) )
{
// In BodyPart, we respect the pinning only on the active Part
lValue = (lCharacter->IsTranslationPin((FBEffectorId)lIter)) ? 1 : 0;
HIKSetPull( mCurrentState.mCREffectorSetState, lIter, lValue );
}
else
{
HIKSetPull( mCurrentState.mCREffectorSetState, lIter, 0.0 );
}
}
}
if ( IsFullBody() ) // Full Body
{
// Left/Right
HIKSetPull( mCurrentState.mCREffectorSetState, RightWristEffectorId, 1.0 );
HIKSetPull( mCurrentState.mCREffectorSetState, LeftWristEffectorId, 1.0 );
}
}
// Some small adjustment to have the same behavior as Motionbuilder.
FBEffectorId lEffectorId = GetCorrespondingEffector();
if ( lEffectorId == kFBHipsEffectorId && !lCharacter->IsRotationPin(kFBHipsEffectorId) )
{
HIKSetResist( mCurrentState.mCREffectorSetState, HipsEffectorId, 1.0 ); // Smoother Rotation on Hips
}
else if ( lEffectorId == kFBLeftShoulderEffectorId ) // Keep Arm Rotations
{
HIKSetRotationActive( mCurrentState.mCREffectorSetState, LeftShoulderEffectorId, 1.0);
}
else if ( lEffectorId == kFBRightShoulderEffectorId ) // Keep Arm Rotations
{
HIKSetRotationActive( mCurrentState.mCREffectorSetState, RightShoulderEffectorId, 1.0);
}
// better feed back
HIKSetPropertyValue(mCurrentState.mCRPropertySetState, HIKPullIterationCount, 30);
}
#define K_180_PI 57.295779513082320876798154814105170332405472466565 // 180/pi
bool ORCharacterManipulatorCtrlSet::NeedRotationSync(FBEffectorId pEffectorIndex,double pAngleTol)
{
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, lCurrentGlobalRot, lInvCurrentGlobalRot, lInitialGlobalRotM, lDeltaRotM;
FBQuaternion lDeltaRotQ;
FBRVector lRotV;
int lHIKId = HIKEffectorIdFromEffectorName( FBCharacterEffectorNameFromId( pEffectorIndex ) );
if (lHIKId != -1)
{
FBModel *lModel = lCharacter->GetEffectorModel( pEffectorIndex );
lModel->GetMatrix(lCurrentGlobalRot, kModelRotation, true, FBGetDisplayInfo() );
FBMatrixInverse( lInvCurrentGlobalRot, lCurrentGlobalRot);
HIKGetEffectorStatedv( mInitialState.mCREffectorSetState, lHIKId, (double*)lInitialGlobalTransformation);
FBMatrixToRotation( lRotV, lInitialGlobalTransformation);
FBRotationToMatrix ( lInitialGlobalRotM, lRotV);
FBMatrixMult(lDeltaRotM, lInitialGlobalRotM, lInvCurrentGlobalRot);
FBMatrixToRotation ( lRotV, lDeltaRotM);
FBRotationToQuaternion( lDeltaRotQ, lRotV );
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
};
int lNodeCounter ;
for(lNodeCounter = 0 ; lNodeCounter < LastNodeId ; lNodeCounter++)
{
if( HIKGetNodeUse(mHIKCharacter, lNodeCounter) )
{
lDescription.mHIKNodeId[lNodeCounter] = lNodeCounter;
}
else
{
lDescription.mHIKNodeId[lNodeCounter] = HIKNotUsed;
}
}
lDescription.mHIKNodeId[LastNodeId] = HIKLastNode;
HIKGetNormalizedCharacterStateTransformTQS( mHIKCurrentControlRigHostEvaluator.mHIKCharacter, mCurrentState.mCRState, &lDescription, pDataSet);
double lTBuffer[4];
double lQBuffer[4];
double lSBuffer[4];
for(lNodeCounter = 0 ; lNodeCounter < LastNodeId ; lNodeCounter++)
{
if( HIKGetNodeUse(mHIKCharacter, lNodeCounter) )
{
lTBuffer[0] = pDataSet[lNodeCounter].mTfv[0];
lTBuffer[1] = pDataSet[lNodeCounter].mTfv[1];
lTBuffer[2] = pDataSet[lNodeCounter].mTfv[2];
lTBuffer[3] = pDataSet[lNodeCounter].mTfv[3];
pDataSet[lNodeCounter].mTfv[0] = lTBuffer[0];
pDataSet[lNodeCounter].mTfv[1] = lTBuffer[1];
pDataSet[lNodeCounter].mTfv[2] = lTBuffer[2];
pDataSet[lNodeCounter].mTfv[3] = lTBuffer[3];
lQBuffer[0] = pDataSet[lNodeCounter].mQfv[0];
lQBuffer[1] = pDataSet[lNodeCounter].mQfv[1];
lQBuffer[2] = pDataSet[lNodeCounter].mQfv[2];
lQBuffer[3] = pDataSet[lNodeCounter].mQfv[3];
pDataSet[lNodeCounter].mQfv[0] = lQBuffer[0];
pDataSet[lNodeCounter].mQfv[1] = lQBuffer[1];
pDataSet[lNodeCounter].mQfv[2] = lQBuffer[2];
pDataSet[lNodeCounter].mQfv[3] = lQBuffer[3];
lSBuffer[0] = pDataSet[lNodeCounter].mSfv[0];
lSBuffer[1] = pDataSet[lNodeCounter].mSfv[1];
lSBuffer[2] = pDataSet[lNodeCounter].mSfv[2];
lSBuffer[3] = pDataSet[lNodeCounter].mSfv[3];
pDataSet[lNodeCounter].mSfv[0] = lSBuffer[0];
pDataSet[lNodeCounter].mSfv[1] = lSBuffer[1];
pDataSet[lNodeCounter].mSfv[2] = lSBuffer[2];
pDataSet[lNodeCounter].mSfv[3] = lSBuffer[3];
}
}
}
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
};
int lNodeCounter ;
float lTBuffer[4];
float lQBuffer[4];
float lSBuffer[4];
for(lNodeCounter = 0 ; lNodeCounter < LastNodeId ; lNodeCounter++)
{
if( HIKGetNodeUse(mHIKCharacter, lNodeCounter) )
{
lDescription.mHIKNodeId[lNodeCounter] = lNodeCounter;
lTBuffer[0] = pDataSet[lNodeCounter].mTfv[0];
lTBuffer[1] = pDataSet[lNodeCounter].mTfv[1];
lTBuffer[2] = pDataSet[lNodeCounter].mTfv[2];
lTBuffer[3] = pDataSet[lNodeCounter].mTfv[3];
pDataSet[lNodeCounter].mTfv[0] = lTBuffer[0];
pDataSet[lNodeCounter].mTfv[1] = lTBuffer[1];
pDataSet[lNodeCounter].mTfv[2] = lTBuffer[2];
pDataSet[lNodeCounter].mTfv[3] = lTBuffer[3];
lQBuffer[0] = pDataSet[lNodeCounter].mQfv[0];
lQBuffer[1] = pDataSet[lNodeCounter].mQfv[1];
lQBuffer[2] = pDataSet[lNodeCounter].mQfv[2];
lQBuffer[3] = pDataSet[lNodeCounter].mQfv[3];
pDataSet[lNodeCounter].mQfv[0] = lQBuffer[0];
pDataSet[lNodeCounter].mQfv[1] = lQBuffer[1];
pDataSet[lNodeCounter].mQfv[2] = lQBuffer[2];
pDataSet[lNodeCounter].mQfv[3] = lQBuffer[3];
lSBuffer[0] = pDataSet[lNodeCounter].mSfv[0];
lSBuffer[1] = pDataSet[lNodeCounter].mSfv[1];
lSBuffer[2] = pDataSet[lNodeCounter].mSfv[2];
lSBuffer[3] = pDataSet[lNodeCounter].mSfv[3];
pDataSet[lNodeCounter].mSfv[0] = lSBuffer[0];
pDataSet[lNodeCounter].mSfv[1] = lSBuffer[1];
pDataSet[lNodeCounter].mSfv[2] = lSBuffer[2];
pDataSet[lNodeCounter].mSfv[3] = lSBuffer[3];
}
else
{
lDescription.mHIKNodeId[lNodeCounter] = HIKNotUsed;
}
}
lDescription.mHIKNodeId[LastNodeId] = HIKLastNode;
HIKSetNormalizedCharacterStateTransformTQS(mHIKCharacter, mCurrentState.mCRState, &lDescription, pDataSet);
}
int ORCharacterManipulatorCtrlSet::GetManipulatedNodeExtraFKIndex()
{
int lReturnedIndex = -1;
FBModel *lModel = GetManipulatedNode();
FBCharacter *lCharacter = GetCharacter();
FBCharacterSolver * lSolver = lCharacter->GetExternalSolver();
if (lSolver)
{
int lIter, lCount = lSolver->GetExtraFKCount();
for ( lIter = 0; lIter < lCount && lReturnedIndex==-1; lIter++ )
{
if ( lModel == lSolver->GetExtraFKModelAt(lIter) )
lReturnedIndex = lIter;
}
}
return lReturnedIndex;
}
int ORCharacterManipulatorCtrlSet::GetManipulatedNodeExtraBoneIndex()
{
int lReturnedIndex = -1;
FBModel *lModel = GetManipulatedNode();
FBCharacter *lCharacter = GetCharacter();
FBCharacterSolver * lSolver = lCharacter->GetExternalSolver();
if (lSolver)
{
int lIter, lCount = lSolver->GetExtraBoneCount();
for ( lIter = 0; lIter < lCount && lReturnedIndex==-1; lIter++ )
{
if ( lModel == lSolver->GetExtraBoneModelAt(lIter) )
lReturnedIndex = lIter;
}
}
return lReturnedIndex;
}
FBEffectorId ORCharacterManipulatorCtrlSet::GetCorrespondingEffector()
{
FBEffectorId lEffectorId = GetManipulatedEffectorId();
if ( lEffectorId == kFBInvalidEffectorId )
{
if ( GetManipulatorMode() == FBManip_EditorTranslation )
{
int lFKIndex = GetManipulatedNodeExtraFKIndex();
if ( lFKIndex != -1 )
{
if ( lFKIndex == LEFT_EXTRA_COLLAR )
else if ( lFKIndex == RIGHT_EXTRA_COLLAR )
}
}
}
return lEffectorId;
}