#include "orcharactersolver_hik.h"
#include "filterset2fbcharacter.h"
#include <math.h>
#define ORCHARACTERMANIPULATORCTRLSET__CLASS ORCHARACTERMANIPULATORCTRLSET__CLASSNAME
#ifndef _WIN32
#define _aligned_malloc(size, alignment) memalign(alignment, size)
#define _aligned_free(p) free(p)
#endif
bool ORCharacterManipulatorCtrlSet::FBCreate()
{
mManipulatedObjectCount = 0;
mCurrentObjectManipulatedCount = 0;
mDataSet = (KHIKNodeState*)_aligned_malloc( sizeof(KHIKNodeState) * LastNodeId, 16);
return true;
}
void ORCharacterManipulatorCtrlSet::FBDestroy()
{
DeallocateState();
_aligned_free(mDataSet);
FBCharacterManipulatorCtrlSet::FBDestroy();
}
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
pEditable = true;
}
return lRet;
}
{
FBCharacter *lCharacter = GetCharacter();
if ( lCharacter->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
{
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 );
}
}
{
HIKGetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lGTVector.mValue, lInitialGR.mValue, lGSVector.mValue );
TranslationSetApplyPivot(lTBackup, pRefType);
HIKSetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lTBackup.mValue, lInitialGR.mValue, lGSVector.mValue );
mCurrentObjectManipulatedCount++;
if(mManipulatedObjectCount == mCurrentObjectManipulatedCount)
{
mCurrentObjectManipulatedCount = 0;
RestoreInitialState();
}
}
{
FBModel *lModel = lCharacter->GetEffectorModel(lEffectorIndex, lEffectorSet);
FBTVector lLocalTranslation(l3DLocalTrans[0], l3DLocalTrans[1], l3DLocalTrans[2], 0);
FBTVector lGlobalTranslation(l3dGlobalTrans[0], l3dGlobalTrans[1], l3dGlobalTrans[2], 0);
FBSub(lDeltaTranslation, lLocalTranslation, pT);
FBAdd(lGlobalTranslation, lLocalTranslation, lDeltaTranslation);
}
else
{
}
}
}
}
}
return lResult;
}
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
int lExtraFKIndex = -1;
lExtraFKIndex = GetManipulatedNodeExtraFKIndex();
{
{
{
GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(lEffectorIndex, GetEffectorSet())->GetVector(lVector,
kModelTranslation,
true, lEvaluateInfo);
pT[0] = lVector[0];
pT[1] = lVector[1];
pT[2] = lVector[2];
}
{
GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(lEffectorIndex, GetEffectorSet())->GetVector(lVector,
kModelTranslation,
false, lEvaluateInfo);
pT[0] = lVector[0];
pT[1] = lVector[1];
pT[2] = lVector[2];
}
}
}
}
}
return lResult;
}
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
ManipulationStart();
}
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::TranslationStopManipulation ()
{
}
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
pEditable = true;
}
return lRet;
}
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
{
{
lResult = RotationSetEffectorGlobal(pRM);
}
{
lResult = RotationSetEffectorLocal(pRM);
}
RotationSetApplyPivot(lEffectorIndex );
}
else
{
{
}
else
{
int lFKIndex = -1;
lFKIndex = GetManipulatedNodeExtraFKIndex();
{
lModel = GetCharacter()->GetCtrlRigModel(lBodyNodeIndex);
else
{
FBCharacterSolver* lExtSolver = GetCharacter()->GetExternalSolver();
if (lExtSolver)
lModel = lExtSolver->GetExtraFKModelAt(lFKIndex);
}
{
lResult = RotationSetFKGlobal(lModel, pRM);
}
{
RotationSetFKLocal(lModel, pRM);
}
}
}
}
mCurrentObjectManipulatedCount++;
if(mManipulatedObjectCount == mCurrentObjectManipulatedCount)
{
mCurrentObjectManipulatedCount = 0;
RestoreInitialState();
}
}
}
return lResult;
}
{
FBCharacter* lCharacter = GetCharacter();
if( GetCharacter()->IsCtrlSetReady() )
{
if( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
lEffectorIndex = GetCorrespondingEffector();
{
FBModel *lModel = lCharacter->GetEffectorModel(lEffectorIndex, GetEffectorSet());
if ( lModel )
{
{
AddPostRotation(lModel, pRM);
}
{
}
{
}
else
{
}
}
}
}
}
return lRet;
}
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
ManipulationStart();
}
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::RotationStopManipulation ()
{
}
{
int lHIKId = -1;
if(lHIKId != -1)
{
FBModel *lModel = GetCharacter()->GetEffectorModel(lEffectorIndex, lEffectorSet);
lModel = GetCharacter()->GetCtrlRigModel(lBodyNode);
if( NeedRotationSync(lEffectorIndex,120.0) )
{
ManipulationStart();
}
HIKSetRotationActive(mCurrentState.mCREffectorSetState, lHIKId, 1.0);
FBMatrix lResultRotation(pRM);
HIKGetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lGTVector.mValue, lGRQuat.mValue, lGSVector.mValue );
RemovePostRotation(lModel, lResultRotation);
HIKSetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lGTVector.mValue, lGRQuat.mValue, lGSVector.mValue );
}
}
{
int lHIKId = -1;
if(lHIKId != -1)
{
int lFKIndex = -1;
lFKIndex = GetManipulatedNodeExtraFKIndex();
{
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);
}
else
{
AddPostRotation(lModel, lGlobalRotation);
}
lResult = RotationSetEffectorGlobal(lGlobalRotation);
}
}
return lResult;
}
{
FBMatrix lRM(pRM);
HIKNodeId lNodeId = LastNodeId;
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 );
}
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);
}
{
FBMatrix lGlobalRotation;
if ( pModel->Parent )
{
FBMatrix lParentGlobalRotation;
pModel->Parent->GetMatrix( lParentGlobalRotation,
kModelRotation,
true );
AddPostRotation(pModel, lParentGlobalRotation);
}
else
{
AddPostRotation(pModel, lGlobalRotation);
}
RotationSetFKGlobal(pModel, lGlobalRotation);
}
{
}
{
}
{
}
{
}
void ORCharacterManipulatorCtrlSet::ScalingStopManipulation ()
{}
void ORCharacterManipulatorCtrlSet::AllocateState(FBCharacter* pCharacter)
{
HIKHostPropertiesInit(mHIKCharacterHost);
HIKCharacterHostFromMBCharacterHierarchy(mHIKCharacterHost,pCharacter,
NULL);
FBCharacterSolver* lExtSolver = pCharacter->GetExternalSolver();
ORCharacterSolver_HIK* lConstraint =
NULL;
if(lExtSolver)
{
lConstraint = dynamic_cast<ORCharacterSolver_HIK*>(lExtSolver);
if(lConstraint)
{
lLeft = lConstraint->GetExtraBoneModelAt(LEFT_EXTRA_COLLAR);
lRight = lConstraint->GetExtraBoneModelAt(RIGHT_EXTRA_COLLAR);
HIKCharacterHostFromHIKHostMB(mHIKCharacterHost, lLeft, lRight,
NULL);
}
}
if (!mHIKCharacter)
mHIKCharacter = mHIKCharacterHost.CharacterCreate(&malloc);
HIKCharacterHostFromMBCharacterGeometry(mHIKCharacterHost,mHIKCharacter,pCharacter);
if(lConstraint && (lLeft || lRight) )
{
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);
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();
{
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);
}
mHIKCurrentControlRigHostEvaluator.Clear(&free);
mHIKInitialControlRigHostEvaluator.Clear(&free);
mCurrentState.Clear(&free);
mInitialState.Clear(&free);
}
bool ORCharacterManipulatorCtrlSet::IsDescendantPulling(int pEffectorId)
{
bool lRet = HIKGetPull(mCurrentState.mCREffectorSetState, lHIKId) >= 0.5;
int lCount = GetDescendantEffectorCount((
FBEffectorId)pEffectorId);
for(int lIter = 0; !lRet && lIter < lCount; 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)
{
}
void ORCharacterManipulatorCtrlSet::ApplySync(FBEvaluateInfo *pEvalInfo, bool pSolveBackOnRig , bool pWriteRig )
{
FBCharacter *lCharacter = GetCharacter();
int lSolvingStep = HIKSolvingStepAll;
if( IsBodyPart()|| IsFullBodyNoPull() )
{
bool lActiveBodyPart[12];
lCharacter->GetActiveBodyPart(lActiveBodyPart);
{
{
lSolvingStep = lSolvingStep | GetSolvingStepBasedOnActivePart(i);
}
}
lSolvingStep &= ~HIKSolvingStepBodyPull;
}
{
lSolvingStep &= ~HIKSolvingStepContact;
}
mHIKCurrentControlRigHostEvaluator.mSolvingStep = lSolvingStep;
FBCharacterSolver* lExtSolver = lCharacter->GetExternalSolver();
if(lExtSolver)
{
ORCharacterSolver_HIK* lConstraint = dynamic_cast<ORCharacterSolver_HIK*>(lExtSolver);
if(lConstraint)
{
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)
{
mHIKCurrentControlRigHostEvaluator.EffectorStateFromControlSet(&mCurrentState);
}
if (pWriteRig)
{
mHIKCurrentControlRigHostEvaluator.WriteRigCandidate(pEvalInfo,&mCurrentState);
}
SyncEffectorPivot(pEvalInfo);
}
void ORCharacterManipulatorCtrlSet::SyncEffectorPivot(FBEvaluateInfo* pEvaluateInfo)
{
FBControlSet* lControlRig = GetCharacter()->GetCurrentControlSet();
{
{
FBModelMarker* lPivotEffector =
dynamic_cast<FBModelMarker*
>(lControlRig->GetIKEffectorModel((
FBEffectorId)lIter, lSetIter));
if(lPivotEffector && lPivotEffector->IKSync)
{
lPivotEffector->IKPivot.GetData(lTemp4dVector, sizeof(lTemp4dVector));
lIKPivot.
mValue[0] = lTemp4dVector.mValue[0];
lIKPivot.mValue[1] = lTemp4dVector.mValue[1];
lIKPivot.mValue[2] = lTemp4dVector.mValue[2];
HIKGetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, (
FBEffectorId)lIter, lGTVector.mValue, lCurrentGRQuat.mValue, lGSVector.mValue );
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;
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;
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(lGRVector,
kModelRotation,
true,
false, pEvaluateInfo);
}
}
}
}
void ORCharacterManipulatorCtrlSet::RestoreInitialState()
{
HIKEffectorSetFromCharacter( mHIKInitialControlRigHostEvaluator.mHIKCharacter, mCurrentState.mCREffectorSetState,
mInitialState.mCRState, mInitialState.mCRPropertySetState );
HIKCharacterStateCopy( mCurrentState.mCRState, mInitialState.mCRState );
}
{
{
FBModelMarker* lPivotEffector = dynamic_cast<FBModelMarker*>(GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(lHIKId, GetEffectorSet()));
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 )
{
FBMatrix lGRM;
FBMatrix lGX;
HIKGetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lGTVector.mValue, lGRQuat.mValue, lTempVector.mValue );
FBSVector lGSVector;
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]);
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(IsIKPivotUse())
{
FBModelMarker* lPivotEffector = dynamic_cast<FBModelMarker*>(GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(pEffectorIndex, GetEffectorSet()));
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)
{
if(lHIKId != -1)
{
HIKSetTranslationActive(mCurrentState.mCREffectorSetState, pEffectorIndex, 1.0);
FBMatrix lInitialGX;
HIKGetEffectorStateTQSdv(mInitialState.mCREffectorSetState, lHIKId, lInitialGT.mValue, lInitialGRQuat.mValue, lInitialGS.mValue );
FBSVector lGSVector;
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];
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);
HIKGetEffectorStateTQSdv(mCurrentState.mCREffectorSetState, lHIKId, lCurrentGT.mValue, lCurrentGRQuat.mValue, lCurrentGS.mValue );
FBMatrix lCurrentGRM;
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()
{
mHIKInitialControlRigHostEvaluator.ReadSkeletonSolveOnRig( lEvalInfo, &mInitialState);
mHIKCurrentControlRigHostEvaluator.ReadSkeletonSolveOnRig( lEvalInfo, &mCurrentState);
UndoSetup(GetCharacter());
mHIKCurrentControlRigHostEvaluator.WriteRigCandidate(lEvalInfo, &mCurrentState);
UpdateManipulatedObjectCount();
PrepareSetForManip();
}
void ORCharacterManipulatorCtrlSet::UpdateManipulatedObjectCount()
{
FBCharacter *lCharacter = GetCharacter();
mManipulatedObjectCount = 0;
{
{
if(lModel && lModel->Selected)
{
mManipulatedObjectCount++;
}
}
}
{
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();
lCharacter->GetActiveBodyPart(lActiveBodyPart);
float lReachR, lReachT;
{
if (lHIKId != -1)
{
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
{
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);
}
}
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 )
{
HIKSetResist(mCurrentState.mCREffectorSetState, LeftShoulderEffectorId, 0.35f);
HIKSetResist(mCurrentState.mCREffectorSetState, RightShoulderEffectorId, 0.35f);
}
HIKSetResist(mCurrentState.mCREffectorSetState, HeadEffectorId, 1.0);
}
}
if( IsReachOverride() )
{
if ( !IsReleasePinPressed() )
{
float lValue;
for(int lIter = HipsEffectorId; lIter < LastEffectorId; lIter++)
{
if(lIter <= RightHandEffectorId && ( IsFullBody() || lActiveBodyPart[lBodyPart]) )
{
lValue = (lCharacter->IsTranslationPin((
FBEffectorId)lIter)) ? 1 : 0;
HIKSetPull( mCurrentState.mCREffectorSetState, lIter, lValue );
}
else
{
HIKSetPull( mCurrentState.mCREffectorSetState, lIter, 0.0 );
}
}
}
if ( IsFullBody() )
{
HIKSetPull( mCurrentState.mCREffectorSetState, RightWristEffectorId, 1.0 );
HIKSetPull( mCurrentState.mCREffectorSetState, LeftWristEffectorId, 1.0 );
}
}
{
HIKSetResist( mCurrentState.mCREffectorSetState, HipsEffectorId, 1.0 );
}
{
HIKSetRotationActive( mCurrentState.mCREffectorSetState, LeftShoulderEffectorId, 1.0);
}
{
HIKSetRotationActive( mCurrentState.mCREffectorSetState, RightShoulderEffectorId, 1.0);
}
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();
{
FBControlSet *lControlSet = lCharacter->GetCurrentControlSet();
{
{
FBMatrix lInitialGlobalTransformation, lCurrentGlobalRot, lInvCurrentGlobalRot, lInitialGlobalRotM, lDeltaRotM;
if (lHIKId != -1)
{
FBModel *lModel = lCharacter->GetEffectorModel( pEffectorIndex );
HIKGetEffectorStatedv( mInitialState.mCREffectorSetState, lHIKId, (double*)lInitialGlobalTransformation);
FBMatrixMult(lDeltaRotM, lInitialGlobalRotM, lInvCurrentGlobalRot);
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;
}
}
void ORCharacterManipulatorCtrlSet::RemovePostRotation(FBModel *pModel, FBMatrix &pMatrix)
{
if( pModel && pModel->RotationActive && !pModel->RotationSpaceForLimitOnly )
{
FBRVector lPostRotationV( pModel->PostRotation );
FBMatrix lPostMultInv;
}
}
void ORCharacterManipulatorCtrlSet::GetNormalizedLocalState( KHIKNodeState pDataSet[LastNodeId] ) const
{
int lNodeIdDesc[LastNodeId + 1];
HIKDataDescription lDescription = {
HIKDataDescription::HIKLocalSpace,
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,
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()
{
{
{
int lFKIndex = GetManipulatedNodeExtraFKIndex();
if ( lFKIndex != -1 )
{
if ( lFKIndex == LEFT_EXTRA_COLLAR )
else if ( lFKIndex == RIGHT_EXTRA_COLLAR )
}
}
}
return lEffectorId;
}