constraints/CharacterSolver/HIK2013Solver/orcharactersolver_hik.cxx

constraints/CharacterSolver/HIK2013Solver/orcharactersolver_hik.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 "orcharactersolver_hik.h"
#include "filterset2fbcharacter.h"
//--- implementation and registration
FBCharacterSolverImplementation ( ORCONSTRAINTHIK__CLASS );
FBRegisterCharacterSolver ( ORCONSTRAINTHIK__NAME,
ORCONSTRAINTHIK__CLASS,
ORCONSTRAINTHIK__LABEL,
ORCONSTRAINTHIK__DESC,
"character_solver.tif" ); // Icon filename (default=Open Reality icon)
int Actor2HIK[] =
{
HipsNodeId, // HipsIndex
LeftHipNodeId, // LeftHipIndex
LeftKneeNodeId, // LeftKneeIndex
LeftAnkleNodeId, // LeftAnkleIndex
LeftFootNodeId, // LeftFootIndex
RightHipNodeId, // RightHipIndex
RightKneeNodeId, // RightKneeIndex
RightAnkleNodeId, // RightAnkleIndex
RightFootNodeId, // RightFootIndex
WaistNodeId, // WaistIndex
Spine1NodeId, // ChestIndex
LeftCollarNodeId, // LeftCollarIndex
LeftShoulderNodeId, // LeftShoulderIndex
LeftElbowNodeId, // LeftElbowIndex
LeftWristNodeId, // LeftWristIndex
RightCollarNodeId, // RightCollarIndex
RightShoulderNodeId, // RightShoulderIndex
RightElbowNodeId, // RightElbowIndex,
RightWristNodeId, // RightWristIndex,
NeckNodeId, // NeckIndex
HeadNodeId, // HeadIndex
LeftThumbANodeId,
LeftThumbBNodeId,
LeftThumbCNodeId,
LeftIndexANodeId,
LeftIndexBNodeId,
LeftIndexCNodeId,
LeftMiddleANodeId,
LeftMiddleBNodeId,
LeftMiddleCNodeId,
LeftRingANodeId,
LeftRingBNodeId,
LeftRingCNodeId,
LeftPinkyANodeId,
LeftPinkyBNodeId,
LeftPinkyCNodeId,
RightThumbANodeId,
RightThumbBNodeId,
RightThumbCNodeId,
RightIndexANodeId,
RightIndexBNodeId,
RightIndexCNodeId,
RightMiddleANodeId,
RightMiddleBNodeId,
RightMiddleCNodeId,
RightRingANodeId,
RightRingBNodeId,
RightRingCNodeId,
RightPinkyANodeId,
RightPinkyBNodeId,
RightPinkyCNodeId,
ReferenceNodeId, // ReferenceIndex
};
// keep synchronicity between Tables, if there is a compilation error, it's because there is a mismatch in the number of Index in the table and the FBSkeletonNodeId Enum
K_STATIC_ASSERT( sizeof(Actor2HIK)/sizeof(int) == kFBSkeletonLastIndex );
static void LegSNSSet(HIObject pObject, bool pValue)
{
ORCharacterSolver_HIK* lConstraint = FBCast<ORCharacterSolver_HIK>(pObject);
int LegFilter = (HIKSolvingStepLeftLegSnS | HIKSolvingStepRightLegSnS);
if(pValue)
lConstraint->SolvingStepFilter |= LegFilter;
else
lConstraint->SolvingStepFilter &= ~LegFilter;
lConstraint->LegSNS.SetPropertyValue(pValue);
lConstraint->mHIKCharacterHostEvaluator.SetSolvingStep( lConstraint->SolvingStepFilter );
lConstraint->mHIKControlRigHostEvaluator.SetSolvingStep( lConstraint->SolvingStepFilter );
}
static void ArmSNSSet(HIObject pObject, bool pValue)
{
ORCharacterSolver_HIK* lConstraint = FBCast<ORCharacterSolver_HIK>(pObject);
int ArmFilter = (HIKSolvingStepLeftArmSnS | HIKSolvingStepRightArmSnS);
if(pValue)
lConstraint->SolvingStepFilter |= ArmFilter;
else
lConstraint->SolvingStepFilter &= ~ArmFilter;
lConstraint->ArmSNS.SetPropertyValue(pValue);
lConstraint->mHIKCharacterHostEvaluator.SetSolvingStep( lConstraint->SolvingStepFilter );
lConstraint->mHIKControlRigHostEvaluator.SetSolvingStep( lConstraint->SolvingStepFilter );
}
int ORCharacterSolver_HIK::mInternalPropertiesCount = 0;
int ORCharacterSolver_HIK::mSolverPropertiesCount = 0;
/************************************************
* Creation function.
************************************************/
bool ORCharacterSolver_HIK::FBCreate()
{
mHIKCharacter = NULL;
mHIKCharacterSrc = NULL;
mHIKActorSrc = NULL;
mHIKEffectorSetStatePlot = NULL;
mHIKCharacterStateStance = NULL;
mHIKCharacterDebug = NULL;
mHIKCharacterStateDebug = NULL;
mHIKCharacterStateOutDebug = NULL;
mHIKEffectorSetStateDebug = NULL;
mHIKPropertySetStateDebug = NULL;
mControlSetManipulator = NULL;
mEvaluationId = -1;
mAlreadyConnected = false;
HIKHostPropertiesInit(mHIKCharacterHost);
HIKHostPropertiesInit(mHIKCharacterHostSrc);
HIKHostPropertiesInit(mHIKCharacterHostDebug);
EventListen(NULL,NULL);
// Register proper callbacks to suspend notification and avoid overload.
mApplication.OnFileNewCompleted.Add( this, (FBCallback)&ORCharacterSolver_HIK::EventListen );
mApplication.OnFileOpenCompleted.Add( this, (FBCallback)&ORCharacterSolver_HIK::EventListen );
mApplication.OnFileNew.Add( this, (FBCallback)&ORCharacterSolver_HIK::EventSuspend );
mApplication.OnFileOpen.Add( this, (FBCallback)&ORCharacterSolver_HIK::EventSuspend );
mApplication.OnFileExit.Add( this, (FBCallback)&ORCharacterSolver_HIK::EventSuspend );
FBPropertyPublish(this, ExtraCollarRatio,"Extra Collar Ratio", NULL, NULL);
ExtraCollarRatio.SetMinMax(0.,100.);
FBPropertyPublish(this, LegSNS, "Leg SNS", NULL, LegSNSSet);
FBPropertyPublish(this, ArmSNS, "Arm SNS", NULL, ArmSNSSet);
FBPropertyPublish(this, CollarStiffnessX, "Collar Stiffness X", NULL, NULL);
FBPropertyPublish(this, CollarStiffnessY, "Collar Stiffness Y", NULL, NULL);
FBPropertyPublish(this, CollarStiffnessZ, "Collar Stiffness Z", NULL, NULL);
CollarStiffnessX.SetMinMax(-100.,100.);
CollarStiffnessY.SetMinMax(-100.,100.);
CollarStiffnessZ.SetMinMax(-100.,100.);
FBPropertyPublish(this, ReachLeftShoulder,"Reach Left Shoulder", NULL, NULL);
FBPropertyPublish(this, ReachRightShoulder,"Reach Right Shoulder", NULL, NULL);
ReachLeftShoulder.SetMinMax(0, 100);
ReachRightShoulder.SetMinMax(0, 100);
FBPropertyPublish(this, FingerSolvingPropagation, "Finger Propagation", NULL, NULL);
FBPropertyPublish(this, RealisticLeftKneeSolving, "Realistic Left Knee Solving", NULL, NULL);
FBPropertyPublish(this, RealisticRightKneeSolving, "Realistic Right Knee Solving", NULL, NULL);
RealisticLeftKneeSolving.SetMinMax(0, 100);
RealisticRightKneeSolving.SetMinMax(0, 100);
// SnS
FBPropertyPublish(this, StretchStartArmsAndLegs,"StretchStartArmsAndLegs", NULL, NULL);
FBPropertyPublish(this, StretchStopArmsAndLegs,"StretchStopArmsAndLegs", NULL, NULL);
FBPropertyPublish(this, SnSScaleArmsAndLegs,"SnSScaleArmsAndLegs", NULL, NULL);
FBPropertyPublish(this, SnSReachLeftWrist,"SnSReachLeftWrist", NULL, NULL);
FBPropertyPublish(this, SnSReachRightWrist,"SnSReachRightWrist", NULL, NULL);
FBPropertyPublish(this, SnSReachLeftAnkle,"SnSReachLeftAnkle", NULL, NULL);
FBPropertyPublish(this, SnSReachRightAnkle,"SnSReachRightAnkle", NULL, NULL);
FBPropertyPublish(this, SnSScaleSpine,"SnSScaleSpine", NULL, NULL);
FBPropertyPublish(this, SnSScaleSpineChildren,"SnSScaleSpineChildren", NULL, NULL);
FBPropertyPublish(this, SnSSpineFreedom,"SnSSpineFreedom", NULL, NULL);
FBPropertyPublish(this, SnSReachChestEnd,"SnSReachChestEnd", NULL, NULL);
FBPropertyPublish(this, SnSScaleNeck,"SnSScaleNeck", NULL, NULL);
FBPropertyPublish(this, SnSNeckFreedom,"SnSNeckFreedom", NULL, NULL);
FBPropertyPublish(this, SnSReachHead,"SnSReachHead", NULL, NULL);
StretchStartArmsAndLegs.SetMinMax(0,100);
StretchStopArmsAndLegs.SetMinMax(100,200);
SnSScaleArmsAndLegs.SetMinMax(0,100);
SnSReachLeftWrist.SetMinMax(0,100);
SnSReachRightWrist.SetMinMax(0,100);
SnSReachLeftAnkle.SetMinMax(0,100);
SnSReachRightAnkle.SetMinMax(0,100);
SnSScaleSpine.SetMinMax(0,100);
SnSScaleSpineChildren.SetMinMax(0,100);
SnSSpineFreedom.SetMinMax(0,100);
SnSReachChestEnd.SetMinMax(0,100);
SnSScaleNeck.SetMinMax(0,100);
SnSNeckFreedom.SetMinMax(0,100);
SnSReachHead.SetMinMax(0,100);
ResetPropertiesToDefault();
SolvingStepFilter = HIKSolvingStepAll;
//hide the active property
Active.ModifyPropertyFlag(kFBPropertyFlagHideProperty, true);
if( mInternalPropertiesCount == 0 && mSolverPropertiesCount == 0 )
{
ORCharacterSolver_HIK lSolver("TestSolver");
mInternalPropertiesCount = lSolver.PropertyList.GetCount();
lSolver.FBCreate();
mSolverPropertiesCount = lSolver.PropertyList.GetCount();
}
return true;
}
/************************************************
* Destruction function.
************************************************/
void ORCharacterSolver_HIK::FBDestroy()
{
if(mHIKCharacter != NULL)
{
HIKCharacterDestroy(mHIKCharacter,&free);
mHIKCharacter = NULL;
}
if(mHIKCharacterSrc != NULL)
{
HIKCharacterDestroy(mHIKCharacterSrc,&free);
mHIKCharacterSrc = NULL;
}
if(mHIKCharacterSrc != NULL)
{
HIKCharacterDestroy(mHIKCharacterSrc,&free);
mHIKCharacterSrc = NULL;
}
if(mHIKActorSrc != NULL)
{
HIKCharacterDestroy(mHIKActorSrc,&free);
mHIKActorSrc = NULL;
}
if(mHIKEffectorSetStatePlot != NULL)
{
HIKEffectorSetStateDestroy(mHIKEffectorSetStatePlot,&free);
mHIKEffectorSetStatePlot = NULL;
}
if(mHIKCharacterStateStance != NULL)
{
HIKCharacterStateDestroy(mHIKCharacterStateStance,&free);
mHIKCharacterStateStance = NULL;
}
if(mHIKCharacterDebug != NULL)
{
HIKCharacterDestroy(mHIKCharacterDebug,&free);
mHIKCharacter = NULL;
}
if(mHIKCharacterStateDebug != NULL)
{
HIKCharacterStateDestroy(mHIKCharacterStateDebug,&free);
mHIKCharacterStateDebug = NULL;
}
if(mHIKCharacterStateOutDebug != NULL)
{
HIKCharacterStateDestroy(mHIKCharacterStateOutDebug,&free);
mHIKCharacterStateOutDebug = NULL;
}
if(mHIKEffectorSetStateDebug != NULL)
{
HIKEffectorSetStateDestroy(mHIKEffectorSetStateDebug,&free);
mHIKEffectorSetStateDebug = NULL;
}
if(mHIKPropertySetStateDebug != NULL)
{
HIKPropertySetStateDestroy(mHIKPropertySetStateDebug,&free);
mHIKPropertySetStateDebug = NULL;
}
RemoveAllAnimationNodes();
EventSuspend(NULL, NULL);
}
static bool SolveDestinationOnControlRig(FBCharacter* pCharacter)
{
if(pCharacter->IsPlottingActorToCtrlRig())
{
FBControlSet* lControlSet = pCharacter->GetCurrentControlSet(true);
return (lControlSet && lControlSet->ControlSetType == kFBControlSetTypeFKIK);
}
return false;
}
/************************************************
* Setup all of the animation nodes.
************************************************/
void ORCharacterSolver_HIK::SetupAllAnimationNodes()
{
FBCharacter* lCharacter = TargetCharacter;
FBComponent* lSource = Source;
FBCharacter* lCharacterSrc = dynamic_cast<FBCharacter*>(lSource);
FBControlSet* lControlRig = dynamic_cast<FBControlSet*>(lSource);
mActorSrc = dynamic_cast<FBActor*>(lSource);
if(lCharacter != NULL && lCharacter->GetCharacterize())
{
//call the manipulator to update the state
if(mControlSetManipulator)
{
mControlSetManipulator->DeallocateState();
mControlSetManipulator->AllocateState(lCharacter);
}
SetupTargetCharacter(lCharacter);
SetupAutoProperties(lCharacter);
if(lControlRig != NULL)
{
SetupInputControlRig( lControlRig, GetRigAlign() );
}
else if(lCharacterSrc != NULL)
{
HIKCharacterHostFromMBCharacterHierarchy(mHIKCharacterHostSrc,lCharacterSrc,NULL);
mHIKCharacterSrc = mHIKCharacterHostSrc.CharacterCreate(&malloc);
HIKCharacterHostFromMBCharacterGeometry(mHIKCharacterHostSrc,mHIKCharacterSrc,lCharacterSrc);
HIKCharacterizeGeometry(mHIKCharacterSrc);
HIKHostPropertiesFromCharacter(mHIKCharacterHostSrc,lCharacterSrc);
mHIKCharacterHostEvaluator.Init(mHIKCharacter,&mHIKCharacterHost,mHIKCharacterSrc,&mHIKCharacterHostSrc,&malloc);
RegisterExtraProperties(mHIKCharacterHostSrc); // NOT RIGHT, VALUE OF TARGET ON HOST, NEED TO CHANGE
}
else if(mActorSrc != NULL)
{
HIKCharacterDefinition lHIKDef;
for(int lNodeIter = 0; lNodeIter < LastNodeId; lNodeIter++)
{
lHIKDef.mUsedNodes[lNodeIter] = HIKNodeNotUsed;
}
for(int lActorIter = kFBSkeletonHipsIndex; lActorIter < kFBSkeletonLastIndex; lActorIter++)
{
lHIKDef.mUsedNodes[Actor2HIK[lActorIter]] |= HIKNodeUsed;
}
mHIKActorSrc = HIKCharacterCreate(&lHIKDef,&malloc,AutodeskCustomerString,AutodeskCustomerKey);
FBSkeletonState* lSkeletonState = mActorSrc->GetDefaultSkeletonState();
FBMatrix lGX;
if (lSkeletonState)
{
for(int lActorIter = kFBSkeletonHipsIndex; lActorIter < kFBSkeletonLastIndex; lActorIter++)
{
FBMatrix lGX;
FBSVector lGS;
mActorSrc->GetDefinitionScaleVector((FBSkeletonNodeId)lActorIter, lGS);
lSkeletonState->GetNodeMatrix((FBSkeletonNodeId)lActorIter, lGX);
FBMult(lGX,lGX,lGS);
HIKSetCharacterizeNodeStatedv(mHIKActorSrc,Actor2HIK[lActorIter], (double*)lGX);
}
}
HIKCharacterizeGeometry(mHIKActorSrc);
mHIKCharacterHostEvaluator.Init(mHIKCharacter,&mHIKCharacterHost,mHIKActorSrc,NULL,&malloc);
}
bool lPlotToRig = lCharacter->InputType == kFBCharacterOutputMarkerSet || lCharacter->IsPlottingActorToCtrlRig();
if (lPlotToRig)
{
SetupInputControlRig( lCharacter->GetCurrentControlSet(true), true );
}
}
mHIKCharacterHostEvaluator.SetSolvingStep( SolvingStepFilter );
mHIKControlRigHostEvaluator.SetSolvingStep( SolvingStepFilter );
GetState(0);
}
void ORCharacterSolver_HIK::SetupTargetCharacter(FBCharacter* pCharacter)
{
//
// Init the HIK Character Host
//
HIKCharacterHostFromMBCharacterHierarchy(mHIKCharacterHost,pCharacter,this, SolveDestinationOnControlRig(pCharacter));
//Add the 2 new clavicle bones
FBModel *lLeftShoulderExtra = GetExtraBoneModelAt(LEFT_EXTRA_COLLAR);
FBModel *lRightShoulderExtra = GetExtraBoneModelAt(RIGHT_EXTRA_COLLAR);
if (lLeftShoulderExtra || lRightShoulderExtra)
HIKCharacterHostFromHIKHostMB(mHIKCharacterHost, lLeftShoulderExtra, lRightShoulderExtra, this);
mHIKCharacter = mHIKCharacterHost.CharacterCreate(&malloc);
HIKCharacterHostFromMBCharacterGeometry(mHIKCharacterHost,mHIKCharacter,pCharacter);
//Characterize the 2 new clavicle bones
if(lLeftShoulderExtra || lRightShoulderExtra)
{
//Characterize the 2 new clavicle bones
FBVector3d lLeftT, lRightT;
FBRVector lLeftR, lRightR, lLeftRPOff, lRightRPOff;
FBSVector lLeftS, lRightS;
GetTransformOffset(lLeftT, lLeftR, lLeftS, LEFT_EXTRA_COLLAR);
GetParentRotationOffset(lLeftRPOff, LEFT_EXTRA_COLLAR);
GetTransformOffset(lRightT, lRightR, lRightS, RIGHT_EXTRA_COLLAR);
GetParentRotationOffset(lRightRPOff, RIGHT_EXTRA_COLLAR);
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);
RegisterExtraProperties(mHIKCharacterHost);
//Create the stance pose state
mHIKCharacterStateStance = HIKCharacterStateCreate(mHIKCharacter, &malloc);
HIKGetDefaultState(mHIKCharacter, mHIKCharacterStateStance);
}
void ORCharacterSolver_HIK::SetupExtraFK(FBCharacter* pCharacter, int pExtraBoneId, FBModelMarker** pExtraMarker)
{
FBModel* lParent = NULL;
FBSVector lS;
if((*pExtraMarker))
{
//check if it is still connected. Might be disconnect on a reset hierarchy
if((*pExtraMarker)->Children.GetCount() == 0)
{
(*pExtraMarker)->FBDelete();
(*pExtraMarker) = NULL;
}
}
if (!(*pExtraMarker))
{
(*pExtraMarker) = new FBModelMarker(pExtraBoneId == LEFT_EXTRA_COLLAR ? "Left Shoulder Extra FK" : "Right Shoulder Extra FK");
SetExtraFKModelAt((*pExtraMarker), pExtraBoneId );
// insert in FK Hiarchy
FBModel *lShoulderFK = pCharacter->GetCtrlRigModel(pExtraBoneId == LEFT_EXTRA_COLLAR ? kFBLeftCollarNodeId:kFBRightCollarNodeId);
if (lShoulderFK)
{
for (int lIter = 0; lIter < lShoulderFK->Children.GetCount(); lIter++)
{
FBModel *lChild = dynamic_cast<FBModel*>(lShoulderFK->Children.GetAt(lIter));
(*pExtraMarker)->Children.Add( lChild );
}
lShoulderFK->Children.RemoveAll();
lShoulderFK->Children.Add((*pExtraMarker));
lParent = lShoulderFK;
}
}
else
{
lParent = (*pExtraMarker);
}
FBModel *lShoulderExtraBone = GetExtraBoneModelAt(pExtraBoneId);
for (int lIter = 0; lIter < lShoulderExtraBone->Children.GetCount(); lIter++)
{
FBModel *lChild = dynamic_cast<FBModel*>(lShoulderExtraBone->Children.GetAt(lIter));
FBMatrix lMatrix;
pCharacter->GetTransformOffset(pCharacter->GetIndexByModel(lChild), &lMatrix);
FBModel* lChildFK = dynamic_cast<FBModel*>(lParent->Children.GetAt(lIter));
FBTVector lTVector;
FBMatrixToTranslation(lTVector, lMatrix);
lT.Set(lTVector);
lChildFK->SetVector(lT);
}
GetTransformOffset(lT, lR, lS, pExtraBoneId);
(*pExtraMarker)->SetVector(lT, kModelTranslation, true);
}
void ORCharacterSolver_HIK::SetupInputControlRig(FBControlSet* pControlRig, bool pConstraintRig /*=false*/)
{
// we Create(if needed) and characterize the Shoulder Effector
FBModelMarker* lLeftShoulderExtraFK = NULL;
FBModelMarker* lRightShoulderExtraFK = NULL;
if(GetExtraBoneModelAt(LEFT_EXTRA_COLLAR) || GetExtraBoneModelAt(RIGHT_EXTRA_COLLAR) )
{
FBCharacter* lCharacter = TargetCharacter;
lLeftShoulderExtraFK = dynamic_cast<FBModelMarker*>(GetExtraFKModelAt(LEFT_EXTRA_COLLAR));
lRightShoulderExtraFK = dynamic_cast<FBModelMarker*>(GetExtraFKModelAt(RIGHT_EXTRA_COLLAR));
FBMatrix lMatrix;
FBSVector lS;
//not fully implemented yet
if( GetExtraBoneModelAt(LEFT_EXTRA_COLLAR) )
{
SetupExtraFK(lCharacter, LEFT_EXTRA_COLLAR, &lLeftShoulderExtraFK);
}
if( GetExtraBoneModelAt(RIGHT_EXTRA_COLLAR) )
{
SetupExtraFK(lCharacter, RIGHT_EXTRA_COLLAR, &lRightShoulderExtraFK);
/*
if(lRightShoulderExtraFK)
{
//check if it is still connected. Might be disconnect on a reset hierarchy
if(lRightShoulderExtraFK->Children.GetCount() == 0)
{
lRightShoulderExtraFK->Destroy();
lRightShoulderExtraFK = NULL;
}
}
if (!lRightShoulderExtraFK)
{
lRightShoulderExtraFK = new FBModelMarker("Right Shoulder Extra FK");
SetExtraFKModelAt(lRightShoulderExtraFK, RIGHT_EXTRA_COLLAR );
// insert in FK Hiarchy
FBModel *lShoulderFK = lCharacter->GetCtrlRigModel(kFBRightCollarNodeId);
if (lShoulderFK)
{
for (int lIter = 0; lIter < lShoulderFK->Children.GetCount(); lIter++)
{
FBModel *lChild = dynamic_cast<FBModel*>(lShoulderFK->Children.GetAt(lIter));
lRightShoulderExtraFK->Children.Add( lChild );
}
lShoulderFK->Children.RemoveAll();
lShoulderFK->Children.Add(lRightShoulderExtraFK);
lParent = lShoulderFK;
}
}
else
{
lParent = lRightShoulderExtraFK;
}
FBModel *lShoulderExtraBone = GetExtraBoneModelAt(RIGHT_EXTRA_COLLAR);
for (int lIter = 0; lIter < lShoulderExtraBone->Children.GetCount(); lIter++)
{
FBModel *lChild = dynamic_cast<FBModel*>(lShoulderExtraBone->Children.GetAt(lIter));
FBMatrix lMatrix;
lCharacter->GetTransformOffset(lCharacter->GetIndexByModel(lChild), &lMatrix);
FBModel* lChildFK = dynamic_cast<FBModel*>(lParent->Children.GetAt(lIter));
FBTVector lTVector;
FBMatrixToTranslation(lTVector, lMatrix);
FBMatrixToRotation(lR, lMatrix);
lT.Set(lTVector);
lChildFK->SetVector(lT);
}
*/
GetTransformOffset(lT, lR, lS, RIGHT_EXTRA_COLLAR);
lRightShoulderExtraFK->SetVector(lT, kModelTranslation, true);
}
}
HIKControlRigHostFromMBControlRig(mHIKControlRigHost, pControlRig, (pConstraintRig) ? this : NULL );
HIKExtraShoulderControlRigHost(mHIKControlRigHost,lLeftShoulderExtraFK, lRightShoulderExtraFK, (pConstraintRig) ? this : NULL );
mHIKControlRigHostEvaluator.Init(mHIKCharacter,&mHIKCharacterHost,&mHIKControlRigHost, &malloc);
}
void ORCharacterSolver_HIK::SetupAutoProperties(FBCharacter* pCharacter)
{
for ( int i = 0; i < HIKLastPropertyId; i++ )
{
if ( HIKGetPropertyInfoModeType(i) == HIKPropertyOffAutoUser || HIKGetPropertyInfoModeType(i) == HIKPropertyAutoUser )
{
// this property may be in automatic Mode
FBAnimationNode* lANode = AnimationNodeInCreate( 2000+i, mHIKCharacterHost.GetProperty(i).Get().mValueP );
if (lANode)
mAutoPropertyConnArray.Add( lANode );
}
}
}
/************************************************
* Removed all of the animation nodes.
************************************************/
void ORCharacterSolver_HIK::RemoveAllAnimationNodes()
{
// all animation Nodes are cleared by the system
// lean HIK
if(mHIKCharacter != NULL)
{
HIKCharacterDestroy(mHIKCharacter,&free);
mHIKCharacter = NULL;
}
if(mHIKCharacterSrc != NULL)
{
HIKCharacterDestroy(mHIKCharacterSrc,&free);
mHIKCharacterSrc = NULL;
}
if(mHIKActorSrc != NULL)
{
HIKCharacterDestroy(mHIKActorSrc,&free);
mHIKActorSrc = NULL;
}
for(int lNodeIter = 0; lNodeIter < LastNodeId; lNodeIter++)
{
mHIKCharacterHost.GetNode(lNodeIter).Get().Clear();
mHIKCharacterHostSrc.GetNode(lNodeIter).Get().Clear();
mHIKControlRigHost.GetNode(lNodeIter).Get().Clear();
}
for(int lEffIter = 0; lEffIter < kFBLastEffectorId; lEffIter++)
{
for(int lSetIter = 0; lSetIter < FBLastEffectorSetIndex; lSetIter++)
{
mHIKControlRigHost.GetEffector(lEffIter,lSetIter).Get().Clear();
}
}
mHIKControlRigHostEvaluator.Clear(&free);
mHIKCharacterHostEvaluator.Clear(&free);
mAutoPropertyConnArray.Clear();
ClearStates();
}
/************************************************
* FBX storage of constraint parameters.
************************************************/
bool ORCharacterSolver_HIK::FbxStore( FBFbxObject* pFbxObject, kFbxObjectStore pStoreWhat )
{
return true;
}
/************************************************
* FBX retrieval of constraint parameters.
************************************************/
bool ORCharacterSolver_HIK::FbxRetrieve( FBFbxObject* pFbxObject, kFbxObjectStore pStoreWhat )
{
return true;
}
/************************************************
* Suggest a freeze.
************************************************/
void ORCharacterSolver_HIK::FreezeSuggested()
{
FBConstraint::FreezeSuggested();
if( ReferenceGet( 0,0 ) )
{
FreezeSRT( (FBModel*)ReferenceGet( 0, 0), true, true, true );
}
}
/************************************************
* MultiState implementation.
************************************************/
HIKEvaluationState* ORCharacterSolver_HIK::EvaluationStateCreator()
{
HIKEvaluationState* lNewState = new HIKEvaluationState();
bool lInitControlRig = (mHIKControlRigHostEvaluator.mHIKCharacter != NULL); //if it's initialized
if(mHIKActorSrc != NULL)
{
lNewState->Init(mHIKCharacter, mHIKActorSrc, lInitControlRig, &malloc);
}
else
{
lNewState->Init(mHIKCharacter, mHIKCharacterSrc, lInitControlRig, &malloc);
}
return lNewState;
}
/************************************************
* Real-Time Engine Evaluation
************************************************/
bool ORCharacterSolver_HIK::AnimationNodeNotify(FBAnimationNode* pConnector,FBEvaluateInfo* pEvaluateInfo,FBConstraintInfo* pConstraintInfo)
{
// because we want to catch all possible patch for double solve we allow constraint to loop back.
// infinite recursion prevention
if(pEvaluateInfo->GetRecursionLevel(this) > 0)
{
// there should only one level of recursion be allowed, pEvaluateInfo->GetRecursionLevel(this) == 1.
return false;
}
//we constraint character reference but not write any data. This makes problem for MT Advance, because without writing data we don't build dependency link.
//we will do similar to internal character and catch that special case.
if(mHIKControlRigHostEvaluator.mHIKCharacterHost)
{
if(mHIKControlRigHostEvaluator.mHIKCharacterHost->GetNode(ReferenceNodeId).Valid())
{
if( mHIKControlRigHostEvaluator.mHIKCharacterHost->GetNode(ReferenceNodeId).Get().IsDestinationConn(pConnector) )
return false;
}
}
if(mHIKControlRigHostEvaluator.mHIKControlRigHost)
{
if(mHIKControlRigHostEvaluator.mHIKControlRigHost->GetNode(ReferenceNodeId).Valid())
{
if( mHIKControlRigHostEvaluator.mHIKControlRigHost->GetNode(ReferenceNodeId).Get().IsDestinationConn(pConnector) )
return false;
}
}
HIKEvaluationState* lCurrentState = GetState(pEvaluateInfo);
if(lCurrentState->mEvaluateId != pEvaluateInfo->GetEvaluationID())
{
lCurrentState->mEvaluateId = pEvaluateInfo->GetEvaluationID();
if(mHIKActorSrc != NULL)
{
mActorSrc->UpdateValues(pEvaluateInfo);
FBMatrix lGX;
FBSVector lGS;
FBSkeletonState* lSkeletonState = mActorSrc->GetCurrentSkeletonState ();
for(int lActorIter = kFBSkeletonHipsIndex; lActorIter < kFBSkeletonLastIndex; lActorIter++)
{
mActorSrc->GetDefinitionScaleVector((FBSkeletonNodeId)lActorIter, lGS);
lSkeletonState->GetNodeMatrix((FBSkeletonNodeId)lActorIter, lGX);
FBMult(lGX,lGX,lGS);
HIKSetNodeStatedv(mHIKCharacterHostEvaluator.mHIKCharacterSrc, lCurrentState->mStateSrc,Actor2HIK[lActorIter], (double*)lGX);
}
for(int lPIter = 0; lPIter < HIKLastPropertyId; lPIter++)
{
HIKSetPropertyMode(lCurrentState->mSrcPropertySetState,lPIter,HIKGetPropertyInfoDefaultMode(lPIter));
if(lPIter == HIKFootBottomToAnkleId)
{
FBSVector lGS;
mActorSrc->GetDefinitionScaleVector(kFBSkeletonLeftAnkleIndex, lGS);
HIKSetPropertyValue(lCurrentState->mSrcPropertySetState,lPIter,7.5f*lGS[1]);
}
else
{
HIKSetPropertyValue(lCurrentState->mSrcPropertySetState,lPIter,HIKGetPropertyInfoDefaultValue(lPIter));
}
}
}
if(mHIKCharacterHostEvaluator.mHIKCharacterDst != 0)
{
mHIKCharacterHostEvaluator.Read(pEvaluateInfo, lCurrentState);
if(lCurrentState->mDstPropertySetState)
{
// inverted chest offset
HIKSetPropertyValue( lCurrentState->mDstPropertySetState, HIKChestTOffsetXId, -1.0 * HIKGetPropertyValue(lCurrentState->mDstPropertySetState, HIKChestTOffsetXId));
HIKSetPropertyValue( lCurrentState->mDstPropertySetState, HIKChestTOffsetYId, -1.0 * HIKGetPropertyValue(lCurrentState->mDstPropertySetState, HIKChestTOffsetYId));
HIKSetPropertyValue( lCurrentState->mDstPropertySetState, HIKChestTOffsetZId, -1.0 * HIKGetPropertyValue(lCurrentState->mDstPropertySetState, HIKChestTOffsetZId));
}
FBCharacter* lCharacter = TargetCharacter;
mHIKCharacterHostEvaluator.Solve(lCurrentState);
mHIKCharacterHostEvaluator.Write(pEvaluateInfo, lCurrentState, SolveDestinationOnControlRig(lCharacter));
if(lCharacter->IsPlottingActorToCtrlRig())
{
//get the effector state, set the ik effector TRS
if(mHIKEffectorSetStatePlot == NULL)
{
mHIKEffectorSetStatePlot = HIKEffectorSetStateCreate(&malloc);
}
HIKEffectorSetFromCharacter(mHIKCharacterHostEvaluator.mHIKCharacterDst, mHIKEffectorSetStatePlot,
lCurrentState->mStateDst, lCurrentState->mDstPropertySetState);
if(mHIKControlRigHostEvaluator.mHIKControlRigHost)
{
mHIKControlRigHostEvaluator.mHIKControlRigHost->WriteEffectors(mHIKEffectorSetStatePlot, pEvaluateInfo, false);
mHIKControlRigHostEvaluator.mHIKControlRigHost->WriteState(mHIKControlRigHostEvaluator.mHIKCharacter, lCurrentState->mStateDst, pEvaluateInfo, true);
}
}
}
else if(mHIKControlRigHostEvaluator.mHIKCharacter != 0)
{
bool lPlotToRig = TargetCharacter->InputType == kFBCharacterOutputMarkerSet || TargetCharacter->IsPlottingActorToCtrlRig();
if (lPlotToRig)
{
mHIKControlRigHostEvaluator.ReadSkeletonSolveOnRig(pEvaluateInfo,lCurrentState);
mHIKControlRigHost.WriteState(mHIKCharacter, lCurrentState->mCRState, pEvaluateInfo, true);
FBControlSet* lCtrlSet = TargetCharacter->GetCurrentControlSet(true);
for(int lIKIter = kFBHipsEffectorId; lIKIter < kFBLastEffectorId && mHIKControlRigHostEvaluator.mHIKControlRigHost; lIKIter++)
{
int lPivotCount = lCtrlSet->GetIKEffectorPivotCount((FBEffectorId)lIKIter);
for(int lPivotIter = 0; lPivotIter < lPivotCount; lPivotIter++)
{
if ( mHIKControlRigHostEvaluator.mHIKControlRigHost->GetEffector(lIKIter,lPivotIter).Valid() )
{
FBRVector lRot;
HIKGetEffectorStateTQSdv(lCurrentState->mCREffectorSetState, lIKIter, lT.mValue, lQ.mValue, lS.mValue);
mHIKControlRigHostEvaluator.mHIKControlRigHost->GetEffector(lIKIter,lPivotIter).Get().mDestTConn->WriteData(lT,pEvaluateInfo);
mHIKControlRigHostEvaluator.mHIKControlRigHost->GetEffector(lIKIter,lPivotIter).Get().mDestRConn->WriteData(lRot,pEvaluateInfo);
mHIKControlRigHostEvaluator.mHIKControlRigHost->GetEffector(lIKIter,lPivotIter).Get().mDestSConn->WriteData(lS,pEvaluateInfo);
}
}
}
}
else
{
FBControlSet* lControlSet = TargetCharacter->GetCurrentControlSet(true);
FBEvaluateInfo* lEvaluation = GetRigAlign() ? BackgroundEvaluateInfoBegin(pEvaluateInfo) : pEvaluateInfo;
mHIKControlRigHostEvaluator.Read(lEvaluation,lControlSet->ControlSetType == kFBControlSetTypeFKIK,lCurrentState);
mHIKControlRigHostEvaluator.Solve(lCurrentState);
mHIKControlRigHostEvaluator.Write(pEvaluateInfo,lCurrentState,GetRigAlign());
if(!GetRigAlign())
SyncPivot(lControlSet);
else
BackgroundEvaluateInfoEnd(lEvaluation);
}
}
else
{
//Stance
mHIKCharacterHost.WriteState(mHIKCharacter, mHIKCharacterStateStance, pEvaluateInfo, false);
}
}
// Optionnal Code, Only update UI Values to the Automatic Values
HIKPropertySetState * lPropState = (lCurrentState->mDstPropertySetState) ? lCurrentState->mDstPropertySetState : lCurrentState->mCRPropertySetState;
if (lPropState)
{
int lHIKPropIndex = pConnector->Reference - 2000;
if ( lHIKPropIndex >= 0 && lHIKPropIndex < HIKLastPropertyId)
{
if ( HIKIsPropertyAuto(lPropState, lHIKPropIndex) )
{
double lValue = HIKGetPropertyValue( lPropState, lHIKPropIndex );
pConnector->WriteData(&lValue, pEvaluateInfo);
}
}
}
return true;
}
FBCharacterManipulatorCtrlSet* ORCharacterSolver_HIK::CreateCharacterManipulatorCtrlSet(const char* pName)
{
mControlSetManipulator = new ORCharacterManipulatorCtrlSet(pName);
mControlSetManipulator->FBCreate();
return mControlSetManipulator;
}
void ORCharacterSolver_HIK::EventListen( HISender pSender, HKEvent pEvent )
{
if(!mAlreadyConnected)
{
mSystem.OnConnectionNotify.Add( this, (FBCallback)&ORCharacterSolver_HIK::ConnectionNotify);
mAlreadyConnected = true;
}
}
void ORCharacterSolver_HIK::EventSuspend( HISender pSender, HKEvent pEvent )
{
if(mAlreadyConnected)
{
mSystem.OnConnectionNotify.Remove( this, (FBCallback)&ORCharacterSolver_HIK::ConnectionNotify);
mAlreadyConnected = false;
}
}
void ORCharacterSolver_HIK::ConnectionNotify(HISender /*pSender*/, HKEvent pEvent)
{
/*
if ( GetObjectStatus(kFBStatusRetrieving) || GetObjectStatus(kFBStatusMerging) )
return;
FBEventConnectionNotify lEvent(pEvent);
FBPlug* lSrcPlug = lEvent.SrcPlug;
FBPlug* lDstPlug = lEvent.DstPlug;
FBPlug* lNewPlug = lEvent.NewPlug;
if( lSrcPlug && lSrcPlug->Is( FBCharacter::TypeInfo ) && TargetCharacter.GetCount() > 0 && TargetCharacter.GetAt( 0 ) == lSrcPlug )
{
if( lEvent.Action == kFBConnected )
{
// Check if every bone already characterized fit with newly attach Character:
if ( CharacterizeExtraShoulder )
{
FBCharacter* lCharacter = (FBCharacter*)(TargetCharacter.GetAt(0));
FBModel* lLeftShoulder = lCharacter->GetModel( kFBLeftCollarNodeId );
FBModel* lLeftArm = lCharacter->GetModel( kFBLeftShoulderNodeId );
FBModel* lLeftArmParent = lLeftArm->Parent;
FBModel* lLeftShoulderExtra = GetExtraBoneModelAt(LEFT_EXTRA_COLLAR);
FBModel* lRightShoulder = lCharacter->GetModel( kFBRightCollarNodeId );
FBModel* lRightArm = lCharacter->GetModel( kFBRightShoulderNodeId );
FBModel* lRightArmParent = lRightArm->Parent;
FBModel* lRightShoulderExtra = GetExtraBoneModelAt(RIGHT_EXTRA_COLLAR);
bool lRecharacterize = lLeftShoulderExtra && lLeftArmParent != lLeftShoulder ||
lRightShoulderExtra && lRightArmParent != lRightShoulder;
if ( lRecharacterize )
{
// Do not force recharacterization if on the same midshoulder
if ( lLeftShoulderExtra != lLeftArmParent ||
lRightShoulderExtra != lRightArmParent )
{
SetExtraBoneModelAt( lLeftArmParent, LEFT_EXTRA_COLLAR );
SetExtraBoneModelAt( lRightArmParent, RIGHT_EXTRA_COLLAR );
CharacterizeExtraShoulder = false;
CharacterizeExtraShoulder = true;
}
}
else
{
// Reset all values:
SetExtraBoneModelAt(NULL, LEFT_EXTRA_COLLAR);
SetExtraBoneModelAt(NULL, RIGHT_EXTRA_COLLAR);
CharacterizeExtraShoulder = false;
}
}
}
}
*/
}
int ORCharacterSolver_HIK::GetExtraFKCount()
{
return 2;
}
FBString gLeftExtraCollar_FK = FBString(HIKNodeNameFromNodeId(LeftCollarExtraNodeId)) + "_FK";
FBString gRightExtraCollar_FK = FBString(HIKNodeNameFromNodeId(RightCollarExtraNodeId)) + "_FK";
const char* ORCharacterSolver_HIK::GetExtraFKNameAt(int pIndex)
{
switch (pIndex)
{
case LEFT_EXTRA_COLLAR:
return gLeftExtraCollar_FK;
break;
case RIGHT_EXTRA_COLLAR:
return gRightExtraCollar_FK;
break;
}
return NULL;
}
FBBodyPartId ORCharacterSolver_HIK::GetExtraFKBodyPartAt(int pIndex)
{
switch (pIndex)
{
case LEFT_EXTRA_COLLAR:
break;
case RIGHT_EXTRA_COLLAR:
break;
}
}
int ORCharacterSolver_HIK::GetExtraBoneCount()
{
return 2;
}
const char* ORCharacterSolver_HIK::GetExtraBoneNameAt(int pIndex)
{
switch (pIndex)
{
case LEFT_EXTRA_COLLAR:
return HIKNodeNameFromNodeId(LeftCollarExtraNodeId);
break;
case RIGHT_EXTRA_COLLAR:
return HIKNodeNameFromNodeId(RightCollarExtraNodeId);
break;
}
return NULL;
}
FBBodyPartId ORCharacterSolver_HIK::GetExtraBoneBodyPartAt(int pIndex)
{
switch (pIndex)
{
case LEFT_EXTRA_COLLAR:
break;
case RIGHT_EXTRA_COLLAR:
break;
}
}
void ORCharacterSolver_HIK::RegisterExtraProperties(HIKCharacterHost<HIKHostNodeMB,HIKHostPropertyMB> &pHIKCharacterHost)
{
// extra HIK 3.6 Properties
pHIKCharacterHost.GetProperty(HIKExtraCollarRatioId).Get().Init(NULL, &ExtraCollarRatio, 0,100.f, false);
pHIKCharacterHost.GetProperty(HIKCollarStiffnessX).Get().Init(NULL, &CollarStiffnessX, 0, 100.f, false);
pHIKCharacterHost.GetProperty(HIKCollarStiffnessY).Get().Init(NULL, &CollarStiffnessY, 0,100.f, false);
pHIKCharacterHost.GetProperty(HIKCollarStiffnessZ).Get().Init(NULL, &CollarStiffnessZ, 0, 100.f, false);
pHIKCharacterHost.GetProperty(HIKReachActorLeftShoulderId).Get().Init(NULL, &ReachLeftShoulder, 0, 100.f, false);
pHIKCharacterHost.GetProperty(HIKReachActorRightShoulderId).Get().Init(NULL, &ReachRightShoulder, 0, 100.f, false);
// HIK 4.0 properties
pHIKCharacterHost.GetProperty(HIKFingerPropagationId).Get().Init(&FingerSolvingPropagation, NULL, 0, 1.f, false);
pHIKCharacterHost.GetProperty(HIKRealisticLeftKneeSolvingId).Get().Init(NULL, &RealisticLeftKneeSolving, 0, 100.f, false);
pHIKCharacterHost.GetProperty(HIKRealisticRightKneeSolvingId).Get().Init(NULL, &RealisticRightKneeSolving, 0, 100.f, false);
// SnS
pHIKCharacterHost.GetProperty(HIKStretchStartArmsAndLegs).Get().Init(NULL, &StretchStartArmsAndLegs,0,100,false);
pHIKCharacterHost.GetProperty(HIKStretchStopArmsAndLegs).Get().Init(NULL, &StretchStopArmsAndLegs,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSScaleArmsAndLegs).Get().Init(NULL, &SnSScaleArmsAndLegs,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSReachLeftWrist).Get().Init(NULL, &SnSReachLeftWrist,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSReachRightWrist).Get().Init(NULL, &SnSReachRightWrist,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSReachLeftAnkle).Get().Init(NULL, &SnSReachLeftAnkle,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSReachRightAnkle).Get().Init(NULL, &SnSReachRightAnkle,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSScaleSpine).Get().Init(NULL, &SnSScaleSpine,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSScaleSpineChildren).Get().Init(NULL, &SnSScaleSpineChildren,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSSpineFreedom).Get().Init(NULL, &SnSSpineFreedom,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSReachChestEnd).Get().Init(NULL, &SnSReachChestEnd,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSScaleNeck).Get().Init(NULL, &SnSScaleNeck,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSNeckFreedom).Get().Init(NULL, &SnSNeckFreedom,0,100,false);
pHIKCharacterHost.GetProperty(HIKSnSReachHead).Get().Init(NULL, &SnSReachHead,0,100,false);
}
void ORCharacterSolver_HIK::ResetExtraProperties()
{
RemoveAnimations();
ResetPropertiesToDefault();
}
void ORCharacterSolver_HIK::ResetPropertiesToDefault()
{
Weight = 100;
ExtraCollarRatio = 50;
CollarStiffnessX = 0;
CollarStiffnessY = 0;
CollarStiffnessZ = 0;
ReachLeftShoulder = 0;
ReachRightShoulder = 0;
FingerSolvingPropagation = false;
RealisticLeftKneeSolving = 0;
RealisticRightKneeSolving = 0;
// SnS
LegSNS = false;
ArmSNS = false;
StretchStartArmsAndLegs = HIKGetPropertyInfoDefaultValue(HIKStretchStartArmsAndLegs) * 100;
StretchStopArmsAndLegs = HIKGetPropertyInfoDefaultValue(HIKStretchStopArmsAndLegs) * 100;
SnSScaleArmsAndLegs = HIKGetPropertyInfoDefaultValue(HIKSnSScaleArmsAndLegs) * 100;
SnSReachLeftWrist = HIKGetPropertyInfoDefaultValue(HIKSnSReachLeftWrist) * 100;
SnSReachRightWrist = HIKGetPropertyInfoDefaultValue(HIKSnSReachRightWrist) * 100;
SnSReachLeftAnkle = HIKGetPropertyInfoDefaultValue(HIKSnSReachLeftAnkle) * 100;
SnSReachRightAnkle = HIKGetPropertyInfoDefaultValue(HIKSnSReachRightAnkle) * 100;
SnSScaleSpine = HIKGetPropertyInfoDefaultValue(HIKSnSScaleSpine) * 100;
SnSScaleSpineChildren = HIKGetPropertyInfoDefaultValue(HIKSnSScaleSpineChildren) * 100;
SnSSpineFreedom = HIKGetPropertyInfoDefaultValue(HIKSnSSpineFreedom) * 100;
SnSReachChestEnd = HIKGetPropertyInfoDefaultValue(HIKSnSReachChestEnd) * 100;
SnSScaleNeck = HIKGetPropertyInfoDefaultValue(HIKSnSScaleNeck) * 100;
SnSNeckFreedom = HIKGetPropertyInfoDefaultValue(HIKSnSNeckFreedom) * 100;
SnSReachHead = HIKGetPropertyInfoDefaultValue(HIKSnSReachHead) * 100;
}
void ORCharacterSolver_HIK::RemoveAnimations()
{
FBUndoManager lUndoManager;
//Clears animation for the properties defined in this class(excludes those defined in the parent classes)
const int lPropertyCount = mSolverPropertiesCount < PropertyList.GetCount() ? mSolverPropertiesCount : PropertyList.GetCount();
for( int lPropID = mInternalPropertiesCount; lPropID < lPropertyCount; ++lPropID )
{
FBProperty* lProp = PropertyList[lPropID];
//Don't touch the ones that are hidden
if( lProp->GetPropertyFlag(kFBPropertyFlagHideProperty) == false )
{
if( lUndoManager.TransactionIsOpen() )
lUndoManager.TransactionAddProperty(lProp);
if( lProp->IsAnimatable() )
{
FBPropertyAnimatable* lEvalProp = (FBPropertyAnimatable*) lProp;
if(lEvalProp)
lEvalProp->SetAnimated(false);
}
}
}
if( lUndoManager.TransactionIsOpen() )
lUndoManager.TransactionAddProperty( &Weight );
Weight.SetAnimated(false);
}
void ORCharacterSolver_HIK::SyncPivot(FBControlSet* pControlSet)
{
if(!pControlSet)
return;
FBEvaluateInfo* lEvalInfo = FBGetDisplayInfo();
HIKEvaluationState* lCurrentState = GetState(lEvalInfo);
for(int lIter = 0; lIter < kFBLastEffectorId; lIter++)
{
for(int lSetIter = 1; lSetIter < FBLastEffectorSetIndex; lSetIter++)
{
FBModelMarker* lPivotEffector = dynamic_cast<FBModelMarker*>(pControlSet->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(lCurrentState->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, lEvalInfo);
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, lEvalInfo);
lPivotEffector->SetVector(lGRVector, kModelRotation, true, false, lEvalInfo);
}
}
}
}
void ORCharacterSolver_HIK::CharacterPasteState( FBCharacter* pFromCharacter, FBCharacterPose* pPose, FBCharacterPoseOptions& pCharacterPoseOptions )
{
if(mHIKControlRigHostEvaluator.mHIKCharacter == 0)
{
//no pose without a control rig
return;
}
// Step#1: Paste pose to temporary HIKCharacter.
HIKEvaluationState* lCurrentState = GetState(FBGetDisplayInfo()); // get evaluation state in the beginning of pasting for once
HIKCharacter* lHIKFromCharacter = NULL;
HIKCharacterState* lFromCharacterState = NULL;
HIKPropertySetState* lFromPropertySetState = NULL;
PastePoseToTempHikCharacter( pFromCharacter, pPose, pCharacterPoseOptions, lCurrentState, // inputs
lHIKFromCharacter, lFromCharacterState, lFromPropertySetState ); // outputs
// Step#2: Calculate match model's offset between current state and pose state, and do real pasting
bool lPasted = false;
FBMatrix lHipsOffsetGX;
if( pCharacterPoseOptions.mModelToMatch )
{
// 1. Calculate TR of match model from current character
FBMatrix lMatchModel_CurrentGRM;
FBVector3d lMatchModel_CurrentGT;
pCharacterPoseOptions.mModelToMatch->GetMatrix(lMatchModel_CurrentGRM, kModelRotation, true);
pCharacterPoseOptions.mModelToMatch->GetVector(lMatchModel_CurrentGT, kModelTranslation, true);
// 2. Calculate TRS of match mode from pose.
FBMatrix lMatchModel_PoseTRS;
int lHIKNodeId = -1;
int lHIKEffectorId = -1;
GetNodeEffectIds( pCharacterPoseOptions.mModelToMatch, lHIKNodeId, lHIKEffectorId, lEffectorId, lPivotIndex );
bool lIsMatchModelExtension = (lHIKNodeId == -1 && lHIKEffectorId == -1);
// 3. R offset pasting
if( pCharacterPoseOptions.GetFlag( kFBCharacterPoseMatchR ) )
{
// a. Calculate lMatchModel_PoseTRS
if( lIsMatchModelExtension )
{
GetTRSForMatchModel_Extension( pPose, pCharacterPoseOptions.mModelToMatch, lMatchModel_PoseTRS );
}
else
{
GetTRSForMatchModel_Body(pCharacterPoseOptions, lHIKFromCharacter, lFromCharacterState, lFromPropertySetState, lCurrentState, &lHipsOffsetGX, lHIKNodeId, lHIKEffectorId, lEffectorId, lPivotIndex, &lMatchModel_PoseTRS);
}
// b. Calculate R offset
GetRotationOffset( lMatchModel_CurrentGRM, lMatchModel_PoseTRS, pCharacterPoseOptions.GetFlag( kFBCharacterPoseGravity ), lHipsOffsetGX );
// c. Do real pasting with R offset
DoPasteCharacter(lHIKFromCharacter, lFromCharacterState, lFromPropertySetState, lCurrentState, pCharacterPoseOptions.mCharacterPoseKeyingMode, &lHipsOffsetGX);
// d. say pasted
lPasted = true;
}
// 4. T offset pasting
if( (pCharacterPoseOptions.GetFlag( kFBCharacterPoseMatchTX ) ||
pCharacterPoseOptions.GetFlag( kFBCharacterPoseMatchTY ) ||
pCharacterPoseOptions.GetFlag( kFBCharacterPoseMatchTZ )))
{
// a. Calculate lMatchModel_PoseTRS
if( lIsMatchModelExtension )
{
GetTRSForMatchModel_Extension( pPose, pCharacterPoseOptions.mModelToMatch, lMatchModel_PoseTRS );
}
else
{
GetTRSForMatchModel_Body(pCharacterPoseOptions, lHIKFromCharacter, lFromCharacterState, lFromPropertySetState, lCurrentState, &lHipsOffsetGX, lHIKNodeId, lHIKEffectorId, lEffectorId, lPivotIndex, &lMatchModel_PoseTRS);
}
// b. Calculate T offset
GetTranslationOffset( lMatchModel_CurrentGT, lMatchModel_PoseTRS,
pCharacterPoseOptions.GetFlag( kFBCharacterPoseMatchTX ),
pCharacterPoseOptions.GetFlag( kFBCharacterPoseMatchTY ),
pCharacterPoseOptions.GetFlag( kFBCharacterPoseMatchTZ ),
pCharacterPoseOptions.GetFlag( kFBCharacterPoseGravity ),
lHipsOffsetGX );
// c. Do real pasting with T offset
DoPasteCharacter(lHIKFromCharacter, lFromCharacterState, lFromPropertySetState, lCurrentState, pCharacterPoseOptions.mCharacterPoseKeyingMode, &lHipsOffsetGX);
// d. say pasted
lPasted = true;
}
}
// Step#3: Check if any pasting has been done, if no, just paste it
if(!lPasted)
{
DoPasteCharacter(lHIKFromCharacter, lFromCharacterState, lFromPropertySetState, lCurrentState, pCharacterPoseOptions.mCharacterPoseKeyingMode, &lHipsOffsetGX);
}
// Step#4: Set candidate on Character and ControlRig
mHIKControlRigHostEvaluator.WriteRigCandidate(FBGetDisplayInfo(),lCurrentState);
mHIKControlRigHostEvaluator.WriteCandidate(FBGetDisplayInfo(),lCurrentState);
DoPasteCharacterExtension(pPose, pCharacterPoseOptions, lCurrentState);
// Step#5: Sync effector pivot
FBCharacter* lToCharacter = TargetCharacter;
SyncPivot(lToCharacter->GetCurrentControlSet());
// Step#6: Cleanup
HIKCharacterDestroy(lHIKFromCharacter,&free);
HIKPropertySetStateDestroy(lFromPropertySetState,&free);
HIKCharacterStateDestroy(lFromCharacterState,&free);
}
//
// The output of this function are: pValue
//
void ORCharacterSolver_HIK::GetTRSForMatchModel_Body(FBCharacterPoseOptions& pCharacterPoseOptions, HIKCharacter* pHIKFromCharacter, HIKCharacterState* pFromCharacterState,
HIKPropertySetState* pFromPropertySetState, HIKEvaluationState* pCurrentState, FBMatrix* pHipsOffsetGX, int pHIKNodeId, int pHIKEffectorId, FBEffectorId pEffectorId, int pPivotIndex, FBMatrix* pValue)
{
if(pCharacterPoseOptions.mModelToMatch)
{
// create lPoseState from mHIKCharacter
HIKCharacterState* lPoseState = HIKCharacterStateCreate(mHIKCharacter,&malloc);
// Copy pFromCharacterState and paste to lPoseState, no relationship with pCurrentState!
HIKPasteState(mHIKCharacter, lPoseState, pCurrentState->mCRState,
pHIKFromCharacter, pFromCharacterState,
pCurrentState->mCRPropertySetState, pFromPropertySetState,
(double*)pHipsOffsetGX );
if(pHIKNodeId != -1)
{
HIKGetNodeStatedv(mHIKCharacter, lPoseState, pHIKNodeId, (double*)pValue);
}
else if(pHIKEffectorId != -1)
{
//keep the old state in backup
HIKCharacterState* lTempCharacterState = HIKCharacterStateCreate(mHIKCharacter,&malloc);
HIKCharacterStateCopy(lTempCharacterState, pCurrentState->mCRState);
//paste lPoseState to pCurrentState
if(pCharacterPoseOptions.mCharacterPoseKeyingMode == kFBCharacterPoseKeyingModeBodyPart)
{
DoPasteForBodyPart(pCurrentState, lPoseState);
}
else
{
HIKCharacterStateCopy(pCurrentState->mCRState, lPoseState);
mHIKControlRigHostEvaluator.EffectorStateFromControlSet(pCurrentState);
}
//get pCurrentState's EffecotrID's transformation
double lValue[16];
HIKGetEffectorStatedv(pCurrentState->mCREffectorSetState, pHIKEffectorId, lValue);
if(pPivotIndex)
{
FBModelMarker* lPivotEffector = dynamic_cast<FBModelMarker*>(TargetCharacter->GetCurrentControlSet()->GetIKEffectorModel(pEffectorId, pPivotIndex));
if(lPivotEffector)
{
FBTVector lTemp;
lPivotEffector->IKPivot.GetData(lTemp, sizeof(lTemp));
lValue[12] -= lTemp.mValue[0];
lValue[13] -= lTemp.mValue[1];
lValue[14] -= lTemp.mValue[2];
}
}
pValue->Set(lValue);
//reset pCurrentState to the temp state
HIKCharacterStateCopy(pCurrentState->mCRState, lTempCharacterState);
mHIKControlRigHostEvaluator.EffectorStateFromControlSet(pCurrentState);
// destroy temp state
HIKCharacterStateDestroy(lTempCharacterState,&free);
}
// destroy lPoseState
HIKCharacterStateDestroy(lPoseState,&free);
}
}
void ORCharacterSolver_HIK::DoPasteCharacter(HIKCharacter* pHIKFromCharacter, HIKCharacterState* pFromCharacterState, HIKPropertySetState* pFromPropertySetState, HIKEvaluationState* pCurrentState, FBCharacterPoseKeyingMode pKeyingMode, FBMatrix* pHipsOffsetGX)
{
// create lPoseState from mHIKCharacter
HIKCharacterState* lPoseState = HIKCharacterStateCreate(mHIKCharacter,&malloc);
// copy pFromCharacterState and paste to lPoseState
HIKPasteState(mHIKCharacter, lPoseState, pCurrentState->mCRState,
pHIKFromCharacter, pFromCharacterState,
pCurrentState->mCRPropertySetState, pFromPropertySetState,
*pHipsOffsetGX );
// copy lPoseState and paste to pCurrentState
// check if we are in body part, if so, copy in local only the necessary bone
{
DoPasteForBodyPart(pCurrentState, lPoseState);
}
else
{
//simply copy the entire state
HIKCharacterStateCopy(pCurrentState->mCRState, lPoseState);
mHIKControlRigHostEvaluator.EffectorStateFromControlSet(pCurrentState);
}
// destroy the lPoseState
HIKCharacterStateDestroy(lPoseState,&free);
}
#ifndef _WIN32
#include <stdlib.h>
static void* _aligned_malloc(size_t pSize, size_t pAlignment)
{
void* lMemory = NULL;
if ( posix_memalign( &lMemory, pAlignment, pSize ) == 0 )
return lMemory;
else
return NULL;
}
#define _aligned_free free
#endif
void ORCharacterSolver_HIK::DoPasteForBodyPart(HIKEvaluationState* pCurrentState, HIKCharacterState* pPoseState)
{
bool lActiveBodyPart[kFBLastCtrlSetPartIndex];
TargetCharacter->GetActiveBodyPart(lActiveBodyPart);
//Get the local character state for the new state
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(mHIKControlRigHostEvaluator.mHIKCharacter, lNodeCounter) )
{
lDescription.mHIKNodeId[lNodeCounter] = lNodeCounter;
}
else
{
lDescription.mHIKNodeId[lNodeCounter] = HIKNotUsed;
}
}
lDescription.mHIKNodeId[LastNodeId] = HIKLastNode;
// get pose state's data set
KHIKNodeState* lPoseDataSet = (KHIKNodeState*)_aligned_malloc( sizeof(KHIKNodeState) * LastNodeId, 16);
HIKGetCharacterStateTransformTQS( mHIKCharacter, pPoseState, &lDescription, lPoseDataSet);
// get current state's data set
KHIKNodeState* lCurrentDataSet = (KHIKNodeState*)_aligned_malloc( sizeof(KHIKNodeState) * LastNodeId, 16);
HIKGetCharacterStateTransformTQS( mHIKCharacter, pCurrentState->mCRState, &lDescription, lCurrentDataSet);
//Transfer the active bone
for(lNodeCounter = 0; lNodeCounter < kFBLastNodeId_Old; lNodeCounter++)
{
if(lActiveBodyPart[FBGetBodyNodeBodyPart((FBBodyNodeId)lNodeCounter)])
{
HIKNodeId lNodeId = gFBBodyNodeToHIKNodeId[lNodeCounter];
//replace the selected body part
lCurrentDataSet[lNodeId].mTfv[0] = lPoseDataSet[lNodeId].mTfv[0];
lCurrentDataSet[lNodeId].mTfv[1] = lPoseDataSet[lNodeId].mTfv[1];
lCurrentDataSet[lNodeId].mTfv[2] = lPoseDataSet[lNodeId].mTfv[2];
lCurrentDataSet[lNodeId].mTfv[3] = lPoseDataSet[lNodeId].mTfv[3];
lCurrentDataSet[lNodeId].mQfv[0] = lPoseDataSet[lNodeId].mQfv[0];
lCurrentDataSet[lNodeId].mQfv[1] = lPoseDataSet[lNodeId].mQfv[1];
lCurrentDataSet[lNodeId].mQfv[2] = lPoseDataSet[lNodeId].mQfv[2];
lCurrentDataSet[lNodeId].mQfv[3] = lPoseDataSet[lNodeId].mQfv[3];
lCurrentDataSet[lNodeId].mSfv[0] = lPoseDataSet[lNodeId].mSfv[0];
lCurrentDataSet[lNodeId].mSfv[1] = lPoseDataSet[lNodeId].mSfv[1];
lCurrentDataSet[lNodeId].mSfv[2] = lPoseDataSet[lNodeId].mSfv[2];
lCurrentDataSet[lNodeId].mSfv[3] = lPoseDataSet[lNodeId].mSfv[3];
}
}
//transfert the extra bones...
if(GetExtraBoneModelAt(LEFT_EXTRA_COLLAR) && lActiveBodyPart[GetExtraBoneBodyPartAt(LEFT_EXTRA_COLLAR)])
{
//replace the selected body part
lCurrentDataSet[LeftCollarExtraNodeId].mTfv[0] = lPoseDataSet[LeftCollarExtraNodeId].mTfv[0];
lCurrentDataSet[LeftCollarExtraNodeId].mTfv[1] = lPoseDataSet[LeftCollarExtraNodeId].mTfv[1];
lCurrentDataSet[LeftCollarExtraNodeId].mTfv[2] = lPoseDataSet[LeftCollarExtraNodeId].mTfv[2];
lCurrentDataSet[LeftCollarExtraNodeId].mTfv[3] = lPoseDataSet[LeftCollarExtraNodeId].mTfv[3];
lCurrentDataSet[LeftCollarExtraNodeId].mQfv[0] = lPoseDataSet[LeftCollarExtraNodeId].mQfv[0];
lCurrentDataSet[LeftCollarExtraNodeId].mQfv[1] = lPoseDataSet[LeftCollarExtraNodeId].mQfv[1];
lCurrentDataSet[LeftCollarExtraNodeId].mQfv[2] = lPoseDataSet[LeftCollarExtraNodeId].mQfv[2];
lCurrentDataSet[LeftCollarExtraNodeId].mQfv[3] = lPoseDataSet[LeftCollarExtraNodeId].mQfv[3];
lCurrentDataSet[LeftCollarExtraNodeId].mSfv[0] = lPoseDataSet[LeftCollarExtraNodeId].mSfv[0];
lCurrentDataSet[LeftCollarExtraNodeId].mSfv[1] = lPoseDataSet[LeftCollarExtraNodeId].mSfv[1];
lCurrentDataSet[LeftCollarExtraNodeId].mSfv[2] = lPoseDataSet[LeftCollarExtraNodeId].mSfv[2];
lCurrentDataSet[LeftCollarExtraNodeId].mSfv[3] = lPoseDataSet[LeftCollarExtraNodeId].mSfv[3];
}
if(GetExtraBoneModelAt(RIGHT_EXTRA_COLLAR) && lActiveBodyPart[GetExtraBoneBodyPartAt(RIGHT_EXTRA_COLLAR)])
{
//replace the selected body part
lCurrentDataSet[RightCollarExtraNodeId].mTfv[0] = lPoseDataSet[RightCollarExtraNodeId].mTfv[0];
lCurrentDataSet[RightCollarExtraNodeId].mTfv[1] = lPoseDataSet[RightCollarExtraNodeId].mTfv[1];
lCurrentDataSet[RightCollarExtraNodeId].mTfv[2] = lPoseDataSet[RightCollarExtraNodeId].mTfv[2];
lCurrentDataSet[RightCollarExtraNodeId].mTfv[3] = lPoseDataSet[RightCollarExtraNodeId].mTfv[3];
lCurrentDataSet[RightCollarExtraNodeId].mQfv[0] = lPoseDataSet[RightCollarExtraNodeId].mQfv[0];
lCurrentDataSet[RightCollarExtraNodeId].mQfv[1] = lPoseDataSet[RightCollarExtraNodeId].mQfv[1];
lCurrentDataSet[RightCollarExtraNodeId].mQfv[2] = lPoseDataSet[RightCollarExtraNodeId].mQfv[2];
lCurrentDataSet[RightCollarExtraNodeId].mQfv[3] = lPoseDataSet[RightCollarExtraNodeId].mQfv[3];
lCurrentDataSet[RightCollarExtraNodeId].mSfv[0] = lPoseDataSet[RightCollarExtraNodeId].mSfv[0];
lCurrentDataSet[RightCollarExtraNodeId].mSfv[1] = lPoseDataSet[RightCollarExtraNodeId].mSfv[1];
lCurrentDataSet[RightCollarExtraNodeId].mSfv[2] = lPoseDataSet[RightCollarExtraNodeId].mSfv[2];
lCurrentDataSet[RightCollarExtraNodeId].mSfv[3] = lPoseDataSet[RightCollarExtraNodeId].mSfv[3];
}
//set the new state
HIKSetCharacterStateTransformTQS(mHIKCharacter, pCurrentState->mCRState, &lDescription, lCurrentDataSet);
HIKEffectorSetFromCharacter(mHIKCharacter, pCurrentState->mCREffectorSetState, pCurrentState->mCRState, pCurrentState->mCRPropertySetState);
mHIKControlRigHostEvaluator.Solve(pCurrentState);
_aligned_free(lPoseDataSet);
_aligned_free(lCurrentDataSet);
}
//
// Paste extension's transform from pPose to TargetCharacter.
//
void ORCharacterSolver_HIK::DoPasteCharacterExtension(FBCharacterPose* pPose, FBCharacterPoseOptions& pCharacterPoseOptions, HIKEvaluationState* pCurrentState)
{
FBEvaluateInfo* lEvaluation = BackgroundEvaluateInfoBegin(FBGetDisplayInfo(), true);
FBCharacter* lToCharacter = TargetCharacter;
int lCharacterExtensionCount = lToCharacter->CharacterExtensions.GetCount();
for(int i = 0; i < lCharacterExtensionCount; i++)
{
FBCharacterExtension* lCharacterExtension = (FBCharacterExtension*)lToCharacter->CharacterExtensions.GetAt(i);
if( pCharacterPoseOptions.mCharacterPoseKeyingMode == kFBCharacterPoseKeyingModeBodyPart )
{
if( lCharacterExtension != NULL && !lCharacterExtension->IsElementSelected() )
continue;
}
FBObjectPose* lCharacterExtPose = NULL;
if(lCharacterExtension != NULL)
{
lCharacterExtPose = pPose->GetCharacterExtensionPose(lCharacterExtension->Label);
}
if(lCharacterExtension && lCharacterExtPose)
{
FBObjectPoseOptions* lObjectPoseOptions = new FBObjectPoseOptions();
//
// Set the transform type
//
if( pCharacterPoseOptions.mCharacterPoseKeyingMode == kFBCharacterPoseKeyingModeFullBody )
{
//
// Full body, paste the local transform relative to the reference.
// If the extension has no reference, it will be relative to the hips.
//
lObjectPoseOptions->mPoseTransformType = kFBPoseTransformLocalRef;
}
else if( pCharacterPoseOptions.mCharacterPoseKeyingMode == kFBCharacterPoseKeyingModeBodyPart )
{
//
// Body Part, paste the local transform relative to the reference if there's a reference or if we mirror.
// Otherwise paste the pure local transform.
//
if( lCharacterExtension->ReferenceModel != 0 || pCharacterPoseOptions.GetFlag( kFBCharacterPoseMirror ) )
{
lObjectPoseOptions->mPoseTransformType = kFBPoseTransformLocalRef;
}
else
{
lObjectPoseOptions->mPoseTransformType = kFBPoseTransformLocal;
}
}
FBModel* lReferenceModel = lCharacterExtension->ReferenceModel != NULL ? lCharacterExtension->ReferenceModel : lToCharacter->GetModel( kFBHipsNodeId );
if(lReferenceModel)
{
lReferenceModel->GetVector(*(FBVector3d*)(&lObjectPoseOptions->mReferenceGT), kModelTranslation, true, lEvaluation);
lReferenceModel->GetMatrix(lObjectPoseOptions->mReferenceGRM, kModelRotation, true, lEvaluation);
lReferenceModel->GetMatrix(lObjectPoseOptions->mReferenceGSM, kModelScaling, true, lEvaluation);
}
else
{
lObjectPoseOptions->mReferenceGT.Init();
lObjectPoseOptions->mReferenceGRM.Identity();
lObjectPoseOptions->mReferenceGSM.Identity();
}
//
// Paste extension
// Mirroring logic follows KCharacterPose::MirrorCharacterExtension
//
if(pCharacterPoseOptions.GetFlag(kFBCharacterPoseMirror) &&
lCharacterExtension->GetMirrorExtension() && // source extension i.e. "mirror partner"
pPose->GetCharacterExtensionPose(lCharacterExtension->GetMirrorExtension()->Label)) // source extension pose
{
FBVector4<double> lMirrorPlaneEquation;
pPose->GetMirrorPlaneEquation(lMirrorPlaneEquation, *lToCharacter, pCharacterPoseOptions);
FBObjectPose lMirrorExtensionPose(FBComponentGetLongName(lCharacterExtPose));
//
// Get source extension i.e. "mirror partner"
//
FBCharacterExtension* lSrcCharacterExtension = lCharacterExtension->GetMirrorExtension();
assert(lSrcCharacterExtension);
//
// Get source extension pose
//
FBObjectPose* lSrcExtensionPose = pPose->GetCharacterExtensionPose(lSrcCharacterExtension->Label);
assert(lSrcExtensionPose);
int lObjectIter = 0;
for( lObjectIter = 0; lObjectIter < lCharacterExtension->GetSubObjectCount(); lObjectIter++ )
{
FBModel* lObject = (FBModel*)(lCharacterExtension->GetSubObject(lObjectIter));
if( lObject )
{
FBString lObjectLabel;
lCharacterExtension->GetLabelNameWithExtensionObject(lObjectLabel,lObject,true);
//
// Retarget the non-transform properties from the SrcPose to the ResultPose.
// ** This will actually copy all the stored properties from the SrcPose.
//
FBObjectPose::RetargetPose
(
lMirrorExtensionPose,
*lSrcExtensionPose,
lObjectLabel,
lObjectLabel
);
//
// Get the stance poses.
//
FBObjectPose* lSrcStancePose = lSrcCharacterExtension->GetStancePose();
FBObjectPose* lDstStancePose = lCharacterExtension->GetStancePose();
if( lSrcStancePose && lDstStancePose )
{
//
// Mirror re-target the transform properties.
//
FBObjectPose::MirrorRetargetPose
(
lMirrorExtensionPose,
*lSrcExtensionPose,
*lDstStancePose,
*lSrcStancePose,
lMirrorPlaneEquation,
lObjectLabel,
lObjectLabel
);
}
lMirrorExtensionPose.PasteTransform( (char*)lObjectLabel, *lObject, *lObjectPoseOptions, lEvaluation );
}
}
}
else
{
int lObjectIter = 0;
for( lObjectIter = 0; lObjectIter < lCharacterExtension->GetSubObjectCount(); lObjectIter++ )
{
FBModel* lObject = (FBModel*)(lCharacterExtension->GetSubObject(lObjectIter));
if( lObject )
{
FBString lObjectLabel;
lCharacterExtension->GetLabelNameWithExtensionObject(lObjectLabel,lObject,true);
lCharacterExtPose->PasteTransform( (char*)lObjectLabel, *lObject, *lObjectPoseOptions, lEvaluation );
}
}
}
}
}
BackgroundEvaluateInfoEnd(lEvaluation);
}
//
// get TRS for match model from pPose directly, when match mode is a extension.
//
void ORCharacterSolver_HIK::GetTRSForMatchModel_Extension( FBCharacterPose* pPose, FBModel* pMatchModel, FBMatrix& pExtensionTRS )
{
// get T/R/S
FBTVector lExtensionT;
FBMatrix lExtensionRM;
FBMatrix lExtensionSM;
for( int i = 0; i < pPose->GetCharacterExtensionPoseCount(); i++ )
{
FBObjectPose* lFBExtensionPose = pPose->GetCharacterExtensionPoseAt( i );
if( lFBExtensionPose->IsTransformStored( pMatchModel->Name.AsString() ) )
{
lFBExtensionPose->GetTransform( lExtensionT, lExtensionRM, lExtensionSM, (pMatchModel->Name).AsString(), kFBPoseTransformGlobal );
// for testing
// FBString lExtensionName = pPose->GetCharacterExtensionNameFromPose(*lFBExtensionPose);
break;
}
}
// convert T/R/S to TRS
FBRVector lRVector;
FBSVector lSVector;
FBMatrixToRotation( lRVector, lExtensionRM );
FBMatrixToScaling( lSVector, lExtensionSM );
FBTRSToMatrix( pExtensionTRS, lExtensionT, lRVector, lSVector );
}
//
// Get match model's offset between current global rotation matrix and pose global rotation matrix.
//
void ORCharacterSolver_HIK::GetRotationOffset( const FBMatrix& pMatchModel_CurrentGRM, const FBMatrix& pMatchMode_PoseTRSM, const bool pGravity, FBMatrix& pHipsOffsetGX )
{
// for testing
// FBRVector lTesting;
// FBMatrixToRotation( lTesting, pMatchMode_PoseTRSM );
FBMatrix lOffsetRM;
FBMatrixInverse( lOffsetRM, pMatchMode_PoseTRSM );
FBMatrixMult( lOffsetRM, pMatchModel_CurrentGRM, lOffsetRM );
if( pGravity )
{
FBTVector lFloorUp( 0.0, 1.0, 0.0, 0.0 );
FBMatrix lRM;
FBTVector lTempVector = (FBTVector &)lOffsetRM(1,0);
FBMult((FBTVector &)lQ,lTempVector,lFloorUp);
lQ[3] = FBDot(lTempVector,lFloorUp) + FBLength(lTempVector) * FBLength(lFloorUp);
FBRVector lTempRVector;
FBQuaternionToRotation( lTempRVector, lQ );
FBRotationToMatrix( lRM, lTempRVector );
FBMatrixMult( lOffsetRM, lRM, lOffsetRM );
}
FBRVector lTempRVector;
FBMatrixToRotation(lTempRVector, lOffsetRM);
FBRotationToMatrix(pHipsOffsetGX, lTempRVector);
}
//
// Get match model's offset between current global translation matrix and pose global translation matrix.
//
void ORCharacterSolver_HIK::GetTranslationOffset( const FBVector3d& pMatchModel_CurrentGT, const FBMatrix& pMatchMode_PoseTRSM, bool pMatchX, bool pMatchY, bool pMatchZ, bool pGravity, FBMatrix& pHipsOffsetGX )
{
//
// Compute the offset.
//
FBTVector lOffsetT;
FBTVector lGTTVector1;
lGTTVector1.mValue[0] = pMatchModel_CurrentGT.mValue[0];
lGTTVector1.mValue[1] = pMatchModel_CurrentGT.mValue[1];
lGTTVector1.mValue[2] = pMatchModel_CurrentGT.mValue[2];
FBTVector lPastedVector;
FBMatrixToTranslation(lPastedVector, pMatchMode_PoseTRSM);
FBSub( lOffsetT, lGTTVector1, lPastedVector );
// Keep only the component(s) we should match
if( !pMatchX )
{
lOffsetT[0] = 0.0;
}
if( !pMatchY || pGravity )
{
lOffsetT[1] = 0.0;
}
if( !pMatchZ )
{
lOffsetT[2] = 0.0;
}
pHipsOffsetGX(3,0) = lOffsetT[0];
pHipsOffsetGX(3,1) = lOffsetT[1];
pHipsOffsetGX(3,2) = lOffsetT[2];
pHipsOffsetGX(3,3) = lOffsetT[3];
}
//
// Get match model's node OR effector ids
//
void ORCharacterSolver_HIK::GetNodeEffectIds( FBModel* pModelToMatch, int& pHIKNodeId, int& pHIKEffectorId, FBEffectorId& pEffectorId, FBEffectorSetID& pPivotIndex )
{
//determine if the matching model is a bone or an effector
int lNodeId = TargetCharacter->GetIndexByModel( pModelToMatch);
if(lNodeId == kFBInvalidNodeId)
{
//check for extra bone
if(GetExtraBoneModelAt(LEFT_EXTRA_COLLAR) == pModelToMatch)
{
lNodeId = LeftCollarExtraNodeId;
}
else if(GetExtraBoneModelAt(RIGHT_EXTRA_COLLAR) == pModelToMatch)
{
lNodeId = RightCollarExtraNodeId;
}
}
if(lNodeId == kFBInvalidNodeId)
{
//check for effector...
for(int i = 0; i < kFBLastEffectorId; i++)
{
for(int j = 0; j < FBLastEffectorSetIndex; j++)
{
if(TargetCharacter->GetEffectorModel((FBEffectorId)i, (FBEffectorSetID)j) == pModelToMatch)
{
pEffectorId = (FBEffectorId)i;
pPivotIndex = (FBEffectorSetID)j;
break;
}
}
}
}
//convert the id to hik
if(lNodeId != kFBInvalidNodeId)
{
if(lNodeId >= kFBLastNodeId_Old)
{
pHIKNodeId = lNodeId; //extra bone
}
else
{
pHIKNodeId = HIKNodeIdFromNodeName( FBCharacterBodyNodeNameFromId( (FBBodyNodeId)lNodeId ) );
}
}
if(pEffectorId != kFBInvalidEffectorId)
{
pHIKEffectorId = HIKEffectorIdFromEffectorName( FBCharacterEffectorNameFromId( pEffectorId ) );
}
}
//
// Paste pose to temporary hik character, which is a media to be used to get the match model's transformation from pose.
// inputs: pFromCharacter, pPose, pCharacterPoseOptions
// outputs: pHIKFromCharacter, pFromCharacterStete, pFromPropertySetState
//
void ORCharacterSolver_HIK::PastePoseToTempHikCharacter( FBCharacter* pFromCharacter, FBCharacterPose* pPose, FBCharacterPoseOptions& pCharacterPoseOptions, HIKEvaluationState* pCurrentState,
HIKCharacter*& pHIKFromCharacter, HIKCharacterState*& pFromCharacterState, HIKPropertySetState*& pFromPropertySetState )
{
//Get the destination character
FBCharacter* lToCharacter = TargetCharacter;
//undo support
UndoSetup(lToCharacter);
HIKCharacterHost<HIKHostNodeMB,HIKHostPropertyMB> lHIKFromCharacterHost;
HIKHostPropertiesInit(lHIKFromCharacterHost);
HIKCharacterHostFromMBCharacterHierarchy(lHIKFromCharacterHost, pFromCharacter, NULL);
//Add the 2 new clavicle bones, if they are available
if(pPose->GetExtraBoneCount() >= 2)
{
FBModel *lLeftShoulderExtra = pPose->GetExtraBoneModelAt(0);
FBModel *lRightShoulderExtra = pPose->GetExtraBoneModelAt(1);
if (lLeftShoulderExtra || lRightShoulderExtra)
{
HIKCharacterHostFromHIKHostMB(lHIKFromCharacterHost, lLeftShoulderExtra, lRightShoulderExtra, NULL);
}
}
pHIKFromCharacter = lHIKFromCharacterHost.CharacterCreate(&malloc);
pFromCharacterState = HIKCharacterStateCreate(pHIKFromCharacter,&malloc);
HIKCharacterHostFromMBCharacterGeometry(lHIKFromCharacterHost,pHIKFromCharacter,pFromCharacter);
//Add the 2 new clavicle bones, if they are available
if(pPose->GetExtraBoneCount() >= 2)
{
FBVector3d lLeftT, lRightT;
FBRVector lLeftR, lRightR, lLeftRPOff, lRightRPOff;
FBSVector lLeftS, lRightS;
pPose->GetExtraBoneTransformOffset(lLeftT, lLeftR, lLeftS, LEFT_EXTRA_COLLAR);
pPose->GetExtraBoneParentRotationOffset(lLeftRPOff, LEFT_EXTRA_COLLAR);
pPose->GetExtraBoneTransformOffset(lRightT, lRightR, lRightS, RIGHT_EXTRA_COLLAR);
pPose->GetExtraBoneParentRotationOffset(lRightRPOff, RIGHT_EXTRA_COLLAR);
FBQuaternion lLeftQ, lRightQ;
FBQuaternion lLeftQPOff, lRightQPOff;
FBRotationToQuaternion(lLeftQ, lLeftR);
FBRotationToQuaternion(lRightQ, lRightR);
FBRotationToQuaternion(lLeftQPOff, lLeftRPOff);
FBRotationToQuaternion(lRightQPOff, lRightRPOff);
CharacterizeHIKCharacterHostFromHIKHostMB(lHIKFromCharacterHost,pHIKFromCharacter,
lLeftT, lLeftQ, lLeftS, lLeftQPOff,
lRightT, lRightQ, lRightS, lRightQPOff);
FBModel *lLeftShoulderExtra = pPose->GetExtraBoneModelAt(0);
FBModel *lRightShoulderExtra = pPose->GetExtraBoneModelAt(1);
FBVector3d lTranslation;
FBVector3d lRotation;
FBVector3d lScale;
FBMatrix lMatrix;
if(lLeftShoulderExtra)
{
pPose->GetExtraBoneTransform(lTranslation, lRotation, lScale, LEFT_EXTRA_COLLAR);
FBTVector lTVector( lTranslation );
FBSVector lSVector( lScale );
FBTRSToMatrix(lMatrix, lTVector, lRotation, lSVector);
HIKSetNodeStatedv(pHIKFromCharacter, pFromCharacterState, LeftCollarExtraNodeId, lMatrix);
}
if(lRightShoulderExtra)
{
pPose->GetExtraBoneTransform(lTranslation, lRotation, lScale, RIGHT_EXTRA_COLLAR);
FBTVector lTVector( lTranslation );
FBSVector lSVector( lScale );
FBTRSToMatrix(lMatrix, lTVector, lRotation, lSVector);
HIKSetNodeStatedv(pHIKFromCharacter, pFromCharacterState, RightCollarExtraNodeId, lMatrix);
}
}
int iter = 0;
for(iter = 0; iter < kFBLastNodeId_Old; iter++)
{
int lHIKId = HIKNodeIdFromNodeName( FBCharacterBodyNodeNameFromId((FBBodyNodeId)iter) );
FBMatrix lMatrix = pPose->GetNodeMatrixGlobal(iter);
HIKSetNodeStatedv(pHIKFromCharacter, pFromCharacterState, lHIKId, (double*)lMatrix);
}
HIKCharacterizeGeometry(pHIKFromCharacter);
HIKHostPropertiesFromCharacter(lHIKFromCharacterHost, pFromCharacter);
RegisterExtraProperties(lHIKFromCharacterHost);
//Read the properties
pFromPropertySetState = HIKPropertySetStateCreate(&malloc);
lHIKFromCharacterHost.ReadPropertySetState(pFromPropertySetState, FBGetDisplayInfo());
//Apply the mirror if necessary
if(pCharacterPoseOptions.GetFlag(kFBCharacterPoseMirror))
{
FBMatrix lMirrorPlaneMatrix;
pPose->GetMirrorPlaneEquation(lMirrorPlaneMatrix, *lToCharacter, pCharacterPoseOptions);
FBQuaternion lMirrorPlaneEquationQ;
FBRVector lPlaneRotationVector;
FBMatrixToRotation(lPlaneRotationVector, lMirrorPlaneMatrix);
FBRotationToQuaternion(lMirrorPlaneEquationQ, lPlaneRotationVector);
HIKMirrorState( pHIKFromCharacter, pFromCharacterState, pFromCharacterState, pHIKFromCharacter, lMirrorPlaneEquationQ.mValue);
}
for(int lIter = kFBHipsEffectorId; lIter < kFBLastEffectorId; lIter++)
{
int lHIKId = HIKEffectorIdFromEffectorName( FBCharacterEffectorNameFromId( (FBEffectorId)lIter ) );
if (lHIKId != -1)
{
HIKSetTranslationActive(pCurrentState->mCREffectorSetState, lIter, 0.0);
HIKSetPull( pCurrentState->mCREffectorSetState, lIter, 0.0 );
}
}
}
/*
// Leave it here for testing purpose
#include <windows.h>
void ORCharacterSolver_HIK::PrintTargetCharacter( const char* pName )
{
char lString[300];
sprintf_s(lString, "\n%s: \n", pName );
OutputDebugString( lString );
FBCharacter* lToCharacter = TargetCharacter;
FBModel* lHips = lToCharacter->GetModel(kFBHipsNodeId);
FBVector3d lGTv;
lHips->GetVector(lGTv, kModelTranslation, true, FBGetDisplayInfo());
sprintf_s(lString, "Hip Translation: (%.2f, %.2f, %.2f) ", lGTv[0], lGTv[1], lGTv[2] );
OutputDebugString(lString);
FBVector3d lGRv;
lHips->GetVector(lGRv, kModelRotation, true, FBGetDisplayInfo());
sprintf_s(lString, "Hip Rotation: (%.2f, %.2f, %.2f) ", lGRv[0], lGRv[1], lGRv[2] );
OutputDebugString(lString);
FBVector3d lGSv;
lHips->GetVector(lGSv, kModelRotation, true, FBGetDisplayInfo());
sprintf_s(lString, "Hip Scale: (%.2f, %.2f, %.2f) ", lGSv[0], lGSv[1], lGSv[2] );
OutputDebugString(lString);
}
void ORCharacterSolver_HIK::PrintState( const char* pName, HIKCharacter* pCharacter, HIKCharacterState* pState )
{
// 2. Current state
if( pState != NULL )
{
char lString[300];
sprintf_s(lString, "\n%s: \n", pName );
OutputDebugString( lString );
OutputDebugString( "FK: " );
PrintDebugFKTRS( pCharacter, pState, HipsNodeId );
//OutputDebugString( "Skeleton: " );
//PrintDebugSkeletonTRS( pCharacter, pState, HipsNodeId );
}
}
*/