#include "orcharactersolver_hik.h"
#include "filterset2fbcharacter.h"
#include <math.h>
#define ORCHARACTERMANIPULATORCTRLSET__CLASS ORCHARACTERMANIPULATORCTRLSET__CLASSNAME
bool ORCharacterManipulatorCtrlSet::FBCreate()
{
CleanForManipulation();
HIKHostPropertiesInit(mHIKCharacterHost);
return true;
}
void ORCharacterManipulatorCtrlSet::FBDestroy()
{
DeallocateState();
FBCharacterManipulatorCtrlSet::FBDestroy();
}
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
pEditable = true;
}
else
{
}
}
{
FBCharacter *lCharacter = GetCharacter();
if ( lCharacter->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
int lHIKId = FBEffectorIdToHIKId(lEffectorIndex);
if(lHIKId != -1)
{
HIKSetTranslationActive(mCurrentState.mEffectorSetState, lHIKId, 1.0);
if(IsReachOverride())
{
HIKSetPull( mCurrentState.mEffectorSetState, lHIKId, 0.0 );
if(!IsDescendantPulling( lEffectorIndex ))
{
HIKSetPull( mCurrentState.mEffectorSetState, lHIKId, 1.0 );
}
}
{
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();
}
{
FBModel *lModel = lCharacter->GetEffectorModel(lEffectorIndex, GetEffectorSet());
Translation_LocalToGlobal(lGlobalTranslation, lModel, pT);
}
else
{
}
}
}
}
return lResult;
}
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
if( IsManipulated_FK() )
{
{
{
GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(lEffectorIndex, GetEffectorSet())->GetVector(*(
FBVector3d*)&pT,
kModelTranslation,
true, lEvaluateInfo);
}
{
GetCharacter()->GetCurrentControlSet()->GetIKEffectorModel(lEffectorIndex, GetEffectorSet())->GetVector(*(
FBVector3d*)&pT,
kModelTranslation,
false, lEvaluateInfo);
}
}
}
}
}
return lResult;
}
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
}
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::TranslationStopManipulation ()
{
CleanForManipulation();
}
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
pEditable = true;
}
else
{
}
}
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
{
{
lResult = RotationSetEffectorGlobal(lEffectorIndex, pRM);
}
{
lResult = RotationSetEffectorLocal(lEffectorIndex, pRM);
}
else
{
}
}
else
{
{
}
else
{
OR_HIK_ASSERT(IsManipulated_FK());
FBModel* lModel = GetManipulatedNode();
{
lResult = RotationSetFKGlobal(lModel, pRM);
}
{
lResult = RotationSetFKLocal(lModel, pRM);
}
else
{
}
}
}
AskSync();
}
}
return lResult;
}
{
FBCharacter* lCharacter = GetCharacter();
if( lCharacter->IsCtrlSetReady() )
{
if( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
{
lEffectorIndex = GetCorrespondingEffector();
}
else
{
}
{
FBModel *lModel = lCharacter->GetEffectorModel(lEffectorIndex, GetEffectorSet());
if ( lModel )
{
{
AddPostRotation(lModel, pRM);
}
{
}
{
}
else
{
}
}
}
}
}
return lRet;
}
{
if ( GetCharacter()->IsCtrlSetReady() )
{
if ( IsFullBody() || IsFullBodyNoPull() || IsBodyPart() )
{
}
}
return lRet;
}
void ORCharacterManipulatorCtrlSet::RotationStopManipulation ()
{
CleanForManipulation();
}
{
int lHIKId = FBEffectorIdToHIKId( pEffectorIndex );
if(lHIKId != -1)
{
if( NeedRotationSync(pEffectorIndex,120.0) )
{
}
HIKSetRotationActive(mCurrentState.mEffectorSetState, lHIKId, 1.0);
FBMatrix lResultRotation(pRM);
HIKGetEffectorStateTQSdv(mCurrentState.mEffectorSetState, lHIKId, lGT.mValue, lGQ.mValue, lGS.mValue );
FBModel* lModel = GetManipulatedNode();
RemovePostRotation(lModel, lResultRotation);
if(IsManipulated_FK())
{
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);
}
Rotation_ApplyPivot(pEffectorIndex, lHIKId, lGT, lGQ, lGS);
HIKSetEffectorStateTQSdv(mCurrentState.mEffectorSetState, lHIKId, lGT.mValue, lGQ.mValue, lGS.mValue );
}
}
{
FBMatrix lGlobalRotation;
FBModel* lModel = GetManipulatedNode();
if(IsManipulated_FK())
{
HIKNodeId lNodeId = GetManipulatedHIKNodeId();
OR_HIK_ASSERT(lNodeId != LastNodeId);
FBMatrix lPGRM;
int const lParentNodeId = HIKGetParentNodeId(mHIKCharacter, lNodeId);
if(lParentNodeId != -1)
{
HIKGetNodeNormalizedStateTQSdv( mHIKCharacter, mInitialState.mState, lParentNodeId, lPGT.mValue, lPGQ.mValue, lPGS.mValue );
}
AddPostRotation(lModel, lPGRM);
}
else
{
if ( lModel->Parent )
{
}
AddPostRotation(lModel, lGlobalRotation);
}
RotationSetEffectorGlobal(pEffectorIndex, lGlobalRotation);
}
{
HIKNodeId lNodeId = GetManipulatedHIKNodeId();
OR_HIK_ASSERT(lNodeId != LastNodeId);
FBMatrix lRM(pRM);
RemovePostRotation(pModel, lRM);
#if 0
int const lParentNodeId = HIKGetParentNodeId(mHIKCharacter, lNodeId);
if(lParentNodeId != -1)
{
FBMatrix lMatrix;
HIKGetNodeNormalizedStateTQSdv( mHIKCharacter, mCurrentState.mState, lParentNodeId, lGTVector.mValue, lGQ.mValue, lGSVector.mValue );
}
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
}
{
HIKNodeId lNodeId = GetManipulatedHIKNodeId();
OR_HIK_ASSERT(lNodeId != LastNodeId);
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
}
{
OR_HIK_ASSERT(pModel);
FBMatrix lParentGlobal;
if ( pModel->Parent )
{
pModel->Parent->GetMatrix( lParentGlobal, pTransformType, true );
}
}
{
}
{
}
{
}
{
}
void ORCharacterManipulatorCtrlSet::ScalingStopManipulation ()
{
CleanForManipulation();
}
void ORCharacterManipulatorCtrlSet::AllocateState(FBCharacter* pCharacter)
{
ORCharacterSolver_HIK* lSolver = GetORCharacterSolver_HIK(pCharacter);
if(lSolver && lSolver->TargetCharacter)
{
if(mHIKCharacter !=
NULL)
{
HIKCharacterDestroy(mHIKCharacter,&free);
}
HIKCharacterHostFromFBCharacterSolver(mHIKCharacterHost, lSolver,
NULL,
true);
HIKCharacterFromFBCharacterSolver(mHIKCharacterHost, mHIKCharacter, lSolver);
HIKHostPropertiesFromCharacter(mHIKCharacterHost,pCharacter);
lSolver->RegisterExtraProperties(mHIKCharacterHost);
FBControlSet* lControlSet = pCharacter->GetCurrentControlSet();
{
HIKControlRigHostFromFBCharacterSolver(mHIKControlRigHost, lSolver, lControlSet,
NULL);
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);
}
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;
if( IsBodyPart() || IsFullBodyNoPull() )
{
lSolvingStep &= ~HIKSolvingStepBodyPull;
}
{
lSolvingStep &= ~HIKSolvingStepContact;
}
ORCharacterSolver_HIK* lConstraint = GetORCharacterSolver_HIK(lCharacter);
if(lConstraint)
{
const int LegFilter = (HIKSolvingStepLeftLegSnS | HIKSolvingStepRightLegSnS);
if(lConstraint->LegSNS)
lSolvingStep |= LegFilter;
else
lSolvingStep &= ~LegFilter;
const int ArmFilter = (HIKSolvingStepLeftArmSnS | HIKSolvingStepRightArmSnS);
if(lConstraint->ArmSNS)
lSolvingStep |= ArmFilter;
else
lSolvingStep &= ~ArmFilter;
}
return lSolvingStep;
}
void ORCharacterManipulatorCtrlSet::SyncCharacter(FBEvaluateInfo *pEvalInfo)
{
ApplySync(pEvalInfo);
}
void ORCharacterManipulatorCtrlSet::AskSync( bool pForce )
{
if(pForce)
{
}
else
{
mCurrentState.mDirtyFlag = true;
}
}
void ORCharacterManipulatorCtrlSet::ApplySync(FBEvaluateInfo *pEvalInfo, bool pForce )
{
if(mCurrentState.mDirtyFlag || pForce)
{
if(HIKGetPropertyValue(mCurrentState.mPropertySetState, HIKRollExtractionMode) == 0)
{
mHIKCurrentControlRigHostEvaluator.SetSolvingStep( GetManipulationSolvingStep() );
}
else
{
mHIKCurrentControlRigHostEvaluator.SetSolvingStep( GetManipulationSolvingStep() & ~HIKSolvingStepRollExtraction );
}
mHIKCurrentControlRigHostEvaluator.Solve(&mCurrentState);
mHIKCurrentControlRigHostEvaluator.EffectorStateFromControlSet(&mCurrentState);
mHIKCurrentControlRigHostEvaluator.WriteRigCandidate(pEvalInfo,&mCurrentState, true);
RestoreInitialState();
mCurrentState.mDirtyFlag = false;
}
}
void ORCharacterManipulatorCtrlSet::RestoreInitialState()
{
HIKCharacterStateCopy( mCurrentState.mState, mInitialState.mState );
HIKEffectorStateCopy( mCurrentState.mEffectorSetState, mInitialState.mEffectorSetState );
}
{
if(IsIKPivotUse() == false) return;
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 )
{
HIKSetRotationActive(mCurrentState.mEffectorSetState, pHIKId, 1.0);
FBMatrix lGX;
}
}
{
OR_HIK_ASSERT(pHIKId != -1);
if(IsIKPivotUse() == false) return;
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 )
{
HIKSetTranslationActive(mCurrentState.mEffectorSetState, pHIKId, 1.0);
FBMatrix lTempGX;
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];
FBTRSToMatrix(lTempGX, lInitialGT, lTempGR, *(FBSVector*)&lInitialGS);
FBSub(pGT,lInitialGT,pGT);
}
}
void ORCharacterManipulatorCtrlSet::ManipulationStart(
FBManipMode pManipMode,
const FBTime *pTime)
{
#if 1
ORCharacterSolver_HIK* lConstraintSolver = GetORCharacterSolver_HIK();
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);
const bool lRelativeRollExtraction = (HIKGetPropertyValue(mCurrentState.mPropertySetState, HIKRollExtractionMode) == 0);
lConstraintSolver->SetBlendAuxiliaryWithEffector(false);
lConstraintSolver->ControlRigInputEvaluateAndWrite(lEvalInfo, &mCurrentState, true, lConstraintSolver->GetDoubleSolve(), false, lRelativeRollExtraction ? 0 : HIKSolvingStepRollExtraction);
lConstraintSolver->SetBlendAuxiliaryWithEffector(true);
mHIKCurrentControlRigHostEvaluator.EffectorStateFromControlSet( &mCurrentState );
}
else
{
OR_HIK_ASSERT(lConstraintSolver);
}
HIKCharacterStateCopy( mInitialState.mState, mCurrentState.mState );
HIKEffectorStateCopy( mInitialState.mEffectorSetState, mCurrentState.mEffectorSetState );
#else
ORCharacterSolver_HIK* lConstraint = GetORCharacterSolver_HIK();
FBEvaluateInfo* lEvaluation = lConstraint ? lConstraint->BackgroundEvaluateInfoBegin(lEvalInfo) : lEvalInfo;
mHIKCurrentControlRigHostEvaluator.Read(lEvaluation,&mCurrentState);
if(HIKGetPropertyValue(mCurrentState.mPropertySetState, HIKRollExtractionMode) == 0)
{
mHIKCurrentControlRigHostEvaluator.SetSolvingStep(HIKSolvingStepAll);
}
else
{
mHIKCurrentControlRigHostEvaluator.SetSolvingStep(HIKSolvingStepAll & ~HIKSolvingStepRollExtraction);
}
mHIKCurrentControlRigHostEvaluator.Solve(&mCurrentState);
mHIKCurrentControlRigHostEvaluator.EffectorStateFromControlSet(&mCurrentState);
HIKCharacterStateCopy( mInitialState.mState, mCurrentState.mState );
HIKEffectorStateCopy( mInitialState.mEffectorSetState, mCurrentState.mEffectorSetState );
if(lConstraint)
{
lConstraint->BackgroundEvaluateInfoEnd(lEvaluation);
}
#endif
if (!mHIKUndoSetup) {
UndoSetup(GetCharacter());
mHIKUndoSetup = true;
}
PrepareSetForManip(pManipMode, lEvalInfo);
if (lBackEvalUsed)
{
lConstraintSolver->BackgroundEvaluateInfoEnd(lEvalInfo);
}
}
void ORCharacterManipulatorCtrlSet::PrepareSetForManip(
FBManipMode pManipMode, FBEvaluateInfo *pEvalInfo)
{
FBCharacter *lCharacter = GetCharacter();
const FBEffectorId lManipulatedEffectorId = GetCorrespondingEffector();
lCharacter->GetActiveBodyPart(lActiveBodyPart);
bool lIsFullBody = IsFullBody();
bool lIsReachOverrride = IsReachOverride();
bool lIsReleasePinPressed = IsReleasePinPressed();
if ( lIsFullBody )
{
}
float lReachR, lReachT;
{
if (lHIKId != -1)
{
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;
}
}
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);
}
}
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 )
{
HIKSetResist(mCurrentState.mEffectorSetState, LeftShoulderEffectorId, 0.35f);
HIKSetResist(mCurrentState.mEffectorSetState, RightShoulderEffectorId, 0.35f);
}
HIKSetResist(mCurrentState.mEffectorSetState, HeadEffectorId, 1.0);
}
}
if( lIsReachOverrride )
{
if ( !lIsReleasePinPressed )
{
float lValue;
for(int lIter = HipsEffectorId; lIter < LastEffectorId; lIter++)
{
if(lIter <= RightHandEffectorId && ( lIsFullBody || lActiveBodyPart[lBodyPart]) )
{
lValue = (lCharacter->IsTranslationPin((
FBEffectorId)lIter)) ? 1 : 0;
HIKSetPull( mCurrentState.mEffectorSetState, lIter, lValue );
}
else
{
HIKSetPull( mCurrentState.mEffectorSetState, lIter, 0.0 );
}
}
}
if ( lIsFullBody )
{
HIKSetPull( mCurrentState.mEffectorSetState, RightWristEffectorId, 1.0 );
HIKSetPull( mCurrentState.mEffectorSetState, LeftWristEffectorId, 1.0 );
}
}
mManipulatedEffectorIdArray[mManipulatedEffectorsCount] = lManipulatedEffectorId;
mManipulatedEffectorsCount++;
for(
int i=0;
i< mManipulatedEffectorsCount;
i++ )
{
int lId = mManipulatedEffectorIdArray[
i];
{
if( HIKGetResist(mCurrentState.mEffectorSetState, HipsEffectorId) != 1.0 )
HIKSetResist( mCurrentState.mEffectorSetState, HipsEffectorId, 1.0 );
}
{
if(HIKGetRotationActive( mCurrentState.mEffectorSetState, LeftShoulderEffectorId ) != 1.0 )
HIKSetRotationActive( mCurrentState.mEffectorSetState, LeftShoulderEffectorId, 1.0);
}
{
if(HIKGetRotationActive( mCurrentState.mEffectorSetState, RightShoulderEffectorId ) != 1.0 )
HIKSetRotationActive( mCurrentState.mEffectorSetState, RightShoulderEffectorId, 1.0);
}
}
HIKSetPropertyValue(mCurrentState.mPropertySetState, HIKPullIterationCount, 30);
}
void ORCharacterManipulatorCtrlSet::CleanForManipulation()
{
mHIKUndoSetup = false;
mManipulatedEffectorsCount = 0;
memset(mManipulatedEffectorIdArray,0,sizeof(float)*LastEffectorId);
}
bool ORCharacterManipulatorCtrlSet::NeedRotationSync(
FBEffectorId pEffectorIndex,
double pAngleTol)
{
static const double K_180_PI = 57.295779513082320876798154814105170332405472466565;
bool lRet = false;
FBCharacter *lCharacter = GetCharacter();
{
FBControlSet *lControlSet = lCharacter->GetCurrentControlSet();
{
{
FBMatrix lInitialGlobalTransformation, lInvCurrentGlobalRot, lInitialGlobalRotM, lDeltaRotM;
int lHIKId = FBEffectorIdToHIKId( pEffectorIndex );
if (lHIKId != -1)
{
FBModel *lModel = lCharacter->GetEffectorModel( pEffectorIndex );
HIKGetEffectorStatedv( mInitialState.mEffectorSetState, 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
};
HIKFillCharacterDescription(mHIKCharacter, lNodeIdDesc);
HIKGetNormalizedCharacterStateTransformTQS( mHIKCurrentControlRigHostEvaluator.mHIKCharacter, mCurrentState.mState, &lDescription, pDataSet);
}
void ORCharacterManipulatorCtrlSet::SetNormalizedLocalState( KHIKNodeState pDataSet[LastNodeId] )
{
int lNodeIdDesc[LastNodeId + 1];
HIKDataDescription lDescription = {
HIKDataDescription::HIKLocalSpace,
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
if(lExtSolver)
{
return dynamic_cast<ORCharacterSolver_HIK*>(lExtSolver);
}
}
HIKNodeId ORCharacterManipulatorCtrlSet::GetManipulatedHIKNodeId()
{
{
return FBBodyNodeIdToHIKNodeId(lBodyNodeIndex);
}
else
{
int lExtraFKIndex = GetManipulatedNodeExtraFKIndex();
if ( lExtraFKIndex == LEFT_EXTRA_COLLAR)
{
return LeftCollarExtraNodeId;
}
else if ( lExtraFKIndex == RIGHT_EXTRA_COLLAR )
{
return RightCollarExtraNodeId;
}
}
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()
{
{
{
const int lFKIndex = GetManipulatedNodeExtraFKIndex();
if ( lFKIndex == LEFT_EXTRA_COLLAR )
{
}
else if ( lFKIndex == RIGHT_EXTRA_COLLAR )
{
}
}
}
return lEffectorId;
}
bool ORCharacterManipulatorCtrlSet::IsManipulated_FK()
{
{
return (GetManipulatedNodeExtraFKIndex() != -1);
}
else
{
return true;
}
}
bool ORCharacterManipulatorCtrlSet::IsManipulated_IK()
{
}