#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)
{
if(IsReachOverride())
{
HIKSetPull( mCurrentState.mEffectorSetState, lHIKId, 0.0 );
if(!IsDescendantPulling( lEffectorIndex ))
{
HIKSetPull( mCurrentState.mEffectorSetState, lHIKId, 1.0 );
}
}
{
lT = pT;
Translation_ApplyPivot(lEffectorIndex, lHIKId, lT, lQ, lS);
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);
}
{
}
{
AddPostRotation(lModel->Parent, 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) )
{
}
FBMatrix lResultRotation(pRM);
FBModel* lModel = GetManipulatedNode();
RemovePostRotation(lModel, lResultRotation);
if(IsManipulated_FK())
{
FBMatrix lGM_FK;
FBMatrix lGM_IK;
FBMatrix lGM_Offset;
HIKNodeId lNodeId = GetManipulatedHIKNodeId();
}
Rotation_ApplyPivot(pEffectorIndex, lHIKId, lGT, lGQ, lGS);
}
}
{
FBMatrix lGlobalRotation;
FBModel* lModel = GetManipulatedNode();
if(IsManipulated_FK())
{
HIKNodeId lNodeId = GetManipulatedHIKNodeId();
FBMatrix lPGRM;
if(lParentNodeId != -1)
{
}
AddPostRotation(lModel, lPGRM);
}
else
{
if ( lModel->Parent )
{
}
AddPostRotation(lModel, lGlobalRotation);
}
RotationSetEffectorGlobal(pEffectorIndex, lGlobalRotation);
}
{
HIKNodeId lNodeId = GetManipulatedHIKNodeId();
FBMatrix lRM(pRM);
RemovePostRotation(pModel, lRM);
#if 0
if(lParentNodeId != -1)
{
FBMatrix lMatrix;
}
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();
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)
{
}
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)
{
}
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)
{
{
HIKSetResist(mCurrentState.mEffectorSetState, lIter, pResist);
}
}
void ORCharacterManipulatorCtrlSet::SetBlendFact(float pFact)
{
{
}
}
int ORCharacterManipulatorCtrlSet::GetManipulationSolvingStep()
{
FBCharacter *lCharacter = GetCharacter();
if( IsBodyPart() || IsFullBodyNoPull() )
{
}
{
}
ORCharacterSolver_HIK* lConstraint = GetORCharacterSolver_HIK(lCharacter);
if(lConstraint)
{
if(lConstraint->LegSNS)
lSolvingStep |= LegFilter;
else
lSolvingStep &= ~LegFilter;
if(lConstraint->ArmSNS)
lSolvingStep |= ArmFilter;
else
lSolvingStep &= ~ArmFilter;
}
return lSolvingStep;
}
void ORCharacterManipulatorCtrlSet::SyncCharacter(FBEvaluateInfo *pEvalInfo)
{
ApplySync(pEvalInfo);
}
void ORCharacterManipulatorCtrlSet::AskSync( bool pForce )
{
if(pForce)
{
}
else
{
mCurrentState.mDirtyFlag = true;
}
}
void ORCharacterManipulatorCtrlSet::ApplySync(FBEvaluateInfo *pEvalInfo, bool pForce )
{
if(mCurrentState.mDirtyFlag || pForce)
{
{
mHIKCurrentControlRigHostEvaluator.SetSolvingStep( GetManipulationSolvingStep() );
}
else
{
}
mHIKCurrentControlRigHostEvaluator.Solve(&mCurrentState);
mHIKCurrentControlRigHostEvaluator.EffectorStateFromControlSet(&mCurrentState);
mHIKCurrentControlRigHostEvaluator.WriteRigCandidate(pEvalInfo,&mCurrentState, true);
RestoreInitialState();
mCurrentState.mDirtyFlag = false;
}
}
void ORCharacterManipulatorCtrlSet::RestoreInitialState()
{
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 )
{
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 )
{
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);
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);
}
HIKEffectorStateCopy( mInitialState.mEffectorSetState, mCurrentState.mEffectorSetState );
#else
ORCharacterSolver_HIK* lConstraint = GetORCharacterSolver_HIK();
FBEvaluateInfo* lEvaluation = lConstraint ? lConstraint->BackgroundEvaluateInfoBegin(lEvalInfo) : lEvalInfo;
mHIKCurrentControlRigHostEvaluator.Read(lEvaluation,&mCurrentState);
{
}
else
{
}
mHIKCurrentControlRigHostEvaluator.Solve(&mCurrentState);
mHIKCurrentControlRigHostEvaluator.EffectorStateFromControlSet(&mCurrentState);
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;
}
}
}
}
if( IsStiffnessOverride() )
{
SetResist(0.0);
if ( lIsFullBody )
{
{
}
}
}
if( lIsReachOverrride )
{
if ( !lIsReleasePinPressed )
{
float lValue;
{
{
lValue = (lCharacter->IsTranslationPin((
FBEffectorId)lIter)) ? 1 : 0;
HIKSetPull( mCurrentState.mEffectorSetState, lIter, lValue );
}
else
{
HIKSetPull( mCurrentState.mEffectorSetState, lIter, 0.0 );
}
}
}
if ( lIsFullBody )
{
}
}
mManipulatedEffectorIdArray[mManipulatedEffectorsCount] = lManipulatedEffectorId;
mManipulatedEffectorsCount++;
for(
int i=0;
i< mManipulatedEffectorsCount;
i++ )
{
int lId = mManipulatedEffectorIdArray[
i];
{
}
{
}
{
}
}
}
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 );
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::HIKLocalSpace,
sizeof(KHIKNodeState),
lNodeIdDesc
};
HIKFillCharacterDescription(mHIKCharacter, lNodeIdDesc);
}
void ORCharacterManipulatorCtrlSet::SetNormalizedLocalState( KHIKNodeState pDataSet[LastNodeId] )
{
int lNodeIdDesc[LastNodeId + 1];
HIKDataDescription::HIKLocalSpace,
sizeof(KHIKNodeState),
lNodeIdDesc
};
HIKFillCharacterDescription(mHIKCharacter, lNodeIdDesc);
}
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)
{
}
else if ( lExtraFKIndex == RIGHT_EXTRA_COLLAR )
{
}
}
}
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()
{
}