constraints/CharacterSolver/HIK2014Solver/hikhost.h

constraints/CharacterSolver/HIK2014Solver/hikhost.h
/***************************************************************************************
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.
***************************************************************************************/
#ifndef __HIKHOST__H_
#define __HIKHOST__H_
#include <humanik/humanik.h>
#include <humanik/hikproperty.h>
#include <humanik/hikutils.h>
// HIK Key
#include <autodeskmwkey.h>
#include "hikdebug.h"
#include "filterset2fbcharacter.h"
template <bool x> struct KStaticAssertionFailure;
template <> struct KStaticAssertionFailure<true> { enum { value = 1 }; };
template<int x> struct KStaticAssertionTest {};
#define K_STATIC_ASSERT( B ) \
typedef ::KStaticAssertionTest<sizeof( KStaticAssertionFailure< (bool)( B ) > )>\
KStaticAssertionTypedef##__LINE__
#ifdef _DEBUG
#define OR_HIK_ASSERT( pCondition ) assert( pCondition );
#else
#define OR_HIK_ASSERT( pCondition );
#endif
#ifndef _WIN32
#ifdef __MACH__
#define _aligned_malloc(size, alignment) malloc(size) // No alignement is necessary on OSX, it is automatically 16 bytes.
#else
#define _aligned_malloc(size, alignment) memalign(alignment, size)
#endif
#define _aligned_free(p) free(p)
#endif
const int HIKLastEffectorSetId = 15;
// forward declarations defined in hikhostmb.h
// hikhostmb.h includes this file so we cannot create a circular dependency...
void HIKEffectorStateCopy(HIKEffectorSetState* pDst, const HIKEffectorSetState* pSrc);
HIKNodeId FBBodyNodeIdToHIKNodeId(FBBodyNodeId pBodyNode);
HIKNodeId ExtraIndexToHIKNodeId(int pExtraIndex);
class KHIKNodeState
{
#if defined(_MSC_VER)
typedef float __declspec(align(16)) FloatArray[4];
#elif defined(__GNUC__)
typedef float __attribute__ ((aligned(16))) FloatArray[4];
#endif
public:
FloatArray mTfv;
FloatArray mQfv;
FloatArray mSfv;
};
enum{ eNoRecursion = 0, eRecursiveT = 1 << 0, eRecursiveR = 1 << 1, eRecursiveS = 1 << 2 };
struct HIKEvaluationState
{
HIKEvaluationState()
{
//src/temp character part
mSrcState = NULL;
mSrcEffectorSetState = NULL;
mSrcPropertySetState = NULL;
//regular solve part
mState = NULL;
mEffectorSetState = NULL;
mPropertySetState = NULL;
//for special computation
mDataSet = NULL;
//manipulator part
mDirtyFlag = true;
//data cache for evaluation id
mEvaluateId = -1;
}
virtual ~HIKEvaluationState()
{
Clear(&free);
}
void Init(HIKCharacter *pHIKCharacterDst, HIKCharacter *pHIKCharacterSrc, HIKMalloc pMalloc)
{
mSrcState = (pHIKCharacterSrc) ? HIKCharacterStateCreate(pHIKCharacterSrc,pMalloc) : NULL;
mSrcEffectorSetState = HIKEffectorSetStateCreate(pMalloc);
mSrcPropertySetState = HIKPropertySetStateCreate(pMalloc);
mState = HIKCharacterStateCreate(pHIKCharacterDst,pMalloc);
mEffectorSetState = HIKEffectorSetStateCreate(pMalloc);
mPropertySetState = HIKPropertySetStateCreate(pMalloc);
mDataSet = (KHIKNodeState*)_aligned_malloc( sizeof(KHIKNodeState) * LastNodeId, 16);
}
void Clear(HIKFree pFree)
{
if(mSrcState != 0)
{
HIKCharacterStateDestroy(mSrcState,pFree);
mSrcState = 0;
}
if(mSrcEffectorSetState != 0)
{
HIKEffectorSetStateDestroy(mSrcEffectorSetState,pFree);
mSrcEffectorSetState = 0;
}
if(mSrcPropertySetState != 0)
{
HIKPropertySetStateDestroy(mSrcPropertySetState,pFree);
mSrcPropertySetState = 0;
}
if(mState != 0)
{
HIKCharacterStateDestroy(mState,pFree);
mState = 0;
}
if(mEffectorSetState != 0)
{
HIKEffectorSetStateDestroy(mEffectorSetState,pFree);
mEffectorSetState = 0;
}
if(mPropertySetState != 0)
{
HIKPropertySetStateDestroy(mPropertySetState,pFree);
mPropertySetState = 0;
}
if(mDataSet != 0)
{
_aligned_free(mDataSet);
mDataSet = 0;
}
mEvaluateId = -1;
mDirtyFlag = true;
}
//src/temp character part
HIKCharacterState* mSrcState;
HIKEffectorSetState* mSrcEffectorSetState;
HIKPropertySetState* mSrcPropertySetState;
//regular solve part
HIKEffectorSetState* mEffectorSetState;
HIKPropertySetState* mPropertySetState;
//data set
KHIKNodeState* mDataSet;
//last evaluation id - used to cache solve and prevent resolve on recursive call.
int mEvaluateId;
//used by manipulator to write sync character
bool mDirtyFlag;
//we use this in double evaluation to identify which connector is being read
//(should be set before query, no default value nor reseting available)
int mEvaluatedEffectorId;
//in double solve mode, we detect recursive effectors on first solve and store them for easy access on second solve.
int mRecursiveFlag[LastEffectorId];
};
class MultiState
{
HIKEvaluationState** mMultiState;
int mStateCount;
public:
MultiState()
{
mMultiState = NULL;
mStateCount = 0;
}
virtual ~MultiState()
{
ClearStates();
}
void ClearStates()
{
//not locked, but should not be needed.
for (int Count=0; Count<mStateCount; Count++)
{
K_SAFE_DELETE_PTR(mMultiState[Count]);
}
mStateCount = 0;
K_SAFE_FREE_PTR(mMultiState);
}
HIKEvaluationState* GetState(FBEvaluateInfo* pEvaluateInfo, FBBox* pCaller)
{
int lIndex = pEvaluateInfo->GetBufferID(pCaller);
return GetState(lIndex);
}
HIKEvaluationState* GetState(int pIndex)
{
if(pIndex >= mStateCount)
{
static FBFastLock sLock;
sLock.Lock();
{
if(pIndex >= mStateCount)
{
int NewCount = pIndex + 1;
mMultiState = (HIKEvaluationState**)realloc( mMultiState,NewCount*sizeof(HIKEvaluationState*) );
for (int Count=mStateCount; Count<NewCount; Count++)
{
mMultiState[Count] = EvaluationStateCreator();
}
mStateCount = NewCount;
}
}
sLock.Unlock();
}
return mMultiState[pIndex];
}
virtual HIKEvaluationState* EvaluationStateCreator() = 0;
};
template<typename HostNode> class HIKHostNode;
template<typename HostProperty> class HIKHostProperty;
template<typename HostNode> class HIKHost;
template<typename HostNode,typename HostProperty> class HIKCharacterHost;
template<typename HostNode> class HIKControlRigHost;
template<typename HostNode,typename HostProperty> class HIKControlRigHostEvaluator;
template<typename HostNode,typename HostProperty> class HIKCharacterHostEvaluator;
template<typename HostNode> class HIKHostNode
{
HostNode mNode;
public:
HostNode& Get() { return mNode; }
bool Valid();
void ReadXForm(double *pXFrom, FBEvaluateInfo* pEvalInfo);
void WriteXForm(double *pXFrom, FBEvaluateInfo* pEvalInfo);
void WriteXFormCandidate(double *pXFrom, FBEvaluateInfo* pEvalInfo);
void ReadTQS(FBTVector &pT, FBQuaternion &pQ, FBVector4d &pS, FBEvaluateInfo* pUserData);
void ReadTQS(FBTVector *pT, FBQuaternion *pQ, FBVector4d *pS, FBEvaluateInfo* pUserData);
void WriteTQS(FBTVector &pT, FBQuaternion &pQ, FBVector4d &pS, FBEvaluateInfo* pEvalInfo);
void WriteTQSIfNotRecursive(FBTVector &pT, FBQuaternion &pQ, FBVector4d &pS, FBEvaluateInfo* pEvalInfo, int &pRecursivityInfo);
void WriteCandidateTQS(const FBTVector &pT, const FBQuaternion &pQ, const FBVector4d &pS, FBEvaluateInfo* pEvalInfo);
void SetCandidateTQS(FBTVector &pT, FBQuaternion &pQ, FBVector4d &pS, FBEvaluateInfo* pEvalInfo);
int IsEvaluationRecursiveTQS(FBEvaluateInfo* pEvalInfo);
double ReadReachT( FBEvaluateInfo* pEvalInfo );
double ReadReachR( FBEvaluateInfo* pEvalInfo );
void ReadIKPivot(double *pIkPivot, FBEvaluateInfo* pEvalInfo);
bool GetIKSync();
void GetVector(FBVector3d &retVector,FBModelTransformationType pWhat,bool pGlobalInfo, FBEvaluateInfo* pEvalInfo);
bool GetUseTranslationLimits();
bool GetUseRotationLimits();
int GetRotationOrder();
void GetPreQ(double *pPreQ);
void GetPostQ(double *pPostQ);
int GetRotationMinXYZ(double *pXYZ);
int GetRotationMaxXYZ(double *pXYZ);
int GetTranslationMinXYZ(double *pXYZ);
int GetTranslationMaxXYZ(double *pXYZ);
};
template<typename HostProperty> class HIKHostProperty
{
HostProperty mProperty;
public:
HostProperty& Get() { return mProperty; }
int ReadMode( FBEvaluateInfo* pEvalInfo );
double ReadValue( FBEvaluateInfo* pEvalInfo );
};
template<typename HostNode> class HIKHost
{
HIKHostNode<HostNode> mNodeArray[LastNodeId];
public:
HIKHostNode<HostNode>& GetNode(int pNodeId) { return mNodeArray[pNodeId]; }
void ReadState(HIKCharacter *pHIKCharacter, HIKCharacterState *pState, FBEvaluateInfo* pEvalInfo, bool pNormalized)
{
for(int lNodeIter = 0; lNodeIter < LastNodeId; lNodeIter++)
{
if(mNodeArray[lNodeIter].Valid())
{
mNodeArray[lNodeIter].ReadTQS(lT, lQ, lS, pEvalInfo);
if(pNormalized)
{
HIKSetNodeNormalizedStateTQSdv(pHIKCharacter, pState,lNodeIter,lT.mValue, lQ.mValue, lS.mValue);
}
else
{
HIKSetNodeStateTQSdv(pHIKCharacter, pState,lNodeIter,lT.mValue, lQ.mValue, lS.mValue);
}
}
}
}
void WriteState(HIKCharacter *pHIKCharacter, HIKCharacterState *pState, FBEvaluateInfo* pEvalInfo, bool pNormalized)
{
// we don't write the reference, we want to be able to offset the Animation
for(int lNodeIter = HipsNodeId; lNodeIter < LastNodeId; lNodeIter++)
{
if(mNodeArray[lNodeIter].Valid())
{
if(pNormalized)
{
HIKGetNodeNormalizedStateTQSdv(pHIKCharacter, pState,lNodeIter,lT.mValue, lQ.mValue, lS.mValue);
}
else
{
HIKGetNodeStateTQSdv(pHIKCharacter, pState,lNodeIter,lT.mValue, lQ.mValue, lS.mValue);
}
mNodeArray[lNodeIter].WriteTQS(lT, lQ, lS,pEvalInfo);
}
}
}
void WriteStateCandidate(HIKCharacter *pHIKCharacter, HIKCharacterState *pState, FBEvaluateInfo* pEvalInfo, bool pNormalized)
{
// we don't write the reference, we want to be able to offset the Animation
for(int lNodeIter = HipsNodeId; lNodeIter < LastNodeId; lNodeIter++)
{
if(mNodeArray[lNodeIter].Valid())
{
if(pNormalized)
{
HIKGetNodeNormalizedStateTQSdv(pHIKCharacter, pState,lNodeIter,lT.mValue, lQ.mValue, lS.mValue);
}
else
{
HIKGetNodeStateTQSdv(pHIKCharacter, pState,lNodeIter,lT.mValue, lQ.mValue, lS.mValue);
}
mNodeArray[lNodeIter].WriteCandidateTQS(lT, lQ, lS,pEvalInfo);
}
}
}
void SetStateCandidate(HIKCharacter *pHIKCharacter, HIKCharacterState *pState, FBEvaluateInfo* pEvalInfo, bool pNormalized)
{
// we don't write the reference, we want to be able to offset the Animation
for(int lNodeIter = HipsNodeId; lNodeIter < LastNodeId; lNodeIter++)
{
if(mNodeArray[lNodeIter].Valid())
{
if(pNormalized)
{
HIKGetNodeNormalizedStateTQSdv(pHIKCharacter, pState,lNodeIter,lT.mValue, lQ.mValue, lS.mValue);
}
else
{
HIKGetNodeStateTQSdv(pHIKCharacter, pState,lNodeIter,lT.mValue, lQ.mValue, lS.mValue);
}
mNodeArray[lNodeIter].SetCandidateTQS(lT, lQ, lS,pEvalInfo);
}
}
}
};
template<typename HostNode,typename HostProperty> class HIKCharacterHost : public HIKHost<HostNode>
{
HIKHostProperty<HostProperty> mPropertyArray[HIKLastPropertyId];
HIKHostProperty<HostProperty> mSpineMinLength[10];
HIKHostProperty<HostProperty> mSpineMaxLength[10];
HIKHostProperty<HostProperty> mNeckMinLength[10];
HIKHostProperty<HostProperty> mNeckMaxLength[10];
HIKHostProperty<HostProperty> mHeadMinLength;
HIKHostProperty<HostProperty> mHeadMaxLength;
HIKHostNode<HostNode> mFloorArray[HIKLastFloorId];
public:
HIKHostProperty<HostProperty>& GetProperty(int pPropertyId) { return mPropertyArray[pPropertyId]; }
HIKHostNode<HostNode>& GetFloorNode(int pFloorId) { return mFloorArray[pFloorId]; }
HIKHostProperty<HostProperty>& GetSpineMinLength(int spineId) { return mSpineMinLength[spineId]; }
HIKHostProperty<HostProperty>& GetSpineMaxLength(int spineId) { return mSpineMaxLength[spineId]; }
HIKHostProperty<HostProperty>& GetNeckMinLength(int neckId) { return mNeckMinLength[neckId]; }
HIKHostProperty<HostProperty>& GetNeckMaxLength(int neckId) { return mNeckMaxLength[neckId]; }
HIKHostProperty<HostProperty>& GetHeadMinLength() { return mHeadMinLength; }
HIKHostProperty<HostProperty>& GetHeadMaxLength() { return mHeadMaxLength; }
bool IsRollPropertyToInverse(int pPropertyIndex)
{
// based on FbxCharacter::SetValuesFromLegacyLoad() and the fact that all RollEx are reversed
if( pPropertyIndex == HIKLeftLegRollId || pPropertyIndex == HIKRightLegRollId ||
pPropertyIndex == HIKLeftForeArmRollId || pPropertyIndex == HIKRightForeArmRollId ||
pPropertyIndex == HIKLeftUpLegRollExId || pPropertyIndex == HIKRightUpLegRollExId ||
pPropertyIndex == HIKLeftLegRollExId || pPropertyIndex == HIKRightLegRollExId ||
pPropertyIndex == HIKLeftArmRollExId || pPropertyIndex == HIKRightArmRollExId ||
pPropertyIndex == HIKLeftForeArmRollExId || pPropertyIndex == HIKRightForeArmRollExId )
{
return true;
}
return false;
}
void ReadPropertySetState(HIKPropertySetState *pPropertyState, FBEvaluateInfo* pEvalInfo)
{
for(int lPropertyIter = 0; lPropertyIter < HIKLastPropertyId; lPropertyIter++)
{
// read property mode
int pMode = mPropertyArray[lPropertyIter].ReadMode(pEvalInfo);
HIKSetPropertyMode(pPropertyState,lPropertyIter,pMode);
// if not auto, read the value
if( HIKIsPropertyAuto(pPropertyState, lPropertyIter) == false )
{
float pValue = float(mPropertyArray[lPropertyIter].ReadValue(pEvalInfo));
//inverse the roll value
if(IsRollPropertyToInverse(lPropertyIter))
{
pValue = 1. - pValue;
}
HIKSetPropertyValue(pPropertyState, lPropertyIter, pValue);
}
}
}
void ReadLengthLimits(HIKCharacter *pCharcter, FBEvaluateInfo* pEvalInfo)
{
for(int i = 0; i < 10; i++)
{
float minL = GetSpineMinLength(i).ReadValue(pEvalInfo);
float maxL = GetSpineMaxLength(i).ReadValue(pEvalInfo);
int nodeid = WaistNodeId;
if(i != 0) nodeid = Spine1NodeId + (i-1);
if(HIKGetNodeUse(pCharcter, nodeid))
{
HIKSetTranslationLimitsf(pCharcter, nodeid, minL, maxL,true);
}
minL = GetNeckMinLength(i).ReadValue(pEvalInfo);
maxL = GetNeckMaxLength(i).ReadValue(pEvalInfo);
nodeid = NeckNodeId;
if(i != 0) nodeid = Neck1NodeId + (i-1);
if(HIKGetNodeUse(pCharcter, nodeid))
{
HIKSetTranslationLimitsf(pCharcter, nodeid, minL, maxL,true);
}
}
float minL = GetHeadMinLength().ReadValue(pEvalInfo);
float maxL = GetHeadMaxLength().ReadValue(pEvalInfo);
HIKSetTranslationLimitsf(pCharcter, HeadNodeId, minL, maxL,true); //Head is always there
}
void TransfertRotationsAndLimits(HIKCharacter *pHIKCharacter, int pNodeIter, bool updatePrePost = false)
{
if(this->GetNode(pNodeIter).Valid())
{
if(this->GetNode(pNodeIter).GetUseRotationLimits())
{
double lPreQ[4];
double lPostQ[4];
double lMaxXYZ[4];
double lMinXYZ[4];
int lOrder = this->GetNode(pNodeIter).GetRotationOrder();
int lMinActiveMask = this->GetNode(pNodeIter).GetRotationMinXYZ(lMinXYZ);
int lMaxActiveMask = this->GetNode(pNodeIter).GetRotationMaxXYZ(lMaxXYZ);
this->GetNode(pNodeIter).GetPreQ(lPreQ);
this->GetNode(pNodeIter).GetPostQ(lPostQ);
HIKSetMinXYZRotationdv(pHIKCharacter,pNodeIter,lMinXYZ,lMinActiveMask);
HIKSetMaxXYZRotationdv(pHIKCharacter,pNodeIter,lMaxXYZ,lMaxActiveMask);
HIKSetRotationOrder(pHIKCharacter,pNodeIter, HIKRotationOrder(lOrder));
if(updatePrePost)
{
HIKSetPreQdv(pHIKCharacter,pNodeIter,lPreQ);
HIKSetPostQdv(pHIKCharacter,pNodeIter,lPostQ);
}
}
}
}
void ReadCharacterAndPropertiesForSolve(HIKCharacter *pHIKCharacter, HIKPropertySetState *pPropertyState, FBEvaluateInfo* pEvalInfo)
{
for(int lNodeIter = 0; lNodeIter < LastNodeId; lNodeIter++)
{
TransfertRotationsAndLimits(pHIKCharacter, lNodeIter);
}
ReadLengthLimits(pHIKCharacter, pEvalInfo);
ReadPropertySetState(pPropertyState,pEvalInfo);
// The chest and hips offset properties must be inverted to be consistent with MB internal solver.
}
void ReadReference(HIKCharacter *pHIKCharacter, HIKCharacterState *pCharacterState, FBEvaluateInfo* pEvalInfo)
{
double lGX[16];
this->GetNode(ReferenceNodeId).ReadXForm(lGX,pEvalInfo);
HIKSetNodeStatedv(pHIKCharacter, pCharacterState, ReferenceNodeId, lGX);
}
void ReadFloorState(HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo)
{
double lXForm[16];
for(int lFloorIter = 0; lFloorIter < HIKLastFloorId; lFloorIter++)
{
GetFloorNode(lFloorIter).ReadXForm(lXForm,pEvalInfo);
HIKSetEffectorFloorStatedv(pEffectorSetState,lFloorIter,lXForm);
}
}
void ReadNormalizedFromState(FBControlSetState* pSrcState, HIKCharacter *pHIKCharacter, HIKCharacterState *pState, FBEvaluateInfo* pEvalInfo)
{
FBMatrix lXForm;
for(int lBodyNodeId = 0; lBodyNodeId < kFBLastNodeId_Old; lBodyNodeId++)
{
pSrcState->GetNodeMatrix((FBBodyNodeId)lBodyNodeId, lXForm);
int lHIKNode = gFBBodyNodeToHIKNodeId[lBodyNodeId]; //kxl: this is ugly, will move it!
HIKSetNodeNormalizedStatedv(pHIKCharacter, pState,lHIKNode,lXForm);
}
}
void ReadEffectorsFromState(FBEffectorSetState* pSrcState, HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo)
{
FBMatrix lXForm;
FBTVector lIKPivot;
double lReachT, lReachR;
for(int lEffectorIter = 0; lEffectorIter < LastEffectorId; lEffectorIter++)
{
pSrcState->GetEffectorMatrix((FBEffectorId)lEffectorIter, lXForm);
pSrcState->GetReach((FBEffectorId)lEffectorIter,lReachT,lReachR);
HIKBlendEffectorPivotsdv(pEffectorSetState, lEffectorIter, (double (*)[16])lXForm.GetData(), &lIKPivot.mValue, &lReachT, &lReachR, 1);
}
}
void ReadFromPose(HIKCharacter *pHIKCharacter, FBEvaluateInfo* pEvalInfo, HIKCharacterState* pState, FBCharacterPose* pPose)
{
// regular bones
for(int lBodyNodeId = 0; lBodyNodeId < kFBLastNodeId_Old; lBodyNodeId++)
{
int lHIKId = FBBodyNodeIdToHIKNodeId((FBBodyNodeId)lBodyNodeId);
FBMatrix lMatrix = pPose->GetNodeMatrixGlobal(lBodyNodeId);
HIKSetNodeStatedv(pHIKCharacter, pState, lHIKId, (double*)lMatrix);
}
// extra bones
const int lExtraCount = pPose->GetExtraBoneCount();
for(int i = 0; i < lExtraCount; i++)
{
int lHIKId = ExtraIndexToHIKNodeId(i);
if(lHIKId != LastNodeId)
{
pPose->GetExtraBoneTransform(*(FBVector3d*)&lT, *(FBVector3d*)&lR, *(FBVector3d*)&lS, i);
lT.mValue[3] = lS.mValue[3] = 1;
HIKSetNodeStateTQSdv(pHIKCharacter, pState, lHIKId, lT.mValue, lQ.mValue, lS.mValue);
}
}
}
bool IsParentDirect( int pNodeId )
{
if ( this->GetNode(pNodeId).Valid() )
{
FBModel *lModel = this->GetNode(pNodeId).Get().mNode;
FBModel *lParent = lModel->Parent;
if (lParent)
{
for(int lNodeIter = 0; lNodeIter < LastNodeId; lNodeIter++)
{
if ( this->GetNode(lNodeIter).Valid() )
{
lModel = this->GetNode(lNodeIter).Get().mNode;
if ( lModel == lParent )
return true;
}
}
}
}
return false;
}
HIKCharacter *CharacterCreate(HIKMalloc pMalloc)
{
for(int lNodeIter = 0; lNodeIter < LastNodeId; lNodeIter++)
{
lHIKDef.mUsedNodes[lNodeIter] = HIKNodeNotUsed;
if(this->GetNode(lNodeIter).Valid())
{
lHIKDef.mUsedNodes[lNodeIter] |= HIKNodeUsed;
//if(GetNode(lNodeIter).GetUseRotationLimits() || GetNode(lNodeIter).GetUseTranslationLimits())
//{ always enable limits since SnS require them
lHIKDef.mUsedNodes[lNodeIter] |= HIKNodeLimits;
//}
// if(!GetNode(lNodeIter).GetParentDirect())
if( !IsParentDirect(lNodeIter) )
{
lHIKDef.mUsedNodes[lNodeIter] |= HIKNodeParentOffset;
}
}
}
HIKCharacter *lHIKCharacter = HIKCharacterCreate(&lHIKDef, pMalloc, AutodeskCustomerString);
for(int lNodeIter = 0; lNodeIter < LastNodeId; lNodeIter++)
{
TransfertRotationsAndLimits(lHIKCharacter, lNodeIter, true);
}
return lHIKCharacter;
}
};
template<typename HostNode> class HIKControlRigHost : public HIKHost<HostNode>
{
HIKHostNode<HostNode> mEffectorArray[LastEffectorId][HIKLastEffectorSetId];
public:
HIKHostNode<HostNode>& GetEffector(int pEffectorId,int pEffectorSetId) { return mEffectorArray[pEffectorId][pEffectorSetId]; }
void ReadEffectorState(HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo, bool pFKIK, bool pBlendAuxiliaryWithEffector)
{
FBTVector lTArray[HIKLastEffectorSetId];
FBQuaternion lQArray[HIKLastEffectorSetId];
FBVector4d lSArray[HIKLastEffectorSetId];
double lIKPivotArray[HIKLastEffectorSetId][4];
double lReachTArray[HIKLastEffectorSetId];
double lReachRArray[HIKLastEffectorSetId];
for(int lEffectorIter = 0; lEffectorIter < LastEffectorId; lEffectorIter++)
{
for(int lSetIter = 0; lSetIter < HIKLastEffectorSetId; lSetIter++)
{
lReachTArray[lSetIter] = pFKIK || lSetIter > 0 ? mEffectorArray[lEffectorIter][lSetIter].ReadReachT(pEvalInfo) : 1;
lReachRArray[lSetIter] = pFKIK || lSetIter > 0 ? mEffectorArray[lEffectorIter][lSetIter].ReadReachR(pEvalInfo) : 1;
if ( (lReachTArray[lSetIter] > 0 || lReachRArray[lSetIter] > 0) && ( lSetIter > 0 ? pBlendAuxiliaryWithEffector : true ) )
{
mEffectorArray[lEffectorIter][lSetIter].ReadTQS(lTArray[lSetIter],lQArray[lSetIter],lSArray[lSetIter],pEvalInfo);
mEffectorArray[lEffectorIter][lSetIter].ReadIKPivot(lIKPivotArray[lSetIter],pEvalInfo);
}
else
{
lTArray[lSetIter].Init();
lQArray[lSetIter].Init();
lSArray[lSetIter].mValue[0]=lSArray[lSetIter].mValue[1]=lSArray[lSetIter].mValue[2]=lSArray[lSetIter].mValue[3]=lQArray[lSetIter].mValue[3]=1.0;
memset(lIKPivotArray[lSetIter],0,sizeof(double)*4);
lReachTArray[lSetIter] = 0.0;
lReachRArray[lSetIter] = 0.0;
}
}
HIKBlendEffectorPivotsTQSdv(pEffectorSetState,lEffectorIter,&lTArray[0].mValue,&lQArray[0].mValue,&lSArray[0].mValue,lIKPivotArray,lReachTArray,lReachRArray,HIKLastEffectorSetId);
}
}
void ReadEffectorState(HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo, bool pFKIK, bool &pDetectedRecursivity, int* pEffectorsRecursiveFlag, bool pBlendAuxiliaryWithEffector)
{
pDetectedRecursivity = false;
FBTVector lTArray[HIKLastEffectorSetId];
FBQuaternion lQArray[HIKLastEffectorSetId];
FBVector4d lSArray[HIKLastEffectorSetId];
double lIKPivotArray[HIKLastEffectorSetId][4];
double lReachTArray[HIKLastEffectorSetId];
double lReachRArray[HIKLastEffectorSetId];
for(int lEffectorIter = 0; lEffectorIter < LastEffectorId; lEffectorIter++)
{
pEffectorsRecursiveFlag[lEffectorIter] = eNoRecursion;
for(int lSetIter = 0; lSetIter < HIKLastEffectorSetId; lSetIter++)
{
lReachTArray[lSetIter] = pFKIK || lSetIter > 0 ? mEffectorArray[lEffectorIter][lSetIter].ReadReachT(pEvalInfo) : 1;
lReachRArray[lSetIter] = pFKIK || lSetIter > 0 ? mEffectorArray[lEffectorIter][lSetIter].ReadReachR(pEvalInfo) : 1;
if ( (lReachTArray[lSetIter] > 0 || lReachRArray[lSetIter] > 0) && ( lSetIter > 0 ? pBlendAuxiliaryWithEffector : true ) )
{
mEffectorArray[lEffectorIter][lSetIter].ReadTQS(lTArray[lSetIter],lQArray[lSetIter],lSArray[lSetIter],pEvalInfo);
pEffectorsRecursiveFlag[lEffectorIter] |= mEffectorArray[lEffectorIter][lSetIter].IsEvaluationRecursiveTQS(pEvalInfo);
if(pEffectorsRecursiveFlag[lEffectorIter] != eNoRecursion) pDetectedRecursivity = true;
mEffectorArray[lEffectorIter][lSetIter].ReadIKPivot(lIKPivotArray[lSetIter],pEvalInfo);
}
else
{
lTArray[lSetIter].Init();
lQArray[lSetIter].Init();
lSArray[lSetIter].mValue[0]=lSArray[lSetIter].mValue[1]=lSArray[lSetIter].mValue[2]=lSArray[lSetIter].mValue[3]=lQArray[lSetIter].mValue[3]=1.0;
memset(lIKPivotArray[lSetIter],0,sizeof(double)*4);
lReachTArray[lSetIter] = 0.0;
lReachRArray[lSetIter] = 0.0;
}
}
HIKBlendEffectorPivotsTQSdv(pEffectorSetState,lEffectorIter,&lTArray[0].mValue,&lQArray[0].mValue,&lSArray[0].mValue,lIKPivotArray,lReachTArray,lReachRArray,HIKLastEffectorSetId);
}
}
void ReadRecursiveEffectorState(HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo, bool pFKIK, int* pEffectorsRecursiveFlag, int &pReadIndex)
{
FBTVector lTArray[HIKLastEffectorSetId];
FBQuaternion lQArray[HIKLastEffectorSetId];
FBVector4d lSArray[HIKLastEffectorSetId];
double lIKPivotArray[HIKLastEffectorSetId][4];
double lReachTArray[HIKLastEffectorSetId];
double lReachRArray[HIKLastEffectorSetId];
for(int lEffectorIter = 0; lEffectorIter < LastEffectorId; lEffectorIter++)
{
//if this is recursive effector, read it to get notification models/connectors that it evaluate - those have to be pin.
if(pEffectorsRecursiveFlag[lEffectorIter] != eNoRecursion)
{
pReadIndex = lEffectorIter;
for(int lSetIter = 0; lSetIter < HIKLastEffectorSetId; lSetIter++)
{
mEffectorArray[lEffectorIter][lSetIter].ReadTQS(lTArray[lSetIter],lQArray[lSetIter],lSArray[lSetIter],pEvalInfo);
mEffectorArray[lEffectorIter][lSetIter].ReadIKPivot(lIKPivotArray[lSetIter],pEvalInfo);
lReachTArray[lSetIter] = pFKIK || lSetIter > 0 ? mEffectorArray[lEffectorIter][lSetIter].ReadReachT(pEvalInfo) : 1;
lReachRArray[lSetIter] = pFKIK || lSetIter > 0 ? mEffectorArray[lEffectorIter][lSetIter].ReadReachR(pEvalInfo) : 1;
}
HIKBlendEffectorPivotsTQSdv(pEffectorSetState,lEffectorIter,&lTArray[0].mValue,&lQArray[0].mValue,&lSArray[0].mValue,lIKPivotArray,lReachTArray,lReachRArray,HIKLastEffectorSetId);
}
}
}
void WriteEffectorsState(HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo, bool pWriteAll=false,int* pEffectorsRecursiveFlag=NULL)
{
for(int lSetIter = 0; lSetIter < FBLastEffectorSetIndex; lSetIter++)
{
WriteEffectorState(pEffectorSetState, pEvalInfo, lSetIter, pWriteAll, pEffectorsRecursiveFlag);
}
}
void WriteEffectorsStateCandidate(HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo, bool pWriteAll=false)
{
for(int lSetIter = 0; lSetIter < FBLastEffectorSetIndex; lSetIter++)
{
WriteEffectorStateCandidate(pEffectorSetState, pEvalInfo, lSetIter, pWriteAll);
}
}
void SetEffectorStateCandidate(HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo, bool pWriteAll=false)
{
for(int lSetIter = 0; lSetIter < FBLastEffectorSetIndex; lSetIter++)
{
SetEffectorStateCandidate(pEffectorSetState, pEvalInfo, lSetIter, pWriteAll);
}
}
private:
void WriteEffectorState(HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo, int pSetIndex, bool pWriteAll=false,int* pEffectorsRecursiveFlag=NULL)
{
int lIter = 0;
for(lIter = 0; lIter < LastEffectorId; lIter++)
{
if(pWriteAll || pSetIndex == 0 || mEffectorArray[lIter][pSetIndex].GetIKSync())
{
FBTVector lIKPivot;
HIKGetEffectorStateTQSdv(pEffectorSetState, lIter, lT.mValue, lQ.mValue, lS.mValue);
mEffectorArray[lIter][pSetIndex].ReadIKPivot(lIKPivot.mValue,pEvalInfo);
if( lIKPivot.mValue[0] != 0.0 ||
lIKPivot.mValue[1] != 0.0 ||
lIKPivot.mValue[2] != 0.0)
{
FBSVector lPivotGSVector;
mEffectorArray[lIter][pSetIndex].GetVector(lPivotGSVector, kModelScaling, true, pEvalInfo);
lIKPivot.mValue[0] *= lPivotGSVector.mValue[0];
lIKPivot.mValue[1] *= lPivotGSVector.mValue[1];
lIKPivot.mValue[2] *= lPivotGSVector.mValue[2];
FBMatrix lGRM;
FBVectorMatrixMult(lIKPivot,lGRM,lIKPivot);
FBSub(lIKPivot,lT,lIKPivot);
}
else
{
lIKPivot = lT;
}
if(pEffectorsRecursiveFlag == NULL)
{
mEffectorArray[lIter][pSetIndex].WriteTQS(lIKPivot, lQ, lS, pEvalInfo);
}
else
{
mEffectorArray[lIter][pSetIndex].WriteTQSIfNotRecursive(lIKPivot, lQ, lS, pEvalInfo, pEffectorsRecursiveFlag[lIter]);
}
}
}
}
void WriteEffectorStateCandidate(HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo, int pSetIndex, bool pWriteAll=false)
{
int lIter = 0;
for(lIter = 0; lIter < LastEffectorId; lIter++)
{
if(pWriteAll || pSetIndex == 0 || mEffectorArray[lIter][pSetIndex].GetIKSync())
{
FBTVector lIKPivot;
HIKGetEffectorStateTQSdv(pEffectorSetState, lIter, lT.mValue, lQ.mValue, lS.mValue);
mEffectorArray[lIter][pSetIndex].ReadIKPivot(lIKPivot.mValue,pEvalInfo);
if( lIKPivot.mValue[0] != 0.0 ||
lIKPivot.mValue[1] != 0.0 ||
lIKPivot.mValue[2] != 0.0)
{
FBSVector lPivotGSVector;
mEffectorArray[lIter][pSetIndex].GetVector(lPivotGSVector, kModelScaling, true, pEvalInfo);
lIKPivot.mValue[0] *= lPivotGSVector.mValue[0];
lIKPivot.mValue[1] *= lPivotGSVector.mValue[1];
lIKPivot.mValue[2] *= lPivotGSVector.mValue[2];
FBMatrix lGRM;
FBVectorMatrixMult(lIKPivot,lGRM,lIKPivot);
FBSub(lIKPivot,lT,lIKPivot);
}
else
{
lIKPivot = lT;
}
mEffectorArray[lIter][pSetIndex].WriteCandidateTQS(lIKPivot, lQ, lS, pEvalInfo);
}
}
}
void SetEffectorStateCandidate(HIKEffectorSetState *pEffectorSetState, FBEvaluateInfo* pEvalInfo, int pSetIndex, bool pWriteAll=false)
{
int lIter = 0;
for(lIter = 0; lIter < LastEffectorId; lIter++)
{
if(pWriteAll || pSetIndex == 0 || mEffectorArray[lIter][pSetIndex].GetIKSync())
{
FBTVector lIKPivot;
HIKGetEffectorStateTQSdv(pEffectorSetState, lIter, lT.mValue, lQ.mValue, lS.mValue);
mEffectorArray[lIter][pSetIndex].ReadIKPivot(lIKPivot.mValue,pEvalInfo);
if( lIKPivot.mValue[0] != 0.0 ||
lIKPivot.mValue[1] != 0.0 ||
lIKPivot.mValue[2] != 0.0)
{
FBSVector lPivotGSVector;
mEffectorArray[lIter][pSetIndex].GetVector(lPivotGSVector, kModelScaling, true, pEvalInfo);
lIKPivot.mValue[0] *= lPivotGSVector.mValue[0];
lIKPivot.mValue[1] *= lPivotGSVector.mValue[1];
lIKPivot.mValue[2] *= lPivotGSVector.mValue[2];
FBMatrix lGRM;
FBVectorMatrixMult(lIKPivot,lGRM,lIKPivot);
FBSub(lIKPivot,lT,lIKPivot);
}
else
{
lIKPivot = lT;
}
mEffectorArray[lIter][pSetIndex].SetCandidateTQS(lIKPivot, lQ, lS, pEvalInfo);
}
}
}
};
template<typename HostNode,typename HostProperty> class HIKControlRigHostEvaluator
{
public:
HIKCharacter* mHIKCharacter;
HIKCharacterHost<HostNode,HostProperty>* mHIKCharacterHost;
HIKControlRigHost<HostNode>* mHIKControlRigHost;
int mSolvingStep;
bool mFKIK;
HIKControlRigHostEvaluator()
{
mHIKCharacter = 0;
mHIKCharacterHost = 0;
mHIKControlRigHost = 0;
mSolvingStep = HIKSolvingStepAll;
mFKIK = true;
}
void SetSolvingStep( int pSolvingState )
{
mSolvingStep = pSolvingState;
}
void Init(HIKCharacter *pHIKCharacter,HIKCharacterHost<HostNode,HostProperty> *pHIKCharacterHost,HIKControlRigHost<HostNode> *pHIKControlRigHost,HIKMalloc pMalloc, bool pFKIK)
{
mHIKCharacter = pHIKCharacter;
mHIKCharacterHost = pHIKCharacterHost;
mHIKControlRigHost = pHIKControlRigHost;
mFKIK = pFKIK;
}
void Clear(HIKFree pFree)
{
mHIKCharacter = 0;
mHIKCharacterHost = 0;
mHIKControlRigHost = 0;
}
static void SetPropertiesDisabledInControlRigSolve(HIKPropertySetState *pPropertyState)
{
// In control rig, we force the smooth reach to 0 to get direct manipulation.
HIKSetPropertyValue(pPropertyState,HIKSnSSmoothReach,0.0f);
// Disable space modification - similar to internal EffectorSetSpaceCorrection that is not called during Control Rig solve
// This is very important, because modifications can be key and then, after they will be applied again accumulating modifications
HIKSetPropertyMode(pPropertyState,HIKMirrorId,0);
HIKSetPropertyValue(pPropertyState,HIKHipsTOffsetXId,0.0f);
HIKSetPropertyValue(pPropertyState,HIKHipsTOffsetYId,0.0f);
HIKSetPropertyValue(pPropertyState,HIKHipsTOffsetZId,0.0f);
}
void Read( FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState, bool* pDetectedIKRecursiveEvaluation=NULL, bool pBlendAuxiliaryWithEffector = true )
{
// read effectors
if(pDetectedIKRecursiveEvaluation)
{
mHIKControlRigHost->ReadEffectorState(pState->mEffectorSetState,pEvalInfo,mFKIK,*pDetectedIKRecursiveEvaluation,pState->mRecursiveFlag, pBlendAuxiliaryWithEffector);
}
else
{
mHIKControlRigHost->ReadEffectorState(pState->mEffectorSetState,pEvalInfo,mFKIK, pBlendAuxiliaryWithEffector);
}
// read state from control rig fk rig
if(mFKIK)
mHIKControlRigHost->ReadState(mHIKCharacter, pState->mState,pEvalInfo,true);
// this is IK only rig, use relax pose for fk state
else
HIKGetRelaxPose(mHIKCharacter, pState->mState);
// read properties and update character limits
mHIKCharacterHost->ReadCharacterAndPropertiesForSolve(mHIKCharacter,pState->mPropertySetState,pEvalInfo);
// read floor contacts
mHIKCharacterHost->ReadFloorState(pState->mEffectorSetState,pEvalInfo);
// copy pull and resist to effector stata from properties
HIKSetEffectorPullResistFromPropertyState(pState->mEffectorSetState, pState->mPropertySetState);
// disable properties that should not be solved in control rig
SetPropertiesDisabledInControlRigSolve(pState->mPropertySetState);
}
void ReadRecursiveEffectors( FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState )
{
mHIKControlRigHost->ReadRecursiveEffectorState(pState->mEffectorSetState,pEvalInfo,mFKIK,pState->mRecursiveFlag,pState->mEvaluatedEffectorId);
}
void ReadSkeletonSolveOnRig( FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState, FBCharacter* pCharacter )
{
// read skeleton state
mHIKCharacterHost->ReadState( mHIKCharacter, pState->mState, pEvalInfo, false);
// read properties and update character limits
mHIKCharacterHost->ReadCharacterAndPropertiesForSolve(mHIKCharacter,pState->mPropertySetState,pEvalInfo);
// read floor contacts
mHIKCharacterHost->ReadFloorState( pState->mEffectorSetState, pEvalInfo);
// copy pull and resist to effector state from properties
HIKSetEffectorPullResistFromPropertyState(pState->mEffectorSetState, pState->mPropertySetState);
// In control rig, we force the smooth reach to 0 to get direct manipulation.
HIKSetPropertyValue(pState->mPropertySetState,HIKSnSSmoothReach,0.0f);
int lMode = HIKGetPropertyMode(pState->mPropertySetState, HIKMirrorId);
if (lMode == 1)
{
// to use default mirror as we want
FBQuaternion lMirrorQ(0.0, 0.0, 0.0, 1.0);
HIKMirrorState( mHIKCharacter, pState->mState, pState->mState, mHIKCharacter, lMirrorQ.mValue);
pCharacter->MirrorCharacterExtensions(pEvalInfo);
}
// solver back on Rig
EffectorStateFromControlSet(pState);
}
void Solve(HIKEvaluationState* pState, const int pDisableSolvingSteps=0)
{
HIKSetIKSolvingStep(pState->mEffectorSetState, (mSolvingStep & ~pDisableSolvingSteps));
HIKSolveForEffectorSet(mHIKCharacter,pState->mState,pState->mEffectorSetState, pState->mPropertySetState);
}
void SolveAndWriteControlRig(FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState, bool pWriteRig, bool pDoubleSolve=false, const int pDisableSolvingSteps=0)
{
if(pDoubleSolve)
{
//recursive effectors should not resist/pull on others.
for(int lEffectorId = 0; lEffectorId < LastEffectorId; lEffectorId++)
{
if((pState->mRecursiveFlag[lEffectorId] & eRecursiveT) > 0)
{
HIKSetPull( pState->mEffectorSetState, lEffectorId, 0.0f );
}
if((pState->mRecursiveFlag[lEffectorId] & eRecursiveR) > 0)
{
HIKSetResist( pState->mEffectorSetState, lEffectorId, 0.0f );
}
}
}
// are we in relative extraction mode - solve and write
if(HIKGetPropertyValue(pState->mPropertySetState, HIKRollExtractionMode) == 0)
{
Solve(pState, pDisableSolvingSteps);
if(pWriteRig)
{
WriteRig(pEvalInfo, pState, false, true, 0, pDoubleSolve);
}
}
// absolute extraction mode - we don't write roll extraction on control rig, it will be only written onto skeleton
// this is done to avoid accumulation of extraction at each manipulation/key
else
{
HIKCharacterState* lCharacterState = pState->mState;
HIKEffectorSetState* lEffectorSetState = pState->mEffectorSetState;
HIKEffectorSetState* lTempEffectorSet = pState->mSrcEffectorSetState;
const HIKPropertySetState* lPropertyState = pState->mPropertySetState;
HIKSetIKSolvingStep(lEffectorSetState, (mSolvingStep & ~pDisableSolvingSteps));
// this function needs to stay align with HIKSolveForEffectorSet (we need to call all solving steps.)
// reason why we have it is because we want to write control rig align without roll bone extraction step
// to keep proper solve after keying
HIKSolveForEffectorSetBegin(mHIKCharacter,lCharacterState,lEffectorSetState,lPropertyState);
HIKSolveForEffectorSetFloorContactApprox(mHIKCharacter,lCharacterState,lEffectorSetState,lPropertyState);
HIKSolveForEffectorSetBodyPull(mHIKCharacter,lCharacterState,lEffectorSetState,lPropertyState);
HIKSolveForEffectorSetArmsAndLegs(mHIKCharacter,lCharacterState,lEffectorSetState,lPropertyState);
HIKSolveForEffectorSetFingersAndToes(mHIKCharacter,lCharacterState,lEffectorSetState,lPropertyState);
HIKSolveForEffectorSetHeadAndNeck(mHIKCharacter,lCharacterState,lEffectorSetState,lPropertyState);
HIKSolveForEffectorSetFloorContact(mHIKCharacter,lCharacterState,lEffectorSetState,lPropertyState);
HIKSolveForEffectorSetHipsTranslation(mHIKCharacter,lCharacterState,lEffectorSetState,lPropertyState);
// write rig align (after full body key, there should be no difference between solve with and without rig align on control rig)
if(pWriteRig)
{
WriteRig(pEvalInfo, pState, false, true, lTempEffectorSet, pDoubleSolve);
}
// finish solve
HIKSolveForEffectorSetRollExtraction(mHIKCharacter,lCharacterState,lEffectorSetState,lPropertyState);
//HIKSolveForEffectorSetModifiers(mHIKCharacter,lCharacterState,lEffectorSetState,lPropertyState);
if(pDoubleSolve)
HIKEffectorStateCopy(lEffectorSetState, lTempEffectorSet);
}
if(pDoubleSolve)
{
// reset pull and resist for all effectors
HIKSetEffectorPullResistFromPropertyState(pState->mEffectorSetState, pState->mPropertySetState);
}
}
void Write( FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState )
{
mHIKCharacterHost->WriteState(mHIKCharacter, pState->mState,pEvalInfo,false);
}
void WriteCandidate( FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState )
{
mHIKCharacterHost->WriteStateCandidate(mHIKCharacter, pState->mState,pEvalInfo,false);
}
void WriteRig(FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState, bool pWriteAll=false, bool pSnapEffectorsToRig=true, HIKEffectorSetState* pEffectorSetToUse=NULL, bool pDoubleSolve=false)
{
if(pEffectorSetToUse == NULL)
{
pEffectorSetToUse = pState->mEffectorSetState;
}
if(pSnapEffectorsToRig)
{
// Disable properties that will be disabled in control rig.
SetPropertiesDisabledInControlRigSolve(pState->mPropertySetState);
HIKEffectorSetFromCharacter(mHIKCharacter, pEffectorSetToUse, pState->mState, pState->mPropertySetState);
}
mHIKControlRigHost->WriteState(mHIKCharacter, pState->mState, pEvalInfo, true);
mHIKControlRigHost->WriteEffectorsState(pEffectorSetToUse, pEvalInfo, pWriteAll, pDoubleSolve ? pState->mRecursiveFlag : NULL);
}
void WriteRigCandidate( FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState, bool pSetGlobalPropertyCandidate = false )
{
if(pSetGlobalPropertyCandidate)
{
mHIKControlRigHost->SetStateCandidate(mHIKCharacter, pState->mState, pEvalInfo, true);
mHIKControlRigHost->SetEffectorStateCandidate(pState->mEffectorSetState, pEvalInfo);
}
else
{
mHIKControlRigHost->WriteStateCandidate(mHIKCharacter, pState->mState, pEvalInfo, true);
mHIKControlRigHost->WriteEffectorsStateCandidate(pState->mEffectorSetState, pEvalInfo);
}
}
void ApplyLock(HIKEvaluationState* pState, bool pLockX, bool pLockY, bool pLockZ)
{
if(pLockX || pLockY || pLockZ)
{
FBTVector lHipsDefaultGT;
FBMatrix lHipsGX, lHipsLX;
FBMatrix lRefGX;
//Get default Hips node position(global space)
HIKGetCharacterizeNodeStateTQSdv(mHIKCharacter, HipsNodeId, lHipsDefaultGT, FBQuaternion(), FBVector4d());
//Get current transformation matrix(global space) for Reference and Hips nodes
HIKGetNodeStatedv(mHIKCharacter, pState->mState, ReferenceNodeId, lRefGX);
HIKGetNodeStatedv(mHIKCharacter, pState->mState, HipsNodeId, lHipsGX);
//Compute current transformation matrix(local space) for Hips node, with respect to Reference node
FBGetLocalMatrix(lHipsLX, lRefGX, lHipsGX);
//Compute offset vector to translate character skeleton to default position (for those axes in which the character is locked-in-place)
FBTVector lOffsetT;
if(pLockX) lOffsetT[0] = lHipsDefaultGT[0] - lHipsLX(3, 0);
if(pLockY) lOffsetT[1] = lHipsDefaultGT[1] - lHipsLX(3, 1);
if(pLockZ) lOffsetT[2] = lHipsDefaultGT[2] - lHipsLX(3, 2);
FBVectorMatrixMult(lOffsetT, lRefGX, lOffsetT);
//Apply offset translational vector to all nodes
HIKNodeStatePreMultTQSUpdv(mHIKCharacter, pState->mState, lOffsetT, FBQuaternion(0,0,0,1), FBVector4d(1,1,1,1), false);
}
}
void EffectorStateFromControlSet(HIKEvaluationState* pState)
{
HIKEffectorSetFromCharacter(mHIKCharacter, pState->mEffectorSetState, pState->mState, pState->mPropertySetState);
}
};
template<typename HostNode,typename HostProperty> class HIKCharacterHostEvaluator
{
public:
HIKCharacter *mHIKCharacterDst;
HIKCharacterHost<HostNode,HostProperty> *mHIKCharacterHostDst;
HIKCharacter *mHIKCharacterSrc;
HIKCharacterHost<HostNode,HostProperty> *mHIKCharacterHostSrc;
int mSolvingStep;
HIKCharacterHostEvaluator()
{
mHIKCharacterDst = 0;
mHIKCharacterHostDst = 0;
mHIKCharacterSrc = 0;
mHIKCharacterHostSrc = 0;
mSolvingStep = HIKSolvingStepAll;
}
void SetSolvingStep( int pSolvingState )
{
mSolvingStep = pSolvingState;
}
void Init(HIKCharacter *pHIKCharacterDst,HIKCharacterHost<HostNode,HostProperty> *pHIKCharacterHostDst,HIKCharacter *pHIKCharacterSrc,HIKCharacterHost<HostNode,HostProperty> *pHIKCharacterHostSrc,HIKMalloc pMalloc)
{
mHIKCharacterDst = pHIKCharacterDst;
mHIKCharacterHostDst = pHIKCharacterHostDst;
mHIKCharacterSrc = pHIKCharacterSrc;
mHIKCharacterHostSrc = pHIKCharacterHostSrc;
}
void Clear(HIKFree pFree)
{
mHIKCharacterDst = 0;
mHIKCharacterHostDst = 0;
mHIKCharacterSrc = 0;
mHIKCharacterHostSrc = 0;
}
void Read( FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState )
{
if(mHIKCharacterHostSrc != NULL)
{
// read source state
mHIKCharacterHostSrc->ReadState(mHIKCharacterSrc, pState->mSrcState,pEvalInfo,false);
// read source properties
mHIKCharacterHostSrc->ReadCharacterAndPropertiesForSolve(mHIKCharacterSrc,pState->mSrcPropertySetState,pEvalInfo);
}
// read destination properties
mHIKCharacterHostDst->ReadCharacterAndPropertiesForSolve(mHIKCharacterDst,pState->mPropertySetState,pEvalInfo);
// read destination floor contacts (not needed for source)
mHIKCharacterHostDst->ReadFloorState(pState->mEffectorSetState,pEvalInfo);
// read reference position
mHIKCharacterHostDst->ReadReference(mHIKCharacterDst, pState->mState, pEvalInfo);
}
void ReadFromActor( FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState, FBActor* pActor ) //kxl: this should be done as a host...but for now
{
pActor->UpdateValues(pEvalInfo);
FBMatrix lGX;
FBSVector lGS;
FBSkeletonState* lSkeletonState = pActor->GetCurrentSkeletonState();
for(int lActorIter = kFBSkeletonHipsIndex; lActorIter < kFBSkeletonLastIndex; lActorIter++)
{
pActor->GetDefinitionScaleVector((FBSkeletonNodeId)lActorIter, lGS);
lSkeletonState->GetNodeMatrix((FBSkeletonNodeId)lActorIter, lGX);
FBMult(lGX,lGX,lGS);
HIKSetNodeStatedv(mHIKCharacterSrc, pState->mSrcState, gActor2HIK[lActorIter], (double*)lGX);
}
for(int lPIter = 0; lPIter < HIKLastPropertyId; lPIter++)
{
HIKSetPropertyMode(pState->mSrcPropertySetState,lPIter,HIKGetPropertyInfoDefaultMode(lPIter));
if(lPIter == HIKFootBottomToAnkleId)
{
pActor->GetDefinitionScaleVector(kFBSkeletonLeftAnkleIndex, lGS);
HIKSetPropertyValue(pState->mSrcPropertySetState,lPIter,7.5f*lGS[1]);
}
else
{
HIKSetPropertyValue(pState->mSrcPropertySetState,lPIter,HIKGetPropertyInfoDefaultValue(lPIter));
}
}
}
void ReadFromFKIK( FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState, FBControlSetState* pFKState, FBEffectorSetState* pIKState )
{
// read destination properties
mHIKCharacterHostDst->ReadCharacterAndPropertiesForSolve(mHIKCharacterDst,pState->mPropertySetState,pEvalInfo);
// read destination floor contacts (not needed for source)
mHIKCharacterHostDst->ReadFloorState(pState->mEffectorSetState,pEvalInfo);
// read states from given internal states
mHIKCharacterHostDst->ReadNormalizedFromState(pFKState, mHIKCharacterDst, pState->mState, pEvalInfo);
mHIKCharacterHostDst->ReadEffectorsFromState(pIKState, pState->mEffectorSetState, pEvalInfo);
// copy pull and resist to effector state from properties
HIKSetEffectorPullResistFromPropertyState(pState->mEffectorSetState, pState->mPropertySetState);
}
void SolveRetarget(HIKEvaluationState* pState)
{
HIKSetIKSolvingStep(pState->mEffectorSetState, mSolvingStep);
HIKSolveForCharacter(mHIKCharacterDst,pState->mState,mHIKCharacterSrc,pState->mSrcState,pState->mEffectorSetState, pState->mPropertySetState, pState->mSrcPropertySetState);
}
void SolveIK(HIKEvaluationState* pState)
{
HIKSetIKSolvingStep(pState->mEffectorSetState, mSolvingStep);
HIKSolveForEffectorSet(mHIKCharacterDst, pState->mState, pState->mEffectorSetState, pState->mPropertySetState);
}
void Write( FBEvaluateInfo* pEvalInfo, HIKEvaluationState* pState, bool pNormalized = false )
{
mHIKCharacterHostDst->WriteState(mHIKCharacterDst, pState->mState,pEvalInfo,pNormalized);
}
};
#endif