diff options
Diffstat (limited to 'Runtime/mecanim')
55 files changed, 19648 insertions, 0 deletions
diff --git a/Runtime/mecanim/animation/avatar.cpp b/Runtime/mecanim/animation/avatar.cpp new file mode 100644 index 0000000..441353b --- /dev/null +++ b/Runtime/mecanim/animation/avatar.cpp @@ -0,0 +1,1799 @@ +#include "UnityPrefix.h" + +#include "Runtime/mecanim/animation/avatar.h" + +#include "Runtime/mecanim/generic/valuearray.h" +#include "Runtime/mecanim/generic/crc32.h" +#include "Runtime/mecanim/generic/stringtable.h" +#include "Runtime/mecanim/skeleton/skeleton.h" +#include "Runtime/mecanim/human/human.h" +#include "Runtime/mecanim/animation/clipmuscle.h" +#include "Runtime/mecanim/animation/curvedata.h" +#include "Runtime/mecanim/statemachine/statemachine.h" +#include "Runtime/Serialize/TransferFunctions/TransferNameConversions.h" + +#include "Runtime/Misc/BuildSettings.h" + +namespace mecanim +{ + +namespace memory +{ + Profiler* Profiler::s_Profiler = NULL; +} + +namespace animation +{ + + AnimationSet* CreateAnimationSet(ControllerConstant const* controller, memory::Allocator& alloc) + { + SETPROFILERLABEL(AnimationSet); + + AnimationSet* animationSet = alloc.Construct<AnimationSet>(); + + animationSet->m_LayerCount = controller->m_LayerCount; + + animationSet->m_ClipPerLayer = alloc.ConstructArray<uint32_t>(animationSet->m_LayerCount); + memset(animationSet->m_ClipPerLayer,0,sizeof(uint32_t)*animationSet->m_LayerCount); + + animationSet->m_ClipConstant = alloc.ConstructArray<AnimationSet::Clip*>(animationSet->m_LayerCount); + + animationSet->m_AdditionalCount = controller->m_Values->m_Count; + animationSet->m_AdditionalIndexArray = alloc.ConstructArray<int32_t>(animationSet->m_AdditionalCount); + for(int i = 0; i < animationSet->m_AdditionalCount; i++) + animationSet->m_AdditionalIndexArray[i] = -1; + + animationSet->m_DynamicValuesMaskArray = alloc.ConstructArray<ValueArrayMask *>(animationSet->m_LayerCount); + + for(int layerIter = 0; layerIter < animationSet->m_LayerCount; layerIter++) + { + int clipCount = 0; + + mecanim::uint32_t stateMachineIndex = controller->m_LayerArray[layerIter]->m_StateMachineIndex; + + if(stateMachineIndex != DISABLED_SYNCED_LAYER_IN_NON_PRO) + { + mecanim::uint32_t motionSetIndex = controller->m_LayerArray[layerIter]->m_StateMachineMotionSetIndex; + + const statemachine::StateMachineConstant* stateMachineConstant = controller->m_StateMachineArray[stateMachineIndex].Get(); + + for(int stateIter = 0; stateIter < stateMachineConstant->m_StateConstantCount; stateIter++) + { + statemachine::StateConstant const& stateConstant = *controller->m_StateMachineArray[stateMachineIndex]->m_StateConstantArray[stateIter]; + + clipCount += stateConstant.m_LeafInfoArray[motionSetIndex].m_Count; + + for(int blendTreeIter = 0; blendTreeIter < stateConstant.m_BlendTreeCount; blendTreeIter++) + { + uint32_t blendCount = GetMaxBlendCount(*stateConstant.m_BlendTreeConstantArray[blendTreeIter]); + + if(blendCount > animationSet->m_MaxBlendState) + { + animationSet->m_MaxBlendState = blendCount; + } + } + } + } + + animationSet->m_ClipPerLayer[layerIter] = clipCount; + animationSet->m_ClipConstant[layerIter] = alloc.ConstructArray<AnimationSet::Clip>(clipCount); + + animationSet->m_DynamicValuesMaskArray[layerIter] = 0; + } + + // if there is no blend tree the worst case is when we are doing a transition between two state + // in this case we need to evaluate two clip. + animationSet->m_MaxBlendState = math::maximum<uint32_t>(animationSet->m_MaxBlendState*2, 2); + + return animationSet; + } + + void DestroyAnimationSet(AnimationSet* animationSet, memory::Allocator& alloc) + { + if(animationSet) + { + alloc.Deallocate(animationSet->m_ClipPerLayer); + + for(int layerIter = 0; layerIter < animationSet->m_LayerCount; layerIter++) + { + alloc.Deallocate(animationSet->m_ClipConstant[layerIter]); + DestroyValueArrayMask(animationSet->m_DynamicValuesMaskArray[layerIter],alloc); + } + + alloc.Deallocate(animationSet->m_ClipConstant); + alloc.Deallocate(animationSet->m_AdditionalIndexArray); + alloc.Deallocate(animationSet->m_DynamicValuesMaskArray); + DestroyValueArrayConstant(animationSet->m_DynamicFullValuesConstant, alloc); + alloc.Deallocate(animationSet); + } + } + + AnimationSetMemory* CreateAnimationSetMemory(AnimationSet const* animationSet, bool allowConstantCurveOptimization, memory::Allocator& alloc) + { + SETPROFILERLABEL(AnimationSetMemory); + + AnimationSetMemory* memory = alloc.Construct<AnimationSetMemory>(); + + memory->m_LayerCount = animationSet->m_LayerCount; + memory->m_ClipPerLayer = alloc.ConstructArray<uint32_t>(animationSet->m_LayerCount); + memory->m_ClipMemory = alloc.ConstructArray<ClipMemory**>(animationSet->m_LayerCount); + + uint32_t maxCurvesCount = 0; + for(int layerIter = 0; layerIter < animationSet->m_LayerCount; layerIter++) + { + memory->m_ClipPerLayer[layerIter] = animationSet->m_ClipPerLayer[layerIter]; + memory->m_ClipMemory[layerIter] = alloc.ConstructArray<ClipMemory*>(animationSet->m_ClipPerLayer[layerIter]); + + for(int clipIter = 0; clipIter < memory->m_ClipPerLayer[layerIter]; clipIter++) + { + const ClipMuscleConstant* clip = animationSet->m_ClipConstant[layerIter][clipIter].m_Clip; + + if(clip != 0) + { + uint32_t usedCurves; + if (allowConstantCurveOptimization) + usedCurves = animationSet->m_ClipConstant[layerIter][clipIter].m_TotalUsedOptimizedCurveCount; + else + usedCurves = GetClipCurveCount(*clip); + + maxCurvesCount = math::maximum(maxCurvesCount, usedCurves); + memory->m_ClipMemory[layerIter][clipIter] = mecanim::animation::CreateClipMemory(clip->m_Clip.Get(), usedCurves, alloc); + } + else + { + memory->m_ClipMemory[layerIter][clipIter] = 0; + } + } + } + + memory->m_ClipOutput = mecanim::animation::CreateClipOutput(maxCurvesCount, alloc); + + return memory; + } + + void DestroyAnimationSetMemory(AnimationSetMemory* animationSetMemory, memory::Allocator& alloc) + { + if (animationSetMemory) + { + for(int layerIter = 0; layerIter < animationSetMemory->m_LayerCount; layerIter++) + { + for(int clipIter = 0 ; clipIter < animationSetMemory->m_ClipPerLayer[layerIter]; clipIter++) + { + mecanim::animation::DestroyClipMemory(animationSetMemory->m_ClipMemory[layerIter][clipIter], alloc); + } + + alloc.Deallocate(animationSetMemory->m_ClipMemory[layerIter]); + } + + alloc.Deallocate(animationSetMemory->m_ClipPerLayer); + alloc.Deallocate(animationSetMemory->m_ClipMemory); + mecanim::animation::DestroyClipOutput(animationSetMemory->m_ClipOutput, alloc); + alloc.Deallocate(animationSetMemory); + } + } + + void AvatarConstant::InitializeClass() + { + RegisterAllowNameConversion("AvatarConstant", "m_Skeleton", "m_AvatarSkeleton"); + RegisterAllowNameConversion("AvatarConstant", "m_SkeletonPose", "m_AvatarSkeletonPose"); + } + + AvatarConstant* CreateAvatarConstant( skeleton::Skeleton* skeleton, + skeleton::SkeletonPose* skeletonPose, + skeleton::SkeletonPose* defaultPose, + human::Human* human, + skeleton::Skeleton* rootMotionSkeleton, + int rootMotionIndex, + math::xform const& rootMotionX, + memory::Allocator& alloc) + { + SETPROFILERLABEL(AvatarConstant); + + AvatarConstant* cst = alloc.Construct<AvatarConstant>(); + + cst->m_AvatarSkeleton = skeleton; + cst->m_AvatarSkeletonPose = skeletonPose; + cst->m_DefaultPose = defaultPose; + cst->m_Human = human; + cst->m_RootMotionSkeleton = rootMotionSkeleton; + cst->m_RootMotionBoneIndex = rootMotionIndex; + cst->m_RootMotionBoneX = rootMotionX; + + if(human != 0) + { + cst->m_HumanSkeletonIndexCount = human->m_Skeleton->m_Count; + cst->m_HumanSkeletonIndexArray = alloc.ConstructArray<mecanim::int32_t>(cst->m_HumanSkeletonIndexCount); + skeleton::SkeletonBuildIndexArray(cst->m_HumanSkeletonIndexArray.Get(),human->m_Skeleton.Get(),skeleton); + cst->m_HumanSkeletonReverseIndexCount = cst->m_AvatarSkeleton->m_Count; + cst->m_HumanSkeletonReverseIndexArray = alloc.ConstructArray<mecanim::int32_t>(cst->m_HumanSkeletonReverseIndexCount); + skeleton::SkeletonBuildReverseIndexArray(cst->m_HumanSkeletonReverseIndexArray.Get(),cst->m_HumanSkeletonIndexArray.Get(),human->m_Skeleton.Get(),skeleton); + } + else if(rootMotionIndex != -1) + { + cst->m_RootMotionSkeletonIndexCount = rootMotionSkeleton->m_Count; + cst->m_RootMotionSkeletonIndexArray = alloc.ConstructArray<mecanim::int32_t>(cst->m_RootMotionSkeletonIndexCount); + skeleton::SkeletonBuildIndexArray(cst->m_RootMotionSkeletonIndexArray.Get(),cst->m_RootMotionSkeleton.Get(),skeleton); + } + + return cst; + } + + void DestroyAvatarConstant(AvatarConstant* constant, memory::Allocator& alloc) + { + if(constant) + { + alloc.Deallocate(constant->m_HumanSkeletonIndexArray); + alloc.Deallocate(constant->m_HumanSkeletonReverseIndexArray); + alloc.Deallocate(constant->m_RootMotionSkeletonIndexArray); + alloc.Deallocate(constant); + } + } + + void ClearAvatarConstant(AvatarConstant * constant, memory::Allocator& alloc) + { + + } + + AvatarInput* CreateAvatarInput(AvatarConstant const* constant, memory::Allocator& alloc) + { + SETPROFILERLABEL(AvatarInput); + AvatarInput* input = alloc.Construct<AvatarInput>(); + + return input; + } + + void DestroyAvatarInput(AvatarInput* input, memory::Allocator& alloc) + { + if(input) + { + alloc.Deallocate(input->m_GotoStateInfos); + alloc.Deallocate(input); + } + } + + AvatarMemory* CreateAvatarMemory(AvatarConstant const* constant, memory::Allocator& alloc) + { + SETPROFILERLABEL(AvatarMemory); + AvatarMemory* mem = alloc.Construct<AvatarMemory>(); + + return mem; + } + + void DestroyAvatarMemory(AvatarMemory* memory, memory::Allocator& alloc) + { + if(memory) + { + alloc.Deallocate(memory); + } + } + + AvatarWorkspace* CreateAvatarWorkspace(AvatarConstant const* constant, memory::Allocator& alloc) + { + SETPROFILERLABEL(AvatarWorkspace); + AvatarWorkspace* ws = alloc.Construct<AvatarWorkspace>(); + + if(constant->isHuman()) + { + if(!constant->m_AvatarSkeleton.IsNull()) + { + ws->m_BodySkeletonPoseWs = skeleton::CreateSkeletonPose(constant->m_Human->m_Skeleton.Get(), alloc); + ws->m_BodySkeletonPoseWsA = skeleton::CreateSkeletonPose(constant->m_Human->m_Skeleton.Get(), alloc); + ws->m_BodySkeletonPoseWsB = skeleton::CreateSkeletonPose(constant->m_Human->m_Skeleton.Get(), alloc); + + ws->m_HumanPoseWs = alloc.Construct<human::HumanPose>(); + } + } + else if(constant->m_RootMotionBoneIndex != -1) + { + if(!constant->m_RootMotionSkeleton.IsNull()) + { + ws->m_RootMotionSkeletonPoseWsA = skeleton::CreateSkeletonPose(constant->m_RootMotionSkeleton.Get(), alloc); + ws->m_RootMotionSkeletonPoseWsB = skeleton::CreateSkeletonPose(constant->m_RootMotionSkeleton.Get(), alloc); + } + } + + return ws; + } + + void DestroyAvatarWorkspace(AvatarWorkspace* workspace, memory::Allocator& alloc) + { + if(workspace) + { + skeleton::DestroySkeletonPose(workspace->m_BodySkeletonPoseWsB, alloc); + skeleton::DestroySkeletonPose(workspace->m_BodySkeletonPoseWsA, alloc); + skeleton::DestroySkeletonPose(workspace->m_BodySkeletonPoseWs, alloc); + skeleton::DestroySkeletonPose(workspace->m_RootMotionSkeletonPoseWsB, alloc); + skeleton::DestroySkeletonPose(workspace->m_RootMotionSkeletonPoseWsA, alloc); + alloc.Deallocate(workspace->m_HumanPoseWs); + + alloc.Deallocate(workspace); + } + } + + AvatarOutput* CreateAvatarOutput(AvatarConstant const* constant, bool hasTransformHierarchy, memory::Allocator& alloc) + { + SETPROFILERLABEL(AvatarOutput); + AvatarOutput* out = alloc.Construct<AvatarOutput>(); + + if(hasTransformHierarchy) + { + if(!constant->m_Human.IsNull()) + { + if(!constant->m_AvatarSkeleton.IsNull() && constant->m_AvatarSkeleton->m_Count > 0) + { + out-> m_SkeletonPoseOutput = skeleton::CreateSkeletonPose(constant->m_AvatarSkeleton.Get(), alloc); + } + } + } + else + { + if(!constant->m_AvatarSkeleton.IsNull() && constant->m_AvatarSkeleton->m_Count > 0) + { + out->m_SkeletonPoseOutput = skeleton::CreateSkeletonPose(constant->m_AvatarSkeleton.Get(), alloc); + } + } + + if(constant->m_RootMotionBoneIndex != -1 || !constant->m_Human.IsNull()) + { + out->m_MotionOutput = alloc.Construct<MotionOutput>(); + + if(!constant->m_Human.IsNull()) + { + out->m_HumanPoseBaseOutput = alloc.Construct<human::HumanPose>(); + out->m_HumanPoseOutput = alloc.Construct<human::HumanPose>(); + } + } + + return out; + } + + void DestroyAvatarOutput(AvatarOutput* output, memory::Allocator& alloc) + { + if(output) + { + if(output->m_DynamicValuesOutput) DestroyValueArray(output->m_DynamicValuesOutput,alloc); + skeleton::DestroySkeletonPose(output->m_SkeletonPoseOutput, alloc); + alloc.Deallocate(output->m_MotionOutput); + alloc.Deallocate(output->m_HumanPoseOutput); + alloc.Deallocate(output->m_HumanPoseBaseOutput); + alloc.Deallocate(output); + } + } + + void ControllerConstant::InitializeClass() + { + RegisterAllowNameConversion("ControllerConstant", "m_HumanLayerCount", "m_LayerCount"); + RegisterAllowNameConversion("ControllerConstant", "m_HumanLayerArray", "m_LayerArray"); + } + + ControllerConstant* CreateControllerConstant( uint32_t layerCount, LayerConstant** layerArray, + uint32_t stateMachineCount, statemachine::StateMachineConstant** stateMachineConstantArray, + ValueArrayConstant* values, ValueArray* defaultValues, + memory::Allocator& alloc) + { + SETPROFILERLABEL(ControllerConstant); + + ControllerConstant* controller = alloc.Construct<ControllerConstant>(); + + controller->m_LayerCount = layerCount; + controller->m_LayerArray = alloc.ConstructArray< OffsetPtr<LayerConstant> >(controller->m_LayerCount); + + uint32_t i; + for(i=0;i<controller->m_LayerCount;i++) + controller->m_LayerArray[i] = layerArray[i]; + + controller->m_StateMachineCount = stateMachineCount; + controller->m_StateMachineArray = alloc.ConstructArray< OffsetPtr<statemachine::StateMachineConstant> >(controller->m_StateMachineCount); + + for(i=0;i<controller->m_StateMachineCount;i++) + controller->m_StateMachineArray[i] = stateMachineConstantArray[i]; + + controller->m_Values = values; + controller->m_DefaultValues = defaultValues; + + return controller; + } + + void DestroyControllerConstant(ControllerConstant* controller, memory::Allocator& alloc) + { + if(controller) + { + alloc.Deallocate(controller->m_LayerArray); + alloc.Deallocate(controller->m_StateMachineArray); + alloc.Deallocate(controller); + } + } + + + LayerConstant* CreateLayerConstant(mecanim::uint32_t stateMachineIndex, mecanim::uint32_t motionSetIndex, memory::Allocator& alloc) + { + SETPROFILERLABEL(LayerConstant); + + LayerConstant* cst = alloc.Construct<LayerConstant>(); + cst->m_StateMachineIndex = stateMachineIndex; + cst->m_StateMachineMotionSetIndex = motionSetIndex; + return cst; + } + + void DestroyLayerConstant(LayerConstant* constant, memory::Allocator& alloc) + { + if(constant) + { + alloc.Deallocate(constant); + } + } + + ControllerMemory* CreateControllerMemory(ControllerConstant const* controller, AvatarConstant const *avatar, AnimationSet const *animationSet, const ValueArrayConstant* dynamicValueConstant, memory::Allocator& alloc) + { + SETPROFILERLABEL(ControllerMemory); + + ControllerMemory* mem = alloc.Construct<ControllerMemory>(); + + mem->m_LayerCount = controller->m_LayerCount; + mem->m_StateMachineCount = controller->m_StateMachineCount; + mem->m_StateMachineMemory = alloc.ConstructArray< OffsetPtr<statemachine::StateMachineMemory> >(mem->m_StateMachineCount); + mem->m_InteruptedTransitionsBlendingStateArray = alloc.ConstructArray< BlendingState<false> > (mem->m_LayerCount); + mem->m_LayerWeights = alloc.ConstructArray<float>(mem->m_LayerCount); + + mem->m_Values = CreateValueArray( controller->m_Values.Get(), alloc); + ValueArrayCopy(controller->m_DefaultValues.Get(), mem->m_Values.Get()); + + for(int layerIter = 0; layerIter < controller->m_LayerCount; layerIter++) + { + mem->m_LayerWeights[layerIter] = controller->m_LayerArray[layerIter]->m_DefaultWeight; + + mem->m_InteruptedTransitionsBlendingStateArray[layerIter].m_DynamicValuesBlending = mecanim::CreateValueArray(dynamicValueConstant, alloc); + mem->m_InteruptedTransitionsBlendingStateArray[layerIter].m_DynamicValuesBlendingMask = mecanim::CreateValueArrayMask(dynamicValueConstant, alloc); + + if(avatar->isHuman()) + { + mem->m_InteruptedTransitionsBlendingStateArray[layerIter].m_MotionBlending = alloc.Construct<mecanim::animation::MotionOutput>(); + mem->m_InteruptedTransitionsBlendingStateArray[layerIter].m_HumanPoseBlending = alloc.Construct<mecanim::human::HumanPose>(); + } + else if(avatar->m_RootMotionBoneIndex != -1) + { + mem->m_InteruptedTransitionsBlendingStateArray[layerIter].m_MotionBlending = alloc.Construct<mecanim::animation::MotionOutput>(); + mem->m_InteruptedTransitionsBlendingStateArray[layerIter].m_HumanPoseBlending = 0; + } + else + { + mem->m_InteruptedTransitionsBlendingStateArray[layerIter].m_MotionBlending = 0; + mem->m_InteruptedTransitionsBlendingStateArray[layerIter].m_HumanPoseBlending = 0; + } + } + + for(int stateMachineIter = 0; stateMachineIter < mem->m_StateMachineCount; stateMachineIter++) + { + mem->m_StateMachineMemory[stateMachineIter] = statemachine::CreateStateMachineMemory(controller->m_StateMachineArray[stateMachineIter].Get(), alloc); + } + + return mem; + } + + void DestroyControllerMemory(ControllerMemory* controllerMemory, memory::Allocator& alloc) + { + if(controllerMemory) + { + for(int layerIter = 0; layerIter < controllerMemory->m_LayerCount; layerIter++) + { + if(!controllerMemory->m_InteruptedTransitionsBlendingStateArray[layerIter].m_DynamicValuesBlending.IsNull()) DestroyValueArray(controllerMemory->m_InteruptedTransitionsBlendingStateArray[layerIter].m_DynamicValuesBlending.Get(),alloc); + if(!controllerMemory->m_InteruptedTransitionsBlendingStateArray[layerIter].m_DynamicValuesBlendingMask.IsNull()) DestroyValueArrayMask(controllerMemory->m_InteruptedTransitionsBlendingStateArray[layerIter].m_DynamicValuesBlendingMask.Get(),alloc); + if(!controllerMemory->m_InteruptedTransitionsBlendingStateArray[layerIter].m_MotionBlending.IsNull()) alloc.Deallocate(controllerMemory->m_InteruptedTransitionsBlendingStateArray[layerIter].m_MotionBlending.Get()); + if(!controllerMemory->m_InteruptedTransitionsBlendingStateArray[layerIter].m_HumanPoseBlending.IsNull()) alloc.Deallocate(controllerMemory->m_InteruptedTransitionsBlendingStateArray[layerIter].m_HumanPoseBlending.Get()); + } + + for(int smIter = 0; smIter < controllerMemory->m_StateMachineCount; smIter++) + { + statemachine::DestroyStateMachineMemory(controllerMemory->m_StateMachineMemory[smIter].Get(), alloc); + } + + DestroyValueArray(controllerMemory->m_Values.Get(), alloc); + alloc.Deallocate(controllerMemory->m_LayerWeights); + alloc.Deallocate(controllerMemory->m_InteruptedTransitionsBlendingStateArray); + alloc.Deallocate(controllerMemory->m_StateMachineMemory); + + alloc.Deallocate(controllerMemory); + } + } + + BlendingState<true>* CreateBlendingState(uint32_t size, bool createMotionState, bool createHumanPose, memory::Allocator& alloc) + { + SETPROFILERLABEL(BlendingState_true); + + BlendingState<true>* blendingState = alloc.Construct< BlendingState<true> >(); + + blendingState->m_DynamicValuesBlending = alloc.ConstructArray<ValueArray*>(size); + blendingState->m_BlendFactor = alloc.ConstructArray<float>(size); + + memset(&blendingState->m_DynamicValuesBlending[0], 0, sizeof(ValueArray*) * size); + memset(&blendingState->m_BlendFactor[0], 0, sizeof(float) * size); + + if(createMotionState || createHumanPose) + { + blendingState->m_MotionBlending = alloc.ConstructArray<MotionOutput*>(size); + memset(&blendingState->m_MotionBlending[0], 0, sizeof(MotionOutput*) * size); + } + + if(createHumanPose) + { + blendingState->m_HumanPoseBlending = alloc.ConstructArray<human::HumanPose*>(size); + memset(&blendingState->m_HumanPoseBlending[0], 0, sizeof(human::HumanPose*) * size); + } + + blendingState->m_Size = size; + + return blendingState; + } + + void DestroyBlendingState(BlendingState<true>* blendingState, memory::Allocator& alloc) + { + if(blendingState) + { + for(uint32_t i=0;i<blendingState->m_Size;++i) + { + DestroyValueArray(blendingState->m_DynamicValuesBlending[i],alloc); + if(blendingState->m_MotionBlending) alloc.Deallocate(blendingState->m_MotionBlending[i]); + if(blendingState->m_HumanPoseBlending) alloc.Deallocate(blendingState->m_HumanPoseBlending[i]); + } + + alloc.Deallocate(blendingState->m_DynamicValuesBlending); + alloc.Deallocate(blendingState->m_MotionBlending); + alloc.Deallocate(blendingState->m_HumanPoseBlending); + alloc.Deallocate(blendingState->m_BlendFactor); + + alloc.Deallocate(blendingState); + } + } + + ControllerWorkspace *CreateControllerWorkspace(ControllerConstant const* controller, AvatarConstant const *avatar, AnimationSet const *animationSet, const ValueArrayConstant* dynamicValueConstant, memory::Allocator& alloc) + { + SETPROFILERLABEL(ControllerWorkspace); + bool forRootMotion = avatar->m_RootMotionBoneIndex != -1; + bool forHuman = avatar->isHuman(); + + ControllerWorkspace* ws = alloc.Construct<ControllerWorkspace>(); + + ws->m_StateMachineOutput = alloc.ConstructArray<statemachine::StateMachineOutput*>(controller->m_StateMachineCount); + ws->m_StateMachineWorkspace = alloc.ConstructArray<statemachine::StateMachineWorkspace*>(controller->m_StateMachineCount); + ws->m_StateMachineCount = controller->m_StateMachineCount; + + uint32_t i; + uint32_t maxMotionSet = 0; + for(i = 0 ; i < controller->m_StateMachineCount; ++i) + { + maxMotionSet = math::maximum<uint32_t>(maxMotionSet, controller->m_StateMachineArray[i]->m_MotionSetCount); + } + ws->m_MotionSetTimingWeightArray = alloc.ConstructArray<float>(maxMotionSet); + + for(int stateMachineIter = 0; stateMachineIter < ws->m_StateMachineCount; stateMachineIter++) + { + ws->m_StateMachineOutput[stateMachineIter] = statemachine::CreateStateMachineOutput(controller->m_StateMachineArray[stateMachineIter].Get(), animationSet->m_MaxBlendState, alloc); + ws->m_StateMachineWorkspace[stateMachineIter] = statemachine::CreateStateMachineWorkspace(controller->m_StateMachineArray[stateMachineIter].Get(), animationSet->m_MaxBlendState, alloc); + } + + ws->m_BlendingState = CreateBlendingState(animationSet->m_MaxBlendState, forRootMotion, forHuman, alloc); + + for(int blendStateIter = 0; blendStateIter < ws->m_BlendingState->m_Size; blendStateIter++) + { + ws->m_BlendingState->m_DynamicValuesBlending[blendStateIter] = mecanim::CreateValueArray(dynamicValueConstant, alloc); + + if(forHuman) + { + { + SETPROFILERLABEL(MotionOutput); + ws->m_BlendingState->m_MotionBlending[blendStateIter] = alloc.Construct<mecanim::animation::MotionOutput>(); + } + { + SETPROFILERLABEL(HumanPose); + ws->m_BlendingState->m_HumanPoseBlending[blendStateIter] = alloc.Construct<mecanim::human::HumanPose>(); + } + } + else if(forRootMotion) + { + ws->m_BlendingState->m_MotionBlending[blendStateIter] = alloc.Construct<mecanim::animation::MotionOutput>(); + } + } + + ws->m_BlendingStateWs.m_DynamicValuesBlending = mecanim::CreateValueArray(dynamicValueConstant, alloc); + + if (forHuman) + { + ws->m_BlendingStateWs.m_MotionBlending = alloc.Construct<animation::MotionOutput>(); + ws->m_BlendingStateWs.m_HumanPoseBlending = alloc.Construct<human::HumanPose>(); + } + else if (forRootMotion) + { + ws->m_BlendingStateWs.m_MotionBlending = alloc.Construct<mecanim::animation::MotionOutput>(); + } + + ws->m_ValueArrayStart = CreateValueArray(dynamicValueConstant, alloc); + ws->m_ValueArrayStop = CreateValueArray(dynamicValueConstant, alloc); + ws->m_BlendingClipArray = alloc.ConstructArray<mecanim::animation::BlendingClip>(controller->m_LayerCount*animationSet->m_MaxBlendState); + + ws->m_ReadMask = CreateValueArrayMask(dynamicValueConstant, alloc); + ws->m_BlendMask = CreateValueArrayMask(dynamicValueConstant, alloc); + ws->m_DefaultMask = CreateValueArrayMask(dynamicValueConstant, alloc); + + return ws; + } + + void DestroyControllerWorkspace(ControllerWorkspace* controllerWorkspace, memory::Allocator& alloc) + { + if(controllerWorkspace) + { + uint32_t i; + for(i=0;i<controllerWorkspace->m_StateMachineCount;i++) + { + DestroyStateMachineOutput(controllerWorkspace->m_StateMachineOutput[i], alloc); + DestroyStateMachineWorkspace(controllerWorkspace->m_StateMachineWorkspace[i], alloc); + } + + DestroyBlendingState(controllerWorkspace->m_BlendingState, alloc); + + DestroyValueArray(controllerWorkspace->m_BlendingStateWs.m_DynamicValuesBlending.Get(), alloc); + alloc.Deallocate(controllerWorkspace->m_BlendingStateWs.m_MotionBlending); + alloc.Deallocate(controllerWorkspace->m_BlendingStateWs.m_HumanPoseBlending); + + alloc.Deallocate(controllerWorkspace->m_BlendingClipArray); + + alloc.Deallocate(controllerWorkspace->m_MotionSetTimingWeightArray); + + alloc.Deallocate(controllerWorkspace->m_StateMachineWorkspace); + alloc.Deallocate(controllerWorkspace->m_StateMachineOutput); + DestroyValueArray(controllerWorkspace->m_ValueArrayStart,alloc); + DestroyValueArray(controllerWorkspace->m_ValueArrayStop,alloc); + + DestroyValueArrayMask(controllerWorkspace->m_ReadMask,alloc); + DestroyValueArrayMask(controllerWorkspace->m_BlendMask,alloc); + DestroyValueArrayMask(controllerWorkspace->m_DefaultMask,alloc); + + alloc.Deallocate(controllerWorkspace); + } + } + + void UpdateLeafNodeDuration(const ControllerConstant &controllerConstant, const AnimationSet &animationSet, ControllerMemory &controllerMemory) + { + for(int layerIter = 0; layerIter < controllerConstant.m_LayerCount; layerIter++) + { + mecanim::uint32_t stateMachineIndex = controllerConstant.m_LayerArray[layerIter]->m_StateMachineIndex; + mecanim::uint32_t motionSetIndex = controllerConstant.m_LayerArray[layerIter]->m_StateMachineMotionSetIndex; + + const statemachine::StateMachineConstant& stateMachineConstant = *controllerConstant.m_StateMachineArray[stateMachineIndex].Get(); + + for(int stateIter = 0; stateIter < stateMachineConstant.m_StateConstantCount; stateIter++) + { + statemachine::StateConstant const &stateConstant = *stateMachineConstant.m_StateConstantArray[stateIter].Get(); + + for(int leafIter = 0; leafIter < stateConstant.m_LeafInfoArray[motionSetIndex].m_Count ; leafIter++) + { + int clipIndex = leafIter + stateConstant.m_LeafInfoArray[motionSetIndex].m_IndexOffset; + + AnimationSet::Clip& setClip = animationSet.m_ClipConstant[layerIter][clipIndex]; + + statemachine::GetBlendTreeMemory(stateConstant,*controllerMemory.m_StateMachineMemory[stateMachineIndex]->m_StateMemoryArray[stateIter],motionSetIndex)->m_NodeDurationArray[leafIter] = setClip.m_Clip != 0 ? (setClip.m_Clip->m_StopTime - setClip.m_Clip->m_StartTime) : 0.0f; + } + } + } + } + + void SetIKOnFeet(bool left,AvatarConstant const &avatar, const AvatarInput &input, AvatarMemory &memory, AvatarWorkspace &workspace, AvatarOutput &output) + { + float deltaTime = input.m_DeltaTime; + bool stabilizeFeet = input.m_StabilizeFeet; + + math::xform avatarX = memory.m_AvatarX; + + math::float1 scale(avatar.m_Human->m_Scale); + + math::xform ddx = math::xformInvMulNS(avatarX,workspace.m_AvatarX); + float dSpeedT = math::length(ddx.t).tofloat() / deltaTime; + float dSpeedQ = math::length(math::doubleAtan(math::quat2Qtan(ddx.q))).tofloat() /deltaTime; + + int32_t footIndex = left ? human::kLeftFoot : human::kRightFoot; + int32_t goalIndex = left ? human::kLeftFootGoal : human::kRightFootGoal; + + output.m_HumanPoseOutput->m_GoalArray[goalIndex].m_WeightT = 1; + output.m_HumanPoseOutput->m_GoalArray[goalIndex].m_WeightR = 1; + + if(stabilizeFeet && footIndex != -1 && memory.m_FirstEval==0) + { + float speedT = left ? workspace.m_LeftFootSpeedT : workspace.m_RightFootSpeedT; + float speedQ = left ? workspace.m_LeftFootSpeedQ : workspace.m_RightFootSpeedQ; + + speedT += dSpeedT; + speedQ += dSpeedQ; + + math::xform &footX = left ? memory.m_LeftFootX : memory.m_RightFootX; + + math::xform footGoalX0 = output.m_HumanPoseOutput->m_GoalArray[goalIndex].m_X; + math::xform footGoalX = footGoalX0; + + math::xform goalDX = math::xformInvMulNS(footX,footGoalX); + + float goalDTLen = math::length(goalDX.t).tofloat(); + + if(goalDTLen > 0) + { + float goalSpeedT = goalDTLen / deltaTime; + speedT = math::cond(speedT > 0.1f, speedT, 0.0f); + speedT = math::cond(speedT > 1.0f, 2.0f * speedT, speedT); + float goalSpeedTClamp = math::minimum(goalSpeedT,speedT); + goalDX.t = goalDX.t * math::float1(goalSpeedTClamp/goalSpeedT); + } + + float goalDQLen = math::length(math::doubleAtan(math::quat2Qtan(goalDX.q))).tofloat(); + + if(goalDQLen > 0) + { + float goalSpeedQ = goalDQLen / deltaTime; + speedQ = math::cond(speedQ > math::radians(10.0f), speedQ, 0.0f); + speedQ = math::cond(speedQ > math::radians(100.0f), 2.0f * speedQ, speedQ); + float goalSpeedQClamp = math::minimum(goalSpeedQ,speedQ); + goalDX.q = math::qtan2Quat(math::halfTan(math::doubleAtan(math::quat2Qtan(goalDX.q))*math::float1(goalSpeedQClamp/goalSpeedQ))); + } + + footX = math::xformMul(footX,goalDX); + + output.m_HumanPoseOutput->m_GoalArray[goalIndex].m_X = footX; + } + + math::float4 feetSpacing = math::quatMulVec(output.m_HumanPoseOutput->m_GoalArray[goalIndex].m_X.q, math::float4(0,0,(left?-1:1)*avatar.m_Human->m_Scale*avatar.m_Human->m_FeetSpacing,0)); + feetSpacing.y() = math::float1::zero(); + + output.m_HumanPoseOutput->m_GoalArray[goalIndex].m_X.t += avatarX.s * feetSpacing; + } + + void EvaluateAvatarSM( AvatarConstant const* constant, + AvatarInput const* input, + AvatarOutput * output, + AvatarMemory * memory, + AvatarWorkspace * workspace, + ControllerConstant const* controllerConstant) + { + if(controllerConstant) + { + workspace->m_IKOnFeet = false; + + uint32_t i; + + for(i = 0; i < controllerConstant->m_StateMachineCount; i++) + { + statemachine::StateMachineInput stateMachineInput; + stateMachineInput.m_MotionSetTimingWeightArray = workspace->m_ControllerWorkspace->m_MotionSetTimingWeightArray; + + for(int layerIndex = 0; layerIndex < controllerConstant->m_LayerCount; layerIndex++) + { + const uint32_t stateMachineIndex = controllerConstant->m_LayerArray[layerIndex]->m_StateMachineIndex; + const uint32_t motionSetIndex = controllerConstant->m_LayerArray[layerIndex]->m_StateMachineMotionSetIndex; + + if(stateMachineIndex != DISABLED_SYNCED_LAYER_IN_NON_PRO) + { + if(i == stateMachineIndex ) + { + if(motionSetIndex == 0) + stateMachineInput.m_GotoStateInfo = &input->m_GotoStateInfos[layerIndex]; + + if( controllerConstant->m_LayerArray[layerIndex]->m_SyncedLayerAffectsTiming || motionSetIndex == 0 ) + stateMachineInput.m_MotionSetTimingWeightArray[motionSetIndex] = motionSetIndex == 0 ? 1 : memory->m_ControllerMemory->m_LayerWeights[layerIndex]; + else + stateMachineInput.m_MotionSetTimingWeightArray[motionSetIndex] = 0 ; + } + } + } + + + stateMachineInput.m_DeltaTime = input->m_DeltaTime; + + stateMachineInput.m_Values = memory->m_ControllerMemory->m_Values.Get(); + workspace->m_ControllerWorkspace->m_StateMachineWorkspace[i]->m_ValuesConstant = const_cast<mecanim::ValueArrayConstant *>(controllerConstant->m_Values.Get()); + + statemachine::EvaluateStateMachine( controllerConstant->m_StateMachineArray[i].Get(), + &stateMachineInput, + workspace->m_ControllerWorkspace->m_StateMachineOutput[i], + memory->m_ControllerMemory->m_StateMachineMemory[i].Get(), + workspace->m_ControllerWorkspace->m_StateMachineWorkspace[i]); + + workspace->m_IKOnFeet |= workspace->m_ControllerWorkspace->m_StateMachineOutput[i]->m_Left.m_IKOnFeet; + workspace->m_IKOnFeet |= workspace->m_ControllerWorkspace->m_StateMachineOutput[i]->m_Right.m_IKOnFeet; + } + } + } + + void AdjustPoseForMotion(ControllerBindingConstant const* controllerBindingConstant, math::xform const &motionX, ValueArray &values, skeleton::SkeletonPose &pose, skeleton::SkeletonPose &poseWs) + { + AvatarConstant const *constant = controllerBindingConstant->m_Avatar; + + int lastIndex = (IS_CONTENT_NEWER_OR_SAME(kUnityVersion4_3_a1)) ? constant->m_RootMotionSkeleton->m_Count-1 : constant->m_RootMotionBoneIndex ; + + SkeletonPoseFromValue(*constant->m_RootMotionSkeleton.Get(), *constant->m_AvatarSkeletonPose.Get(), values, controllerBindingConstant->m_SkeletonTQSMap, constant->m_RootMotionSkeletonIndexArray.Get(), pose,lastIndex, 0); + + skeleton::SkeletonPoseComputeGlobal(constant->m_RootMotionSkeleton.Get(),&pose,&poseWs); + pose.m_X[0] = motionX; + + if(constant->m_RootMotionBoneIndex > 0) + skeleton::SkeletonPoseComputeGlobal(constant->m_RootMotionSkeleton.Get(),&pose,&poseWs,lastIndex-1,0); + + skeleton::SkeletonPoseComputeLocal(constant->m_RootMotionSkeleton.Get(),&poseWs,&pose,lastIndex,lastIndex); + pose.m_X[0] = math::xformIdentity(); + + ValueFromSkeletonPose(*constant->m_RootMotionSkeleton.Get(), pose, controllerBindingConstant->m_SkeletonTQSMap, constant->m_RootMotionSkeletonIndexArray.Get(), values, lastIndex, 0); + } + + void ComputeRootMotion(ControllerBindingConstant const* controllerBindingConstant, MotionOutput const &motionOutput, ValueArray &values, AvatarWorkspace *workspace) + { + AdjustPoseForMotion(controllerBindingConstant, motionOutput.m_MotionStartX, *workspace->m_ControllerWorkspace->m_ValueArrayStart, *workspace->m_RootMotionSkeletonPoseWsA, *workspace->m_RootMotionSkeletonPoseWsB); + AdjustPoseForMotion(controllerBindingConstant, motionOutput.m_MotionStopX, *workspace->m_ControllerWorkspace->m_ValueArrayStop, *workspace->m_RootMotionSkeletonPoseWsA, *workspace->m_RootMotionSkeletonPoseWsB); + AdjustPoseForMotion(controllerBindingConstant, motionOutput.m_MotionX, values, *workspace->m_RootMotionSkeletonPoseWsA, *workspace->m_RootMotionSkeletonPoseWsB); + } + + void EvaluateBlendNode( ControllerBindingConstant const* controllerBindingConstant, + AvatarInput const* input, + int32_t layerIndex, + bool additive, + bool left, + uint32_t &clipCount, + ControllerMemory *memory, + AvatarWorkspace * workspace, + AvatarOutput * avatarOutput, + AnimationSetMemory* animationSetMemory, + float *blendFactor) + { + AvatarConstant const *constant = controllerBindingConstant->m_Avatar; + ControllerConstant const *controllerConstant = controllerBindingConstant->m_Controller; + AnimationSet const *animationSet = controllerBindingConstant->m_AnimationSet; + + bool hasRootMotion = constant->m_RootMotionBoneIndex != -1; + bool isHuman = constant->isHuman(); + + uint32_t stateMachineIndex = controllerConstant->m_LayerArray[layerIndex]->m_StateMachineIndex; + uint32_t motionSetIndex = controllerConstant->m_LayerArray[layerIndex]->m_StateMachineMotionSetIndex; + + if(stateMachineIndex == DISABLED_SYNCED_LAYER_IN_NON_PRO) + return ; + + statemachine::StateMachineOutput& stateMachineOutput = *workspace->m_ControllerWorkspace->m_StateMachineOutput[stateMachineIndex]; + statemachine::BlendNode &blendNode = left ? stateMachineOutput.m_Left : stateMachineOutput.m_Right; + + animation::ClipOutput* clipOutput = animationSetMemory->m_ClipOutput; + + for(int32_t outputIter=0; outputIter < blendNode.m_BlendNodeLayer[motionSetIndex].m_OutputCount; outputIter++) + { + uint32_t index = blendNode.m_BlendNodeLayer[motionSetIndex].m_OutputIndexArray[outputIter]; + + AnimationSet::Clip& setClip = animationSet->m_ClipConstant[layerIndex][index]; + + if (setClip.m_Clip) + { + animation::ClipMuscleConstant const& muscleConstant = *setClip.m_Clip; + + if(hasRootMotion || isHuman) + { + ClipMuscleInput muscleIn; + muscleIn.m_Time = blendNode.m_CurrentTime; + muscleIn.m_PreviousTime = blendNode.m_PreviousTime; + muscleIn.m_Mirror = blendNode.m_BlendNodeLayer[motionSetIndex].m_MirrorArray[outputIter]; + muscleIn.m_CycleOffset = blendNode.m_BlendNodeLayer[motionSetIndex].m_CycleOffsetArray[outputIter]; + muscleIn.m_Reverse = blendNode.m_BlendNodeLayer[motionSetIndex].m_ReverseArray[outputIter]; + + animation::MotionOutput* motionOutput = workspace->m_ControllerWorkspace->m_BlendingState->m_MotionBlending[clipCount]; + + if(isHuman) EvaluateClipMusclePrevTime(muscleConstant,muscleIn,*motionOutput,*animationSetMemory->m_ClipMemory[layerIndex][index]); + else if(hasRootMotion) EvaluateClipRootMotionDeltaX(muscleConstant,muscleIn,*motionOutput,*animationSetMemory->m_ClipMemory[layerIndex][index]); + } + + ValueArray &values = avatarOutput != 0 && clipCount == 0 ? *avatarOutput->m_DynamicValuesOutput : *workspace->m_ControllerWorkspace->m_BlendingState->m_DynamicValuesBlending[clipCount]; + + ClipInput in; + float timeInt; + float normalizedTime = 0; + in.m_Time = ComputeClipTime(blendNode.m_CurrentTime,muscleConstant.m_StartTime,muscleConstant.m_StopTime, + muscleConstant.m_CycleOffset+blendNode.m_BlendNodeLayer[motionSetIndex].m_CycleOffsetArray[outputIter], + muscleConstant.m_LoopTime, + blendNode.m_BlendNodeLayer[motionSetIndex].m_ReverseArray[outputIter], + normalizedTime,timeInt); + + ValueArrayMask& readMask = *workspace->m_ControllerWorkspace->m_ReadMask; + ValueArrayMask& blendMask = *workspace->m_ControllerWorkspace->m_BlendMask; + + EvaluateClip(muscleConstant.m_Clip.Get(),&in,animationSetMemory->m_ClipMemory[layerIndex][index], clipOutput); + ValuesFromClip(*controllerBindingConstant->m_DynamicValuesDefault, muscleConstant, *clipOutput, setClip.m_Bindings, animationSet->m_IntegerRemapStride, values, readMask); + + OrValueMask(&blendMask,&readMask); + + if(muscleConstant.m_LoopTime && muscleConstant.m_LoopBlend) + { + DeltasFromClip(muscleConstant, setClip.m_Bindings, readMask, *workspace->m_ControllerWorkspace->m_ValueArrayStart, *workspace->m_ControllerWorkspace->m_ValueArrayStop); + } + + if(hasRootMotion || isHuman) + { + ClipMuscleInput muscleIn; + + muscleIn.m_Time = blendNode.m_CurrentTime; + muscleIn.m_PreviousTime = blendNode.m_PreviousTime; + + animation::MotionOutput* motionOutput = workspace->m_ControllerWorkspace->m_BlendingState->m_MotionBlending[clipCount]; + + if(isHuman) + { + muscleIn.m_TargetIndex = input->m_TargetIndex; + muscleIn.m_TargetTime = input->m_TargetTime; + muscleIn.m_TargetIndex = muscleIn.m_TargetIndex > int32_t(animation::kTargetReference) ? muscleIn.m_TargetIndex : int32_t(animation::kTargetReference); + muscleIn.m_TargetIndex = muscleIn.m_TargetIndex < int32_t(animation::kTargetRightHand) ? muscleIn.m_TargetIndex : int32_t(animation::kTargetRightHand); + + muscleIn.m_Mirror = blendNode.m_BlendNodeLayer[motionSetIndex].m_MirrorArray[outputIter]; + muscleIn.m_CycleOffset = blendNode.m_BlendNodeLayer[motionSetIndex].m_CycleOffsetArray[outputIter]; + muscleIn.m_Reverse = blendNode.m_BlendNodeLayer[motionSetIndex].m_ReverseArray[outputIter]; + + human::HumanPose *humanPose = workspace->m_ControllerWorkspace->m_BlendingState->m_HumanPoseBlending[clipCount]; + EvaluateClipMuscle(muscleConstant,muscleIn,clipOutput->m_Values,*motionOutput,*humanPose,*animationSetMemory->m_ClipMemory[layerIndex][index]); + + if(additive) + { + // put goals in same space as GetHumanPose + // @todo check with bob why referential spaces are different + for(int i = 0; i < human::kLastGoal; i++) + humanPose->m_GoalArray[i].m_X = math::xformInvMul(humanPose->m_RootX,humanPose->m_GoalArray[i].m_X); + humanPose->m_RootX = math::xformMul(motionOutput->m_MotionX, humanPose->m_RootX); + + GetHumanPose(muscleConstant,muscleConstant.m_ValueArrayDelta.Get(),*workspace->m_HumanPoseWs); + human::HumanPoseSub(*humanPose,*humanPose,*workspace->m_HumanPoseWs); + } + } + else if(hasRootMotion) + { + ComputeRootMotion(controllerBindingConstant,*motionOutput,values,workspace); + } + + if(animationSet->m_GravityWeightIndex != -1 && readMask.m_FloatValues[animationSet->m_GravityWeightIndex]) + { + values.ReadData(motionOutput->m_GravityWeight, animationSet->m_GravityWeightIndex); + } + else + { + motionOutput->m_GravityWeight = muscleConstant.m_LoopBlendPositionY ? 1 : 0; + } + } + + if(additive) + { + ValueArraySub(*workspace->m_ControllerWorkspace->m_ValueArrayStart,values, &readMask); + } + + if(muscleConstant.m_LoopTime && muscleConstant.m_LoopBlend) + { + ValueArrayLoop(*workspace->m_ControllerWorkspace->m_ValueArrayStart, *workspace->m_ControllerWorkspace->m_ValueArrayStop,values,normalizedTime, readMask); + } + + blendFactor[clipCount] = (left ? ( 1.f - stateMachineOutput.m_BlendFactor) : stateMachineOutput.m_BlendFactor) * blendNode.m_BlendNodeLayer[motionSetIndex].m_OutputBlendArray[outputIter]; + + BlendingClip &blendingClip = workspace->m_ControllerWorkspace->m_BlendingClipArray[workspace->m_ControllerWorkspace->m_BlendingClipCount]; + blendingClip.m_ClipIndex = setClip.m_ClipIndex; + blendingClip.m_LayerIndex = layerIndex; + blendingClip.m_Weight = blendFactor[clipCount]; + blendingClip.m_PrevTime = ComputeClipTime(blendNode.m_PreviousTime,muscleConstant.m_StartTime,muscleConstant.m_StopTime,muscleConstant.m_CycleOffset,muscleConstant.m_LoopTime,blendNode.m_BlendNodeLayer[motionSetIndex].m_ReverseArray[outputIter],normalizedTime,timeInt); + blendingClip.m_Time = in.m_Time; + blendingClip.m_Reverse = blendNode.m_BlendNodeLayer[motionSetIndex].m_ReverseArray[outputIter]; + + workspace->m_ControllerWorkspace->m_BlendingClipCount++; + + clipCount++; + } + } + } + + bool BlendDynamicStates(ValueArray const &valuesDefault, ValueArrayMask const &valueMask, ValueArray **valuesArray, float *blendFactor, int clipCount, ValueArray &valuesOutput, ValueArray *valuesInterupted, ValueArrayMask *valuesMaskInterupted, AvatarOutput *avatarOutput) + { + bool fastCopy = false; + + if(clipCount > 0) + { + if(clipCount == 1 && CompareApproximately(blendFactor[0],1)) + { + if(avatarOutput == 0) + { + ValueArrayCopy(valuesArray[0],&valuesOutput, &valueMask); + } + else + { + fastCopy = true; + } + } + else + { + ValueArray *values0 = valuesArray[0]; + valuesArray[0] = avatarOutput ? avatarOutput->m_DynamicValuesOutput : valuesArray[0]; + ValueArrayBlend(&valuesDefault,&valuesOutput, valuesArray, blendFactor, clipCount, &valueMask); + valuesArray[0] = values0; + } + } + else if(clipCount == 0) + { + ValueArrayCopy(&valuesDefault,&valuesOutput, &valueMask); + } + + if(valuesInterupted != 0) + { + ValueArrayCopy(&valuesOutput,valuesInterupted, &valueMask); + CopyValueMask(valuesMaskInterupted,&valueMask); + } + + return fastCopy; + } + + void BlendMotionStates(MotionOutput **motionArray, float *blendFactor, int clipCount, MotionOutput *motionOutput, MotionOutput *motionInterupted, bool hasRootMotion, bool isHuman, human::HumanPoseMask const &mask) + { + if(clipCount > 0) + { + if(clipCount == 1 && CompareApproximately(blendFactor[0],1)) + { + MotionOutputCopy(motionOutput,motionArray[0], hasRootMotion, isHuman, mask); + } + else + { + MotionOutputBlend(motionOutput,motionArray,blendFactor,clipCount, hasRootMotion, isHuman, mask); + } + } + + if(motionInterupted) + { + MotionOutputCopy(motionInterupted,motionOutput, hasRootMotion, isHuman, mask); + } + } + + void BlendHumanStates(human::HumanPose **poseArray, float *blendFactor, int clipCount, human::HumanPose *poseOut, human::HumanPose *poseInterupted) + { + if(clipCount > 0) + { + if(clipCount == 1 && CompareApproximately(blendFactor[0],1)) + { + human::HumanPoseCopy(*poseOut,*poseArray[0]); + } + else + { + human::HumanPoseBlend(*poseOut,poseArray,blendFactor,clipCount); + } + } + + if(poseInterupted) + { + human::HumanPoseCopy(*poseInterupted,*poseOut); + } + } + + int EvaluateOneLayer( ControllerBindingConstant const* controllerBindingConstant, + AvatarInput const* input, + int32_t layerIndex, + BlendingState<false> * output, + AvatarMemory * memory, + AvatarWorkspace * workspace, + AvatarOutput * avatarOutput, + AnimationSetMemory* animationSetMemory, + bool &fastCopy) + { + AvatarConstant const *constant = controllerBindingConstant->m_Avatar; + ControllerConstant const *controllerConstant = controllerBindingConstant->m_Controller; + + bool hasRootMotion = constant->m_RootMotionBoneIndex != -1; + bool isHuman = constant->isHuman(); + bool additive = controllerConstant->m_LayerArray[layerIndex]->m_LayerBlendingMode == kLayerBlendingModeAdditive; + + uint32_t stateMachineIndex = controllerConstant->m_LayerArray[layerIndex]->m_StateMachineIndex; + + statemachine::StateMachineMemory* stateMachineMemory = memory->m_ControllerMemory->m_StateMachineMemory[stateMachineIndex].Get(); + statemachine::StateMachineOutput& stateMachineOutput = *workspace->m_ControllerWorkspace->m_StateMachineOutput[stateMachineIndex]; + + BlendingState<true>& blendingStates = *workspace->m_ControllerWorkspace->m_BlendingState; + BlendingState<false>& interruptedBlendingState = memory->m_ControllerMemory->m_InteruptedTransitionsBlendingStateArray[layerIndex]; + + uint32_t clipCount = 0; + + SetValueMask(workspace->m_ControllerWorkspace->m_BlendMask,false); + + if(stateMachineMemory->m_InInterruptedTransition) + { + blendingStates.m_BlendFactor[clipCount] = ( 1.f - stateMachineOutput.m_BlendFactor); + + ValueArrayCopy(interruptedBlendingState.m_DynamicValuesBlending.Get(),blendingStates.m_DynamicValuesBlending[clipCount]); + CopyValueMask(workspace->m_ControllerWorkspace->m_ReadMask,interruptedBlendingState.m_DynamicValuesBlendingMask.Get()); + if(hasRootMotion || isHuman) + { + human::HumanPoseMask mask = controllerConstant->m_LayerArray[layerIndex]->m_BodyMask; + MotionOutputCopy(blendingStates.m_MotionBlending[clipCount], interruptedBlendingState.m_MotionBlending.Get(), hasRootMotion, isHuman, mask); + } + if(isHuman) human::HumanPoseCopy(*blendingStates.m_HumanPoseBlending[clipCount],*interruptedBlendingState.m_HumanPoseBlending.Get()); + + clipCount++; + } + else + { + EvaluateBlendNode(controllerBindingConstant,input,layerIndex,additive,true,clipCount, memory->m_ControllerMemory.Get(), workspace, avatarOutput, animationSetMemory, blendingStates.m_BlendFactor); + } + + EvaluateBlendNode(controllerBindingConstant,input,layerIndex,additive,false,clipCount, memory->m_ControllerMemory.Get(), workspace, avatarOutput, animationSetMemory, blendingStates.m_BlendFactor); + + bool isInterruptable = stateMachineMemory->m_InTransition;; // Now with dynamic transition, every transition can be interruptable + + fastCopy = BlendDynamicStates( *controllerBindingConstant->m_DynamicValuesDefault, + *workspace->m_ControllerWorkspace->m_BlendMask, + blendingStates.m_DynamicValuesBlending, + blendingStates.m_BlendFactor, + clipCount, + *output->m_DynamicValuesBlending.Get(), + isInterruptable ? interruptedBlendingState.m_DynamicValuesBlending.Get() : 0, + isInterruptable ? interruptedBlendingState.m_DynamicValuesBlendingMask.Get() : 0, + avatarOutput); + + if(hasRootMotion || isHuman) + { + human::HumanPoseMask mask = controllerConstant->m_LayerArray[layerIndex]->m_BodyMask; + + BlendMotionStates(blendingStates.m_MotionBlending,blendingStates.m_BlendFactor,clipCount,output->m_MotionBlending.Get(),isInterruptable ? interruptedBlendingState.m_MotionBlending.Get() : 0,hasRootMotion,isHuman,mask); + } + + assert(clipCount <= blendingStates.m_Size); + + if(isHuman) + { + BlendHumanStates(blendingStates.m_HumanPoseBlending,blendingStates.m_BlendFactor,clipCount,output->m_HumanPoseBlending.Get(),isInterruptable ? interruptedBlendingState.m_HumanPoseBlending.Get() : 0); + + if(input->m_DeltaTime != 0) + { + float deltaTime = input->m_DeltaTime; + + for(int32_t clipIter = 0; clipIter < clipCount; clipIter++) + { + math::xform leftFootDX = math::xformInvMul(blendingStates.m_MotionBlending[clipIter]->m_PrevLeftFootX,math::xformMul(blendingStates.m_MotionBlending[clipIter]->m_DX,blendingStates.m_HumanPoseBlending[clipIter]->m_GoalArray[human::kLeftFootGoal].m_X)); + math::xform rightFootDX = math::xformInvMul(blendingStates.m_MotionBlending[clipIter]->m_PrevRightFootX,math::xformMul(blendingStates.m_MotionBlending[clipIter]->m_DX,blendingStates.m_HumanPoseBlending[clipIter]->m_GoalArray[human::kRightFootGoal].m_X)); + + workspace->m_LeftFootSpeedT = math::maximum<float>(workspace->m_LeftFootSpeedT,math::length(leftFootDX.t).tofloat()/deltaTime); + workspace->m_LeftFootSpeedQ = math::maximum<float>(workspace->m_LeftFootSpeedQ,math::length(math::doubleAtan(math::quat2Qtan(leftFootDX.q))).tofloat()/deltaTime); + workspace->m_RightFootSpeedT = math::maximum<float>(workspace->m_RightFootSpeedT,math::length(rightFootDX.t).tofloat()/deltaTime); + workspace->m_RightFootSpeedQ = math::maximum<float>(workspace->m_RightFootSpeedQ,math::length(math::doubleAtan(math::quat2Qtan(rightFootDX.q))).tofloat()/deltaTime); + } + } + } + + return clipCount; + } + + void AddMotionLayer(MotionOutput const &motionIn, int index, float weight, bool additive, MotionOutput &motionOut, bool hasRootMotion, bool isHuman, human::HumanPoseMask const &poseMask) + { + if(index == 0) + { + motionOut = motionIn; + } + else if(weight > 0) + { + if(additive) + { + MotionAddAdditiveLayer(&motionOut,&motionIn,weight,hasRootMotion,isHuman,poseMask); + } + else + { + MotionAddOverrideLayer(&motionOut,&motionIn,weight,hasRootMotion,isHuman,poseMask); + } + } + } + + void AddHumanLayer(human::Human const &human, human::HumanPose const &poseIn,human::HumanPoseMask const &poseMask,int index, float weight, bool additive,human::HumanPose &pose,human::HumanPose &poseBase) + { + if(index == 0) + { + human::HumanPoseCopy(poseBase,poseIn, poseMask); + human::HumanPoseCopy(pose,poseIn, poseMask); + } + else if(weight > 0) + { + if(additive) + { + mecanim::human::HumanPoseMask mask = poseMask; + mask.set(mecanim::human::kMaskLeftHand,mask.test(mecanim::human::kMaskLeftHand) && human.m_HasLeftHand); + mask.set(mecanim::human::kMaskRightHand,mask.test(mecanim::human::kMaskRightHand) && human.m_HasRightHand); + + human::HumanPoseAddAdditiveLayer(pose, poseIn, weight, mask); + + if(mask.test(human::kMaskRootIndex)) + { + human::HumanPoseAddAdditiveLayer(poseBase,poseIn,weight, mask); + } + } + else + { + mecanim::human::HumanPoseMask mask = poseMask; + mask.set(mecanim::human::kMaskLeftHand,mask.test(mecanim::human::kMaskLeftHand) && human.m_HasLeftHand); + mask.set(mecanim::human::kMaskRightHand,mask.test(mecanim::human::kMaskRightHand) && human.m_HasRightHand); + + human::HumanPoseAddOverrideLayer(pose,poseIn,weight,mask); + + if(mask.test(human::kMaskRootIndex)) + { + human::HumanPoseAddOverrideLayer(poseBase,poseIn,weight, mask); + } + } + } + } + + void EvaluateAvatarLayers( ControllerBindingConstant const* controllerBindingConstant, + AvatarInput const* input, + AvatarOutput * output, + AvatarMemory * memory, + AvatarWorkspace * workspace, + AnimationSetMemory* animationSetMemory) + { + AvatarConstant const *constant = controllerBindingConstant->m_Avatar; + ControllerConstant const *controllerConstant = controllerBindingConstant->m_Controller; + AnimationSet const *animationSet = controllerBindingConstant->m_AnimationSet; + + bool hasRootMotion = constant->m_RootMotionBoneIndex != -1; + bool isHuman = constant->isHuman(); + + SetValueMask(workspace->m_ControllerWorkspace->m_DefaultMask,true); + workspace->m_ControllerWorkspace->m_BlendingClipCount = 0; + + if(hasRootMotion || isHuman) MotionOutputClear(output->m_MotionOutput); + if(isHuman) + { + workspace->m_LeftFootSpeedT = 0; + workspace->m_LeftFootSpeedQ = 0; + workspace->m_RightFootSpeedT = 0; + workspace->m_RightFootSpeedQ = 0; + } + + if(controllerConstant && !memory->m_ControllerMemory->m_StateMachineMemory.IsNull()) + { + for(int layerIter = 0; layerIter < controllerConstant->m_LayerCount; layerIter++) + { + BlendingState<false> &layerOutput = workspace->m_ControllerWorkspace->m_BlendingStateWs; + + const uint32_t stateMachineIndex = controllerConstant->m_LayerArray[layerIter]->m_StateMachineIndex; + if(stateMachineIndex != DISABLED_SYNCED_LAYER_IN_NON_PRO) + { + const uint32_t motionSetIndex = controllerConstant->m_LayerArray[layerIter]->m_StateMachineMotionSetIndex; + float layerWeight = layerIter == 0 ? 1 : memory->m_ControllerMemory->m_LayerWeights[layerIter] * memory->m_ControllerMemory->m_StateMachineMemory[stateMachineIndex]->m_MotionSetAutoWeightArray[motionSetIndex]; + bool additive = controllerConstant->m_LayerArray[layerIter]->m_LayerBlendingMode == kLayerBlendingModeAdditive; + + bool fastCopy = false; + int clipCount = EvaluateOneLayer(controllerBindingConstant, input, layerIter, &layerOutput, memory, workspace, layerIter == 0 ? output : 0, animationSetMemory,fastCopy); + + AndValueMask(workspace->m_ControllerWorkspace->m_BlendMask, animationSet->m_DynamicValuesMaskArray[layerIter]); + + if (fastCopy) + { + CopyValueMask(workspace->m_ControllerWorkspace->m_DefaultMask,workspace->m_ControllerWorkspace->m_BlendMask); + InvertValueMask(workspace->m_ControllerWorkspace->m_DefaultMask); + } + else + { + ValueArrayAdd(controllerBindingConstant->m_DynamicValuesDefault, layerOutput.m_DynamicValuesBlending.Get(),workspace->m_ControllerWorkspace->m_BlendMask,layerWeight,additive,output->m_DynamicValuesOutput,workspace->m_ControllerWorkspace->m_DefaultMask); + } + + if(hasRootMotion || isHuman) + { + if(layerIter == 0 && clipCount == 0) + { + MotionOutputClear(output->m_MotionOutput); + } + else + { + bool rootMotionMask = hasRootMotion && controllerBindingConstant->m_RootMotionLayerMask[layerIter]; + AddMotionLayer(*layerOutput.m_MotionBlending.Get(),layerIter,layerWeight,additive,*output->m_MotionOutput,rootMotionMask,isHuman,controllerConstant->m_LayerArray[layerIter]->m_BodyMask); + } + } + + if(isHuman) + { + if(layerIter == 0 && clipCount == 0) + { + HumanPoseClear(*output->m_HumanPoseOutput); + HumanPoseClear(*output->m_HumanPoseBaseOutput); + } + else + { + AddHumanLayer(*constant->m_Human.Get(),*layerOutput.m_HumanPoseBlending.Get(),controllerConstant->m_LayerArray[layerIter]->m_BodyMask,layerIter,layerWeight,additive,*output->m_HumanPoseOutput,*output->m_HumanPoseBaseOutput); + } + } + } + } + + ValueArrayCopy(controllerBindingConstant->m_DynamicValuesDefault,output->m_DynamicValuesOutput,workspace->m_ControllerWorkspace->m_DefaultMask); + ValueArrayCopy(controllerBindingConstant->m_DynamicValuesConstant, output->m_DynamicValuesOutput,controllerConstant->m_Values.Get(),memory->m_ControllerMemory->m_Values.Get(),animationSet->m_AdditionalIndexArray); + } + + if(isHuman) + { + ///////////////////////////////////////////////////////////////// + // Pivot management + if(memory->m_FirstEval==0) + { + math::float1 pivotWeight(memory->m_PivotWeight); + math::float4 prevPivot = math::lerp(output->m_MotionOutput->m_PrevLeftFootX.t,output->m_MotionOutput->m_PrevRightFootX.t,pivotWeight); + math::float4 deltaPivot = memory->m_Pivot-prevPivot; + deltaPivot.y() = 0; + deltaPivot *= input->m_FeetPivotActive; + output->m_MotionOutput->m_DX.t = math::xformMulVec(output->m_MotionOutput->m_DX,deltaPivot); + } + + memory->m_PivotWeight = math::saturate(0.5f * (0.5f + 2.0f * workspace->m_LeftFootSpeedT) / (0.5f + workspace->m_LeftFootSpeedT + workspace->m_RightFootSpeedT)); + + math::xform lLeftFootX = output->m_HumanPoseOutput->m_GoalArray[human::kLeftFootGoal].m_X; + math::xform lRightFootX = output->m_HumanPoseOutput->m_GoalArray[human::kRightFootGoal].m_X; + memory->m_Pivot = math::lerp(lLeftFootX.t,lRightFootX.t,math::float1(memory->m_PivotWeight)); + ///////////////////////////////////////////////////////////////// + + math::float1 scale(constant->isHuman() ? constant->m_Human->m_Scale : 1); + workspace->m_AvatarX = memory->m_AvatarX; + math::xform dx = output->m_MotionOutput->m_DX; + dx.t *= scale; + workspace->m_AvatarX = math::xformMul(workspace->m_AvatarX,dx); + } + } + + void EvaluateAvatarX( AvatarConstant const* constant, + AvatarInput const* input, + AvatarOutput * output, + AvatarMemory * memory, + AvatarWorkspace * workspace) + { + bool isHuman = constant->isHuman(); + int rootMotionIndex = constant->m_RootMotionBoneIndex; + + if(isHuman || rootMotionIndex != -1) + { + math::xform dx = output->m_MotionOutput->m_DX; + if(isHuman) dx.t *= math::float1(constant->m_Human->m_Scale); + memory->m_AvatarX = math::xformMul(memory->m_AvatarX, dx); // @Sonny: Can memory avatarX.s degenerate here even if dx.s == 1? + } + } + + void EvaluateAvatarRetarget( AvatarConstant const* constant, + AvatarInput const* input, + AvatarOutput * output, + AvatarMemory * memory, + AvatarWorkspace * workspace, + ControllerConstant const* controllerConstant) + { + if(controllerConstant && constant->isHuman()) + { + math::xform avatarX = memory->m_AvatarX; + + human::HumanPose humanPose; + human::HumanPose *humanPosePtr = 0; + + if(input->m_LayersAffectMassCenter) + { + human::HumanPoseCopy(*output->m_HumanPoseBaseOutput,*output->m_HumanPoseOutput); + } + else if (controllerConstant->m_LayerCount > 1) + { + human::HumanPoseCopy(humanPose,*output->m_HumanPoseOutput); + humanPosePtr = &humanPose; + } + + human::RetargetTo( constant->m_Human.Get(), + output->m_HumanPoseBaseOutput, + humanPosePtr, + avatarX, + output->m_HumanPoseOutput, + workspace->m_BodySkeletonPoseWs, + workspace->m_BodySkeletonPoseWsA); + + if(workspace->m_IKOnFeet) + { + SetIKOnFeet(true,*constant, *input, *memory, *workspace, *output); + SetIKOnFeet(false,*constant, *input, *memory, *workspace, *output); + + /* Usefull to debug IK on arms in avatar preview + output->m_HumanPose.m_GoalArray[human::kLeftHandGoal].m_WeightT = 1; + output->m_HumanPose.m_GoalArray[human::kRightHandGoal].m_WeightT = 1; + output->m_HumanPose.m_GoalArray[human::kLeftHandGoal].m_WeightR = 1; + output->m_HumanPose.m_GoalArray[human::kRightHandGoal].m_WeightR = 1; + */ + } + } + } + + void EvaluateAvatarIK( AvatarConstant const* constant, + AvatarInput const* input, + AvatarOutput * output, + AvatarMemory * memory, + AvatarWorkspace * workspace, + ControllerConstant const* controllerConstant) + { + if(controllerConstant && constant->isHuman()) + { + //////////////////////////////////////////////////////////////////////////////////////////////// + // + bool needIK = any(output->m_HumanPoseOutput->m_LookAtWeight > math::float4::zero()); + for(int i = 0; !needIK && i < mecanim::human::kLastGoal; i++) needIK |= (output->m_HumanPoseOutput->m_GoalArray[i].m_WeightT > 0 || output->m_HumanPoseOutput->m_GoalArray[i].m_WeightR > 0); + + workspace->m_BodySkeletonPoseWs->m_X[0] = output->m_HumanPoseOutput->m_RootX; + + if(needIK) + { + skeleton::SkeletonPoseComputeGlobal(constant->m_Human->m_Skeleton.Get(), workspace->m_BodySkeletonPoseWs, workspace->m_BodySkeletonPoseWsA); + FullBodySolve(constant->m_Human.Get(), output->m_HumanPoseOutput, workspace->m_BodySkeletonPoseWs, workspace->m_BodySkeletonPoseWsA, workspace->m_BodySkeletonPoseWsB); + } + + memory->m_LeftFootX = output->m_HumanPoseOutput->m_GoalArray[human::kLeftFootGoal].m_X; + memory->m_RightFootX = output->m_HumanPoseOutput->m_GoalArray[human::kRightFootGoal].m_X; + + output->m_HumanPoseOutput->m_LookAtWeight = math::float4::zero(); + for(int i = 0; i < mecanim::human::kLastGoal; i++) + { + output->m_HumanPoseOutput->m_GoalArray[i].m_WeightT = 0; + output->m_HumanPoseOutput->m_GoalArray[i].m_WeightR = 0; + } + } + } + + void EvaluateAvatarEnd( AvatarConstant const* constant, + AvatarInput const* input, + AvatarOutput * output, + AvatarMemory * memory, + AvatarWorkspace * workspace, + ControllerConstant const* controllerConstant) + { + if(constant->isHuman()) + { + int32_t rootIndex = constant->m_HumanSkeletonIndexArray[0]; + + skeleton::SkeletonPoseCopy(workspace->m_BodySkeletonPoseWs,workspace->m_BodySkeletonPoseWsA); + TwistSolve(constant->m_Human.Get(), workspace->m_BodySkeletonPoseWsA,workspace->m_BodySkeletonPoseWsB); + + skeleton::SkeletonPoseCopy(constant->m_AvatarSkeletonPose.Get(),output->m_SkeletonPoseOutput); + output->m_SkeletonPoseOutput->m_X[0] = memory->m_AvatarX; + skeleton::SkeletonPoseComputeGlobal(constant->m_AvatarSkeleton.Get(),output->m_SkeletonPoseOutput,output->m_SkeletonPoseOutput,rootIndex,0); + skeleton::SkeletonPoseComputeGlobal(constant->m_Human->m_Skeleton.Get(),workspace->m_BodySkeletonPoseWsA,workspace->m_BodySkeletonPoseWsA,1,1); + workspace->m_BodySkeletonPoseWsA->m_X[0] = output->m_SkeletonPoseOutput->m_X[rootIndex]; + skeleton::SkeletonPoseComputeLocal(constant->m_AvatarSkeleton.Get(),output->m_SkeletonPoseOutput,output->m_SkeletonPoseOutput,rootIndex,0); + skeleton::SkeletonPoseComputeLocal(constant->m_Human->m_Skeleton.Get(),workspace->m_BodySkeletonPoseWsA,workspace->m_BodySkeletonPoseWsA,1,1); + workspace->m_BodySkeletonPoseWsA->m_X[0] = output->m_SkeletonPoseOutput->m_X[rootIndex]; + + skeleton::SkeletonPoseCopy( workspace->m_BodySkeletonPoseWsA, + output->m_SkeletonPoseOutput, + constant->m_HumanSkeletonIndexCount, + constant->m_HumanSkeletonIndexArray.Get()); + } + } + + void ValuesFromClip( mecanim::ValueArray const &defaultValues, + mecanim::animation::ClipMuscleConstant const &cst, + mecanim::animation::ClipOutput const &clip, + const ClipBindings& bindings, + int32_t integerRemapStride, + mecanim::ValueArray &values, + mecanim::ValueArrayMask& valueArrayMask) + { + float* RESTRICT output; + bool* RESTRICT mask; + const float* RESTRICT inputArray = clip.m_Values; + + // Extract position values from curvedata (float[]) + output = reinterpret_cast<float*> (values.m_PositionValues.Get()); + mask = valueArrayMask.m_PositionValues.Get(); + for (int valueIndex=0;valueIndex<values.m_PositionCount;valueIndex++) + { + int curveIndex = bindings.m_PositionIndex[valueIndex]; + + if (curveIndex == -1) + { + values.m_PositionValues[valueIndex] = defaultValues.m_PositionValues[valueIndex]; + mask[valueIndex] = false; + } + else + { + float* outputValue = output + valueIndex * 4; + outputValue[0] = inputArray[curveIndex+0]; + outputValue[1] = inputArray[curveIndex+1]; + outputValue[2] = inputArray[curveIndex+2]; + outputValue[3] = 0.0F; + mask[valueIndex] = true; + } + } + + // Extract quaternion values from curvedata (float[]) + output = reinterpret_cast<float*> (values.m_QuaternionValues.Get()); + mask = valueArrayMask.m_QuaternionValues.Get(); + for (int valueIndex=0;valueIndex<values.m_QuaternionCount;valueIndex++) + { + int curveIndex = bindings.m_QuaternionIndex[valueIndex]; + if (curveIndex == -1) + { + values.m_QuaternionValues[valueIndex] = defaultValues.m_QuaternionValues[valueIndex]; + mask[valueIndex] = false; + } + else + { + float* outputValue = output + valueIndex * 4; + outputValue[0] = inputArray[curveIndex+0]; + outputValue[1] = inputArray[curveIndex+1]; + outputValue[2] = inputArray[curveIndex+2]; + outputValue[3] = inputArray[curveIndex+3]; + mask[valueIndex] = true; + } + } + + // Extract scale values from curvedata (float[]) + output = reinterpret_cast<float*> (values.m_ScaleValues.Get()); + mask = valueArrayMask.m_ScaleValues.Get(); + for (int valueIndex=0;valueIndex<values.m_ScaleCount;valueIndex++) + { + int curveIndex = bindings.m_ScaleIndex[valueIndex]; + + if (curveIndex == -1) + { + values.m_ScaleValues[valueIndex] = defaultValues.m_ScaleValues[valueIndex]; + mask[valueIndex] = false; + } + else + { + float* outputValue = output + valueIndex * 4; + outputValue[0] = inputArray[curveIndex+0]; + outputValue[1] = inputArray[curveIndex+1]; + outputValue[2] = inputArray[curveIndex+2]; + outputValue[3] = 1.0F; + mask[valueIndex] = true; + } + } + + // Extract float values from curvedata (float[]) + output = reinterpret_cast<float*> (values.m_FloatValues.Get()); + mask = valueArrayMask.m_FloatValues.Get(); + for (int valueIndex=0;valueIndex<values.m_FloatCount;valueIndex++) + { + int curveIndex = bindings.m_FloatIndex[valueIndex]; + + if (curveIndex == -1) + { + values.m_FloatValues[valueIndex] = defaultValues.m_FloatValues[valueIndex]; + mask[valueIndex] = false; + } + else + { + output[valueIndex] = inputArray[curveIndex]; + mask[valueIndex] = true; + } + } + + // Extract integer values from curvedata (float[]) + // Used for PPtr animation. The integers actually represent an instanceID. + mask = valueArrayMask.m_IntValues.Get(); + for (int valueIndex=0;valueIndex<values.m_IntCount;valueIndex++) + { + int curveIndex = bindings.m_IntIndex[valueIndex]; + + if (curveIndex == -1) + { + values.m_IntValues[valueIndex] = defaultValues.m_IntValues[valueIndex]; + mask[valueIndex] = false; + } + else + { + uint32_t index = (uint32_t)inputArray[curveIndex]; + const uint8_t* integerRemapBuffer = reinterpret_cast<const uint8_t*> (bindings.m_IntegerRemap); + int32_t valueInt32 = *reinterpret_cast<const int32_t*> (integerRemapBuffer + (index * integerRemapStride)); + + values.m_IntValues[valueIndex] = valueInt32; + mask[valueIndex] = true; + } + } + } + + void DeltasFromClip( ClipMuscleConstant const &cst, + const ClipBindings& bindings, + const ValueArrayMask& mask, + mecanim::ValueArray &starts, + mecanim::ValueArray &stops) + { + + ATTRIBUTE_ALIGN(ALIGN4F) float start[4]; + ATTRIBUTE_ALIGN(ALIGN4F) float stop[4]; + + int curveIter; + + // Positions + for (int valueIndex=0;valueIndex<starts.m_PositionCount;valueIndex++) + { + if (!mask.m_PositionValues[valueIndex]) + continue; + + curveIter = bindings.m_PositionIndex[valueIndex]; + + start[0] = cst.m_ValueArrayDelta[curveIter+0].m_Start; + start[1] = cst.m_ValueArrayDelta[curveIter+1].m_Start; + start[2] = cst.m_ValueArrayDelta[curveIter+2].m_Start; + start[3] = 0; + + stop[0] = cst.m_ValueArrayDelta[curveIter+0].m_Stop; + stop[1] = cst.m_ValueArrayDelta[curveIter+1].m_Stop; + stop[2] = cst.m_ValueArrayDelta[curveIter+2].m_Stop; + stop[3] = 0; + + math::float4 start4 = math::load(start); + math::float4 stop4 = math::load(stop); + + starts.WritePosition(start4,valueIndex); + stops.WritePosition(stop4,valueIndex); + } + + // Quaternions + for (int valueIndex=0;valueIndex<starts.m_QuaternionCount;valueIndex++) + { + if (!mask.m_QuaternionValues[valueIndex]) + continue; + + curveIter = bindings.m_QuaternionIndex[valueIndex]; + + start[0] = cst.m_ValueArrayDelta[curveIter+0].m_Start; + start[1] = cst.m_ValueArrayDelta[curveIter+1].m_Start; + start[2] = cst.m_ValueArrayDelta[curveIter+2].m_Start; + start[3] = cst.m_ValueArrayDelta[curveIter+3].m_Start; + + stop[0] = cst.m_ValueArrayDelta[curveIter+0].m_Stop; + stop[1] = cst.m_ValueArrayDelta[curveIter+1].m_Stop; + stop[2] = cst.m_ValueArrayDelta[curveIter+2].m_Stop; + stop[3] = cst.m_ValueArrayDelta[curveIter+3].m_Stop; + + math::float4 start4 = math::load(start); + math::float4 stop4 = math::load(stop); + + starts.WriteQuaternion(start4,valueIndex); + stops.WriteQuaternion(stop4,valueIndex); + } + + // Scales + for (int valueIndex=0;valueIndex<starts.m_ScaleCount;valueIndex++) + { + if (!mask.m_ScaleValues[valueIndex]) + continue; + + curveIter = bindings.m_ScaleIndex[valueIndex]; + + start[0] = cst.m_ValueArrayDelta[curveIter+0].m_Start; + start[1] = cst.m_ValueArrayDelta[curveIter+1].m_Start; + start[2] = cst.m_ValueArrayDelta[curveIter+2].m_Start; + start[3] = 0; + + stop[0] = cst.m_ValueArrayDelta[curveIter+0].m_Stop; + stop[1] = cst.m_ValueArrayDelta[curveIter+1].m_Stop; + stop[2] = cst.m_ValueArrayDelta[curveIter+2].m_Stop; + stop[3] = 0; + + math::float4 start4 = math::load(start); + math::float4 stop4 = math::load(stop); + + starts.WriteScale(start4,valueIndex); + stops.WriteScale(stop4,valueIndex); + } + + // Generic floats + for (int valueIndex=0;valueIndex<starts.m_FloatCount;valueIndex++) + { + if (!mask.m_FloatValues[valueIndex]) + continue; + + curveIter = bindings.m_FloatIndex[valueIndex]; + + start[0] = cst.m_ValueArrayDelta[curveIter+0].m_Start; + stop[0] = cst.m_ValueArrayDelta[curveIter+0].m_Stop; + + starts.WriteData(start[0],valueIndex); + stops.WriteData(stop[0],valueIndex); + } + } + + void SkeletonPoseFromValue(skeleton::Skeleton const &skeleton, skeleton::SkeletonPose const &defaultPose, ValueArray const &values, SkeletonTQSMap const *skeletonTQSMap, skeleton::SkeletonPose &pose, int32_t const*humanReverseIndex, bool skipRoot) + { + for (int index=skipRoot?1:0;index<skeleton.m_Count;index++) + { + if(!(humanReverseIndex != 0 && humanReverseIndex[index] != -1)) + { + if (skeletonTQSMap[index].m_TIndex != -1) + pose.m_X[index].t = values.ReadPosition(skeletonTQSMap[index].m_TIndex); + else + pose.m_X[index].t = defaultPose.m_X[index].t; + + if (skeletonTQSMap[index].m_QIndex != -1) + pose.m_X[index].q = values.ReadQuaternion(skeletonTQSMap[index].m_QIndex); + else + pose.m_X[index].q = defaultPose.m_X[index].q; + + if (skeletonTQSMap[index].m_SIndex != -1) + pose.m_X[index].s = values.ReadScale(skeletonTQSMap[index].m_SIndex); + else + pose.m_X[index].s = defaultPose.m_X[index].s; + } + } + } + + void SkeletonPoseFromValue(skeleton::Skeleton const &skeleton, skeleton::SkeletonPose const &defaultPose, ValueArray const &values, SkeletonTQSMap const *skeletonTQSMap, int32_t const *indexArray, skeleton::SkeletonPose &pose,int index, int stopIndex) + { + if(index != -1) + { + if(index != stopIndex) + { + SkeletonPoseFromValue(skeleton,defaultPose, values,skeletonTQSMap,indexArray,pose,skeleton.m_Node[index].m_ParentId,stopIndex); + + int avatarIndex = indexArray[index]; + + if(skeletonTQSMap[avatarIndex].m_TIndex != -1) + { + pose.m_X[index].t = values.ReadPosition(skeletonTQSMap[avatarIndex].m_TIndex); + } + else + { + pose.m_X[index].t = defaultPose.m_X[avatarIndex].t; + } + + if(skeletonTQSMap[avatarIndex].m_QIndex != -1) + { + pose.m_X[index].q = values.ReadQuaternion(skeletonTQSMap[avatarIndex].m_QIndex); + } + else + { + pose.m_X[index].q = defaultPose.m_X[avatarIndex].q; + } + + if(skeletonTQSMap[avatarIndex].m_SIndex != -1) + { + pose.m_X[index].s = values.ReadScale(skeletonTQSMap[avatarIndex].m_SIndex); + } + else + { + pose.m_X[index].s = defaultPose.m_X[avatarIndex].s; + } + } + } + } + + void ValueFromSkeletonPose(skeleton::Skeleton const &skeleton, skeleton::SkeletonPose const &pose, SkeletonTQSMap const *skeletonTQSMap, ValueArray &values) + { + for (int index=0;index<skeleton.m_Count;index++) + { + if(skeletonTQSMap[index].m_TIndex != -1) + { + values.WritePosition(pose.m_X[index].t, skeletonTQSMap[index].m_TIndex); + } + + if(skeletonTQSMap[index].m_QIndex != -1) + { + values.WriteQuaternion(pose.m_X[index].q, skeletonTQSMap[index].m_QIndex); + } + + if(skeletonTQSMap[index].m_SIndex != -1) + { + values.WriteScale(pose.m_X[index].s, skeletonTQSMap[index].m_SIndex); + } + } + } + + void ValueFromSkeletonPose(skeleton::Skeleton const &skeleton, skeleton::SkeletonPose const &pose, SkeletonTQSMap const *skeletonTQSMap,int32_t const *indexArray, ValueArray &values, int index, int stopIndex) + { + if(index != -1) + { + if(index != stopIndex) + { + ValueFromSkeletonPose(skeleton,pose,skeletonTQSMap,indexArray, values,skeleton.m_Node[index].m_ParentId,stopIndex); + + int avatarIndex = indexArray[index]; + + if(skeletonTQSMap[avatarIndex].m_TIndex != -1) + { + values.WritePosition(pose.m_X[index].t, skeletonTQSMap[avatarIndex].m_TIndex); + } + + if(skeletonTQSMap[avatarIndex].m_QIndex != -1) + { + values.WriteQuaternion(pose.m_X[index].q, skeletonTQSMap[avatarIndex].m_QIndex); + } + + if(skeletonTQSMap[avatarIndex].m_SIndex != -1) + { + values.WriteScale(pose.m_X[index].s, skeletonTQSMap[avatarIndex].m_SIndex); + } + } + } + } +} +} diff --git a/Runtime/mecanim/animation/avatar.h b/Runtime/mecanim/animation/avatar.h new file mode 100644 index 0000000..e90be7a --- /dev/null +++ b/Runtime/mecanim/animation/avatar.h @@ -0,0 +1,706 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/bind.h" +#include "Runtime/Math/Simd/float4.h" + +#include "Runtime/mecanim/skeleton/skeleton.h" +#include "Runtime/mecanim/human/human.h" +#include "Runtime/mecanim/animation/clipmuscle.h" +#include "Runtime/mecanim/animation/curvedata.h" +#include "Runtime/mecanim/statemachine/statemachine.h" + +#include "Runtime/Serialize/Blobification/offsetptr.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Animation/MecanimArraySerialization.h" + +typedef UInt32 BindingHash; + +namespace mecanim +{ + + static const uint32_t DISABLED_SYNCED_LAYER_IN_NON_PRO = 0xffffffff; + struct ValueArrayConstant; + + typedef int ProcessString(mecanim::String const&); + + namespace animation + { + struct ClipMuscleConstant; + struct AvatarConstant; + struct ControllerConstant; + + enum LayerBlendingMode + { + kLayerBlendingModeOverride, + kLayerBlendingModeAdditive + }; + + struct LayerConstant + { + DEFINE_GET_TYPESTRING(LayerConstant) + + LayerConstant():m_IKPass(true), m_SyncedLayerAffectsTiming(false), m_LayerBlendingMode(kLayerBlendingModeOverride){} + + uint32_t m_StateMachineIndex; + uint32_t m_StateMachineMotionSetIndex; + + human::HumanPoseMask m_BodyMask; + OffsetPtr<skeleton::SkeletonMask> m_SkeletonMask; + + uint32_t m_Binding; + uint32_t m_LayerBlendingMode; //LayerBlendingMode + float m_DefaultWeight; + bool m_IKPass; + bool m_SyncedLayerAffectsTiming; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + + TRANSFER(m_StateMachineIndex); + TRANSFER(m_StateMachineMotionSetIndex); + TRANSFER(m_BodyMask); + TRANSFER(m_SkeletonMask); + TRANSFER(m_Binding); + TRANSFER((int&)m_LayerBlendingMode); + TRANSFER(m_DefaultWeight); + TRANSFER(m_IKPass); + TRANSFER(m_SyncedLayerAffectsTiming); + transfer.Align(); + + + } + }; + + struct ControllerConstant + { + DEFINE_GET_TYPESTRING(ControllerConstant) + + ControllerConstant(): m_LayerCount(0), + m_StateMachineCount(0) {} + + uint32_t m_LayerCount; + OffsetPtr< OffsetPtr<LayerConstant> > m_LayerArray; + + uint32_t m_StateMachineCount; + OffsetPtr< OffsetPtr<statemachine::StateMachineConstant> > m_StateMachineArray; + + OffsetPtr<ValueArrayConstant> m_Values; + OffsetPtr<ValueArray> m_DefaultValues; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_LayerCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<mecanim::animation::LayerConstant>, m_LayerArray, m_LayerCount); + + TRANSFER_BLOB_ONLY(m_StateMachineCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<mecanim::statemachine::StateMachineConstant>, m_StateMachineArray, m_StateMachineCount); + + TRANSFER(m_Values); + TRANSFER(m_DefaultValues); + + } + + static void InitializeClass(); + }; + + struct SkeletonTQSMap + { + SkeletonTQSMap() : m_TIndex(-1), m_QIndex(-1), m_SIndex(-1) {}; + + int32_t m_TIndex; + int32_t m_QIndex; + int32_t m_SIndex; + }; + + struct ClipBindings + { + ClipBindings () : m_PositionIndex(0), m_QuaternionIndex(0), m_ScaleIndex(0), m_FloatIndex(0), m_IntIndex(0), m_IntegerRemap(0) {} + + // Maps from the curve float array of the clip to the ValueArrayConstant. + // This allows us to bring the curve float array into a ValueArray that has the same layout between all clips. + int16_t* m_PositionIndex; + int16_t* m_QuaternionIndex; + int16_t* m_ScaleIndex; + int16_t* m_FloatIndex; + int16_t* m_IntIndex; + + // Points directly to AnimationClipBindingConstant pptrCurveMapping. + int32_t* m_IntegerRemap; + }; + + struct AnimationSet + { + struct Clip + { + Clip() : m_Clip(0), m_TotalUsedOptimizedCurveCount(0), m_ClipIndex(-1) {} + + // The referenced constant clip + ClipMuscleConstant* m_Clip; + int32_t m_ClipIndex; + // The amount of ConstantClip curves that need to be sampled (Constant curves are often ignored during binding if they are determined to have no impact) + uint32_t m_TotalUsedOptimizedCurveCount; + // Binding indices to index from the curve float[] to the ValueArray + ClipBindings m_Bindings; + }; + + AnimationSet() : m_MaxBlendState(0), + m_LayerCount(0), + m_ClipPerLayer(0), + m_ClipConstant(0), + m_AdditionalCount(0), + m_AdditionalIndexArray(0), + m_DynamicFullValuesConstant(0), + m_DynamicValuesMaskArray(0), + m_GravityWeightIndex(-1), + m_IntegerRemapStride(-1) + {} + + uint32_t m_MaxBlendState; + uint32_t m_LayerCount; + uint32_t* m_ClipPerLayer; + + Clip** m_ClipConstant; + + uint32_t m_AdditionalCount; + int32_t* m_AdditionalIndexArray; + + mecanim::ValueArrayConstant* m_DynamicFullValuesConstant; + mecanim::ValueArrayMask** m_DynamicValuesMaskArray; + + int32_t m_GravityWeightIndex; + int32_t m_IntegerRemapStride; + }; + + struct ControllerBindingConstant + { + ControllerBindingConstant(): m_DynamicValuesDefault(0), + m_SkeletonTQSMap(0), + m_RootMotionLayerMask(0), + m_Avatar(0), + m_Controller(0), + m_DynamicValuesConstant(0), + m_AnimationSet(0) {} + + ValueArrayConstant* m_DynamicValuesConstant; + ValueArray* m_DynamicValuesDefault; + + SkeletonTQSMap* m_SkeletonTQSMap; + + bool* m_RootMotionLayerMask; + + AvatarConstant const* m_Avatar; + ControllerConstant const* m_Controller; + AnimationSet const* m_AnimationSet; + }; + + struct AnimationSetMemory + { + AnimationSetMemory() : m_LayerCount(0), m_ClipPerLayer(0), m_ClipMemory(0), m_ClipOutput(0) {} + + uint32_t m_LayerCount; + uint32_t* m_ClipPerLayer; + ClipMemory*** m_ClipMemory; + ClipOutput* m_ClipOutput; + }; + + template<bool dynamic> + struct BlendingState + { + BlendingState(): + m_DynamicValuesBlending(0), + m_MotionBlending(0), + m_HumanPoseBlending(0), + m_BlendFactor(0) + {} + + ValueArray** m_DynamicValuesBlending; + MotionOutput** m_MotionBlending; + human::HumanPose** m_HumanPoseBlending; + float* m_BlendFactor; + + uint32_t m_Size; + }; + + template<> + struct BlendingState<false> + { + BlendingState(): m_BlendFactor(0) {} + + OffsetPtr<ValueArray> m_DynamicValuesBlending; + OffsetPtr<ValueArrayMask> m_DynamicValuesBlendingMask; + OffsetPtr<MotionOutput> m_MotionBlending; + OffsetPtr<human::HumanPose> m_HumanPoseBlending; + float m_BlendFactor; + }; + + struct BlendingClip + { + BlendingClip() : m_ClipIndex(-1), m_LayerIndex(-1), m_Weight(0), m_PrevTime(0), m_Time(0), m_Reverse(false) {} + + int m_ClipIndex; + int m_LayerIndex; + float m_Weight; + float m_PrevTime; + float m_Time; + bool m_Reverse; + }; + + struct ControllerMemory + { + DEFINE_GET_TYPESTRING(ControllerMemory) + + ControllerMemory(): m_StateMachineCount(0), + m_LayerCount(0) {} + + uint32_t m_StateMachineCount; + OffsetPtr< OffsetPtr<statemachine::StateMachineMemory> > m_StateMachineMemory; + + uint32_t m_LayerCount; + OffsetPtr<BlendingState<false> > m_InteruptedTransitionsBlendingStateArray; + OffsetPtr<float> m_LayerWeights; + + OffsetPtr<ValueArray> m_Values; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_StateMachineCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<mecanim::statemachine::StateMachineMemory>, m_StateMachineMemory, m_StateMachineCount); + + TRANSFER_BLOB_ONLY(m_LayerCount); + MANUAL_ARRAY_TRANSFER2( BlendingState<false>, m_InteruptedTransitionsBlendingStateArray, m_LayerCount); + MANUAL_ARRAY_TRANSFER2( float, m_LayerWeights, m_LayerCount); + + TRANSFER(m_Values); + } + }; + + struct ControllerWorkspace + { + ControllerWorkspace() : m_StateMachineWorkspace(0), + m_StateMachineOutput(0), + m_BlendingState(0), + m_BlendingClipCount(0), + m_BlendingClipArray(0), + m_ValueArrayStart(0), + m_ValueArrayStop(0), + m_ReadMask(0), + m_BlendMask(0), + m_DefaultMask(0), + m_DoIK(false), + m_DoWrite(false) {} + + statemachine::StateMachineWorkspace** m_StateMachineWorkspace; + statemachine::StateMachineOutput** m_StateMachineOutput; + + uint32_t m_StateMachineCount; + + float* m_MotionSetTimingWeightArray; + + BlendingState<true>* m_BlendingState; + BlendingState<false> m_BlendingStateWs; + + int m_BlendingClipCount; + BlendingClip* m_BlendingClipArray; + + ValueArray *m_ValueArrayStart; + ValueArray *m_ValueArrayStop; + + ValueArrayMask *m_ReadMask; + ValueArrayMask *m_BlendMask; + ValueArrayMask *m_DefaultMask; + + bool m_DoIK; + bool m_DoWrite; + }; + + struct ExposedTransform + { + DEFINE_GET_TYPESTRING(ExposedTransform); + + // For SkinnedMeshRenderer, the following two indices are different + // - 'skeletonIndex' + // corresponds to the SkinnedMeshRenderer itself + // - 'skeletonIndexForUpdateTransform' + // corresponds to the root bone of the SkinnedMeshRenderer + uint32_t skeletonIndex; + uint32_t skeletonIndexForUpdateTransform; + + BindingHash transformPath; // flattened path + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(skeletonIndex); + TRANSFER(skeletonIndexForUpdateTransform); + TRANSFER(transformPath); + } + }; + + struct AvatarConstant + { + DEFINE_GET_TYPESTRING(AvatarConstant) + + AvatarConstant() : m_SkeletonNameIDCount(0), + m_HumanSkeletonIndexCount(0), + m_HumanSkeletonReverseIndexCount(0), + m_RootMotionBoneIndex(-1), + m_RootMotionBoneX(math::xformIdentity()), + m_RootMotionSkeletonIndexCount(0) {} + + OffsetPtr<skeleton::Skeleton> m_AvatarSkeleton; + OffsetPtr<skeleton::SkeletonPose> m_AvatarSkeletonPose; + + OffsetPtr<skeleton::SkeletonPose> m_DefaultPose; // The default pose when model is imported. + + uint32_t m_SkeletonNameIDCount; + OffsetPtr<uint32_t> m_SkeletonNameIDArray; // CRC(name) + + OffsetPtr<human::Human> m_Human; + + uint32_t m_HumanSkeletonIndexCount; + OffsetPtr<int32_t> m_HumanSkeletonIndexArray; + + // needed to update human pose and additonal bones in optimize mode + // decided to put the info in constant for perf and memory reason vs doing masking at runtime + uint32_t m_HumanSkeletonReverseIndexCount; + OffsetPtr<int32_t> m_HumanSkeletonReverseIndexArray; + + int32_t m_RootMotionBoneIndex; + math::xform m_RootMotionBoneX; + OffsetPtr<skeleton::Skeleton> m_RootMotionSkeleton; + OffsetPtr<skeleton::SkeletonPose> m_RootMotionSkeletonPose; + uint32_t m_RootMotionSkeletonIndexCount; + OffsetPtr<int32_t> m_RootMotionSkeletonIndexArray; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + transfer.SetVersion(3); + + TRANSFER(m_AvatarSkeleton); + + TRANSFER(m_AvatarSkeletonPose); + TRANSFER(m_DefaultPose); + + TRANSFER_BLOB_ONLY(m_SkeletonNameIDCount); + MANUAL_ARRAY_TRANSFER2(uint32_t,m_SkeletonNameIDArray,m_SkeletonNameIDCount); + + TRANSFER(m_Human); + + TRANSFER_BLOB_ONLY(m_HumanSkeletonIndexCount); + MANUAL_ARRAY_TRANSFER2(int32_t,m_HumanSkeletonIndexArray,m_HumanSkeletonIndexCount); + + TRANSFER_BLOB_ONLY(m_HumanSkeletonReverseIndexCount); + MANUAL_ARRAY_TRANSFER2(int32_t,m_HumanSkeletonReverseIndexArray,m_HumanSkeletonReverseIndexCount); + + TRANSFER(m_RootMotionBoneIndex); + TRANSFER(m_RootMotionBoneX); + TRANSFER(m_RootMotionSkeleton); + TRANSFER(m_RootMotionSkeletonPose); + + TRANSFER_BLOB_ONLY(m_RootMotionSkeletonIndexCount); + MANUAL_ARRAY_TRANSFER2(int32_t,m_RootMotionSkeletonIndexArray,m_RootMotionSkeletonIndexCount); + + transfer.Align(); + + if (transfer.IsVersionSmallerOrEqual (1)) + { + if(m_RootMotionBoneIndex != -1) + { + mecanim::memory::Allocator *alloc = reinterpret_cast<mecanim::memory::Allocator *>(transfer.GetUserData()); + + m_RootMotionSkeleton = skeleton::CreateSkeleton(m_AvatarSkeleton->m_Count,m_AvatarSkeleton->m_AxesCount,*alloc); + skeleton::SkeletonCopy(m_AvatarSkeleton.Get(),m_RootMotionSkeleton.Get()); + + m_RootMotionSkeletonPose = skeleton::CreateSkeletonPose(m_RootMotionSkeleton.Get(),*alloc); + skeleton::SkeletonPoseCopy(m_AvatarSkeletonPose.Get(),m_RootMotionSkeletonPose.Get()); + + m_RootMotionSkeletonIndexCount = m_AvatarSkeleton->m_Count; + m_RootMotionSkeletonIndexArray = alloc->ConstructArray<mecanim::int32_t>(m_RootMotionSkeletonIndexCount); + + for(int i = 0; i < m_RootMotionSkeletonIndexCount; i++) + { + m_RootMotionSkeletonIndexArray[i] = i; + } + } + } + + if (transfer.IsVersionSmallerOrEqual (2)) + { + if(isHuman()) + { + mecanim::memory::Allocator *alloc = reinterpret_cast<mecanim::memory::Allocator *>(transfer.GetUserData()); + + m_HumanSkeletonReverseIndexCount = m_AvatarSkeleton->m_Count; + m_HumanSkeletonReverseIndexArray = alloc->ConstructArray<mecanim::int32_t>(m_HumanSkeletonReverseIndexCount); + skeleton::SkeletonBuildReverseIndexArray(m_HumanSkeletonReverseIndexArray.Get(),m_HumanSkeletonIndexArray.Get(),m_Human->m_Skeleton.Get(),m_AvatarSkeleton.Get()); + } + } + } + + bool isHuman() const { return !m_Human.IsNull() && m_Human->GetTypeString() != 0 && m_Human->m_Skeleton->m_Count > 0; }; + + static void InitializeClass(); + }; + + struct AvatarInput + { + AvatarInput() : m_GotoStateInfos(0), m_DeltaTime(0), m_TargetIndex(-1), m_TargetTime(1), m_FeetPivotActive(1), m_StabilizeFeet(false), m_ForceStateTime(false), m_StateTime(0), m_LayersAffectMassCenter(false) {} + + statemachine::GotoStateInfo* m_GotoStateInfos; + float m_DeltaTime; + int m_TargetIndex; + float m_TargetTime; + + float m_FeetPivotActive; + bool m_StabilizeFeet; + + bool m_ForceStateTime; + float m_StateTime; + bool m_LayersAffectMassCenter; + }; + + struct AvatarMemory + { + DEFINE_GET_TYPESTRING(AvatarMemory) + + AvatarMemory() : m_AvatarX(math::xformIdentity()), + m_LeftFootX(math::xformIdentity()), + m_RightFootX(math::xformIdentity()), + m_Pivot(math::float4::zero()), + m_PivotWeight(0.5), + m_FirstEval(1), + m_SkeletonPoseOutputReady(0) {} + + OffsetPtr<ControllerMemory> m_ControllerMemory; + + math::xform m_AvatarX; + + math::xform m_LeftFootX; + math::xform m_RightFootX; + math::float4 m_Pivot; + + float m_PivotWeight; + UInt8 m_FirstEval; + UInt8 m_SkeletonPoseOutputReady; + + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_ControllerMemory); + TRANSFER(m_AvatarX); + TRANSFER(m_LeftFootX); + TRANSFER(m_RightFootX); + TRANSFER(m_Pivot); + TRANSFER(m_PivotWeight); + TRANSFER(m_FirstEval); + TRANSFER(m_SkeletonPoseOutputReady); + transfer.Align(); + } + }; + + struct AvatarWorkspace + { + AvatarWorkspace() : m_BodySkeletonPoseWs(0), + m_BodySkeletonPoseWsA(0), + m_BodySkeletonPoseWsB(0), + m_RootMotionSkeletonPoseWsA(0), + m_RootMotionSkeletonPoseWsB(0), + m_HumanPoseWs(0), + m_ControllerWorkspace(0), + m_LeftFootSpeedT(0), + m_LeftFootSpeedQ(0), + m_RightFootSpeedT(0), + m_RightFootSpeedQ(0), + m_IKOnFeet(false) + {} + + skeleton::SkeletonPose* m_BodySkeletonPoseWs; + skeleton::SkeletonPose* m_BodySkeletonPoseWsA; + skeleton::SkeletonPose* m_BodySkeletonPoseWsB; + + skeleton::SkeletonPose* m_RootMotionSkeletonPoseWsA; + skeleton::SkeletonPose* m_RootMotionSkeletonPoseWsB; + + human::HumanPose* m_HumanPoseWs; + + ControllerWorkspace* m_ControllerWorkspace; + + math::xform m_AvatarX; + + float m_LeftFootSpeedT; + float m_LeftFootSpeedQ; + float m_RightFootSpeedT; + float m_RightFootSpeedQ; + bool m_IKOnFeet; + }; + + struct AvatarOutput + { + AvatarOutput() : m_DynamicValuesOutput(0), + m_SkeletonPoseOutput(0), + m_MotionOutput(0), + m_HumanPoseBaseOutput(0), + m_HumanPoseOutput(0) {} + + ValueArray* m_DynamicValuesOutput; + + skeleton::SkeletonPose* m_SkeletonPoseOutput; + + MotionOutput* m_MotionOutput; + human::HumanPose* m_HumanPoseBaseOutput; + human::HumanPose* m_HumanPoseOutput; + }; + + AvatarConstant* CreateAvatarConstant( skeleton::Skeleton* skeleton, + skeleton::SkeletonPose* skeletonPose, + skeleton::SkeletonPose* defaultPose, + human::Human* human, + skeleton::Skeleton* rootMotionSkeleton, + int rootMotionIndex, + math::xform const& rootMotionX, + memory::Allocator& alloc); + + void DestroyAvatarConstant(AvatarConstant* constant, memory::Allocator& alloc); + + void InitializeAvatarConstant(AvatarConstant * constant, memory::Allocator& alloc); + void ClearAvatarConstant(AvatarConstant * constant, memory::Allocator& alloc); + + ControllerConstant* CreateControllerConstant( uint32_t LayerCount, LayerConstant** layerArray, + uint32_t stateMachineCount, statemachine::StateMachineConstant** stateMachineConstant, + ValueArrayConstant* values, ValueArray* defaultValues, + memory::Allocator& alloc); + + void DestroyControllerConstant(ControllerConstant* controller, memory::Allocator& alloc); + + void InitializeControllerConstant(ControllerConstant * controller, memory::Allocator& alloc); + void ClearControllerConstant(ControllerConstant * controller, memory::Allocator& alloc); + + LayerConstant* CreateLayerConstant(mecanim::uint32_t stateMachineIndex, mecanim::uint32_t motionSetIndex, memory::Allocator& alloc); + void DestroyLayerConstant(LayerConstant* constant, memory::Allocator& alloc); + + ControllerMemory* CreateControllerMemory(ControllerConstant const* controller, AvatarConstant const *avatar, AnimationSet const *animationSet, const ValueArrayConstant* dynamicValueConstant, memory::Allocator& alloc); + void DestroyControllerMemory(ControllerMemory* memory, memory::Allocator& alloc); + + ControllerWorkspace* CreateControllerWorkspace(ControllerConstant const* controller, AvatarConstant const *avatar, AnimationSet const *animationSet, const ValueArrayConstant* dynamicValueConstant, memory::Allocator& alloc); + void DestroyControllerWorkspace(ControllerWorkspace* workspace, memory::Allocator& alloc); + + AvatarInput* CreateAvatarInput(AvatarConstant const* constant, memory::Allocator& alloc); + void DestroyAvatarInput(AvatarInput* input, memory::Allocator& alloc); + + AvatarMemory* CreateAvatarMemory(AvatarConstant const* constant, memory::Allocator& alloc); + void DestroyAvatarMemory(AvatarMemory* memory, memory::Allocator& alloc); + + AvatarWorkspace* CreateAvatarWorkspace(AvatarConstant const* constant, memory::Allocator& alloc); + void DestroyAvatarWorkspace(AvatarWorkspace* workspace, memory::Allocator& alloc); + + AvatarOutput* CreateAvatarOutput(AvatarConstant const* constant, bool hasTransformHierarchy, memory::Allocator& alloc); + void DestroyAvatarOutput(AvatarOutput* output, memory::Allocator& alloc); + + AnimationSet* CreateAnimationSet(ControllerConstant const* controller, memory::Allocator& alloc); + void DestroyAnimationSet(AnimationSet* animationSet, memory::Allocator& alloc); + + AnimationSetMemory* CreateAnimationSetMemory(AnimationSet const* animationSet, bool allowConstantCurveOptimization, memory::Allocator& alloc); + void DestroyAnimationSetMemory(AnimationSetMemory* animationSetMemory, memory::Allocator& alloc); + + void UpdateLeafNodeDuration(const ControllerConstant &controllerConstant, const AnimationSet &animationSet, ControllerMemory &controllerMemory); + + void SetIKOnFeet( bool left, + AvatarConstant const &avatar, + const AvatarInput &input, + AvatarMemory &memory, + AvatarWorkspace &workspace, + AvatarOutput &output); + + void EvaluateAvatarSM( AvatarConstant const* constant, + AvatarInput const* input, + AvatarOutput * output, + AvatarMemory * memory, + AvatarWorkspace * workspace, + ControllerConstant const* controllerConstant); + + void EvaluateAvatarLayers( ControllerBindingConstant const* controllerBindingConstant, + AvatarInput const* input, + AvatarOutput *output, + AvatarMemory *memory, + AvatarWorkspace *workspace, + AnimationSetMemory* animationSetMemory); + + void EvaluateAvatarX( AvatarConstant const* constant, + AvatarInput const* input, + AvatarOutput *output, + AvatarMemory *memory, + AvatarWorkspace *workspace); + + void EvaluateAvatarRetarget( AvatarConstant const* constant, + AvatarInput const* input, + AvatarOutput *output, + AvatarMemory *memory, + AvatarWorkspace *workspace, + ControllerConstant const* controllerConstant); + + void EvaluateAvatarIK( AvatarConstant const* constant, + AvatarInput const* input, + AvatarOutput *output, + AvatarMemory *memory, + AvatarWorkspace *workspace, + ControllerConstant const* controllerConstant); + + void EvaluateAvatarEnd( AvatarConstant const* constant, + AvatarInput const* input, + AvatarOutput *output, + AvatarMemory *memory, + AvatarWorkspace *workspace, + ControllerConstant const* controllerConstant); + + void ValuesFromClip( mecanim::ValueArray const &valuesDefault, + mecanim::animation::ClipMuscleConstant const &cst, + mecanim::animation::ClipOutput const &out, + const ClipBindings& bindings, + int32_t integerRemapStride, + mecanim::ValueArray &values, + mecanim::ValueArrayMask &mask); + + void DeltasFromClip( mecanim::animation::ClipMuscleConstant const &cst, + const ClipBindings& bindings, + const ValueArrayMask& mask, + mecanim::ValueArray &starts, + mecanim::ValueArray &stops); + + void SkeletonPoseFromValue(skeleton::Skeleton const &skeleton, skeleton::SkeletonPose const &defaultPose, ValueArray const &values, SkeletonTQSMap const *skeletonTQSMap, skeleton::SkeletonPose &pose,int32_t const *humanReverseIndex,bool skipRoot); + void SkeletonPoseFromValue(skeleton::Skeleton const &skeleton, skeleton::SkeletonPose const &defaultPose, ValueArray const &values, SkeletonTQSMap const *skeletonTQSMap, int32_t const *indexArray, skeleton::SkeletonPose &pose,int index, int stopIndex); + void ValueFromSkeletonPose(skeleton::Skeleton const &skeleton, skeleton::SkeletonPose const &pose, SkeletonTQSMap const *skeletonTQSMap, ValueArray &values); + void ValueFromSkeletonPose(skeleton::Skeleton const &skeleton, skeleton::SkeletonPose const &pose, SkeletonTQSMap const *skeletonTQSMap, int32_t const *indexArray, ValueArray &values, int index, int stopIndex); + } +} + +template<> +class SerializeTraits< mecanim::animation::BlendingState<false> > : public SerializeTraitsBase< mecanim::animation::BlendingState<false> > +{ + public: + + typedef mecanim::animation::BlendingState<false> value_type; + inline static const char* GetTypeString (void*) { return "BlendingState<1>"; } + inline static bool IsAnimationChannel () { return false; } + inline static bool MightContainPPtr () { return true; } + inline static bool AllowTransferOptimization () { return false; } + + template<class TransferFunction> inline + static void Transfer (value_type& data, TransferFunction& transfer) + { + transfer.Transfer(data.m_DynamicValuesBlending, "m_DynamicValuesBlending"); + transfer.Transfer(data.m_DynamicValuesBlendingMask, "m_DynamicValuesBlendingMask"); + transfer.Transfer(data.m_MotionBlending, "m_MotionBlending"); + transfer.Transfer(data.m_HumanPoseBlending, "m_HumanPoseBlending"); + transfer.Transfer(data.m_BlendFactor, "m_BlendFactor"); + } +}; + + diff --git a/Runtime/mecanim/animation/blendtree.cpp b/Runtime/mecanim/animation/blendtree.cpp new file mode 100644 index 0000000..e326b77 --- /dev/null +++ b/Runtime/mecanim/animation/blendtree.cpp @@ -0,0 +1,967 @@ +#include "UnityPrefix.h" +#include "Runtime/Misc/BuildSettings.h" +#include "Runtime/mecanim/animation/blendtree.h" +#include "Runtime/mecanim/generic/stringtable.h" +#include "Runtime/Utilities/dynamic_array.h" + +namespace mecanim +{ + +namespace animation +{ + void GetWeightsFreeformDirectional (const Blend2dDataConstant& blendConstant, + float* weightArray, int* cropArray, Vector2f* workspaceBlendVectors, + float blendValueX, float blendValueY, bool preCompute); + void GetWeightsFreeformCartesian (const Blend2dDataConstant& blendConstant, + float* weightArray, int* cropArray, Vector2f* workspaceBlendVectors, + float blendValueX, float blendValueY, bool preCompute); + + void PrecomputeFreeform (int type, Blend2dDataConstant& out, memory::Allocator& alloc) + { + const Vector2f* positionArray = out.m_ChildPositionArray.Get(); + mecanim::uint32_t count = out.m_ChildCount; + float* constantMagnitudes = out.m_ChildMagnitudeArray.Get(); + Vector2f* constantChildPairVectors = out.m_ChildPairVectorArray.Get(); + float* constantChildPairAvgMagInv = out.m_ChildPairAvgMagInvArray.Get(); + MotionNeighborList* constantChildNeighborLists = out.m_ChildNeighborListArray.Get(); + + if (type == 2) + { + for (int i=0; i<count; i++) + constantMagnitudes[i] = Magnitude (positionArray[i]); + + for (int i=0; i<count; i++) + { + for (int j=0; j<count; j++) + { + int pairIndex = i + j*count; + + // Calc avg magnitude for pair + float magSum = constantMagnitudes[j] + constantMagnitudes[i]; + if (magSum > 0) + constantChildPairAvgMagInv[pairIndex] = 2.0f / magSum; + else + constantChildPairAvgMagInv[pairIndex] = 2.0f / magSum; + + // Calc mag of vector and divide by avg magnitude + float mag = (constantMagnitudes[j] - constantMagnitudes[i]) * constantChildPairAvgMagInv[pairIndex]; + + if (constantMagnitudes[j] == 0 || constantMagnitudes[i] == 0) + constantChildPairVectors[pairIndex] = Vector2f (0, mag); + else + { + float angle = Angle (positionArray[i], positionArray[j]); + if (positionArray[i].x * positionArray[j].y - positionArray[i].y * positionArray[j].x < 0) + angle = -angle; + constantChildPairVectors[pairIndex] = Vector2f (angle, mag); + } + } + } + } + else if (type == 3) + { + for (int i=0; i<count; i++) + { + for (int j=0; j<count; j++) + { + int pairIndex = i + j*count; + constantChildPairAvgMagInv[pairIndex] = 1 / SqrMagnitude (positionArray[j] - positionArray[i]); + constantChildPairVectors[pairIndex] = positionArray[j] - positionArray[i]; + } + } + } + + float* weightArray; + ALLOC_TEMP (weightArray, float, count); + + int* cropArray; + ALLOC_TEMP (cropArray, int, count); + + Vector2f* workspaceBlendVectors; + ALLOC_TEMP (workspaceBlendVectors, Vector2f, count); + + bool* neighborArray; + ALLOC_TEMP (neighborArray, bool, count*count); + for (int c=0; c<count*count; c++) + neighborArray[c] = false; + + float minX = 10000.0f; + float maxX = -10000.0f; + float minY = 10000.0f; + float maxY = -10000.0f; + for (int c=0; c<count; c++) + { + minX = min (minX, positionArray[c].x); + maxX = max (maxX, positionArray[c].x); + minY = min (minY, positionArray[c].y); + maxY = max (maxY, positionArray[c].y); + } + float xRange = (maxX - minX) * 0.5f; + float yRange = (maxY - minY) * 0.5f; + minX -= xRange; + maxX += xRange; + minY -= yRange; + maxY += yRange; + + for (int i=0; i<=100; i++) + { + for (int j=0; j<=100; j++) + { + float x = i*0.01f; + float y = j*0.01f; + if (type == 2) + GetWeightsFreeformDirectional (out, weightArray, cropArray, workspaceBlendVectors, minX * (1-x) + maxX * x, minY * (1-y) + maxY * y, true); + else if (type == 3) + GetWeightsFreeformCartesian (out, weightArray, cropArray, workspaceBlendVectors, minX * (1-x) + maxX * x, minY * (1-y) + maxY * y, true); + for (int c=0; c<count; c++) + if (cropArray[c] >= 0) + neighborArray[c * count + cropArray[c]] = true; + } + } + for (int c=0; c<count; c++) + { + dynamic_array<int> nList; + for (int d=0; d<count; d++) + if (neighborArray[c * count + d]) + nList.push_back (d); + + constantChildNeighborLists[c].m_Count = nList.size (); + constantChildNeighborLists[c].m_NeighborArray = alloc.ConstructArray<mecanim::uint32_t>(nList.size ()); + + for (int d=0; d<nList.size (); d++) + constantChildNeighborLists[c].m_NeighborArray[d] = nList[d]; + } + } + + void GetAllBlendValue(uint32_t nodeIndex, OffsetPtr<BlendTreeNodeConstant>* const allTreeNodes, dynamic_array<int> &arBlendValueIds) + { + BlendTreeNodeConstant* const currentNode = allTreeNodes[nodeIndex].Get(); + + if(currentNode->m_BlendEventID != -1) + { + dynamic_array<int>::const_iterator it = std::find(arBlendValueIds.begin(), arBlendValueIds.end(), currentNode->m_BlendEventID); + if(it == arBlendValueIds.end()) + arBlendValueIds.push_back(currentNode->m_BlendEventID); + + if(currentNode->m_BlendType >= 1) + { + dynamic_array<int>::const_iterator itY = std::find(arBlendValueIds.begin(), arBlendValueIds.end(), currentNode->m_BlendEventYID); + if(itY == arBlendValueIds.end()) + arBlendValueIds.push_back(currentNode->m_BlendEventYID); + } + + for(mecanim::uint32_t i = 0 ; i < currentNode->m_ChildCount; i++) + { + GetAllBlendValue(currentNode->m_ChildIndices[i], allTreeNodes, arBlendValueIds); + } + } + + } + void GetAllBlendValue(BlendTreeConstant* const constant, dynamic_array<int> &arBlendValueIds) + { + if(constant->m_NodeCount > 0) + GetAllBlendValue(0, constant->m_NodeArray.Get(), arBlendValueIds); + } + + + BlendTreeNodeConstant* CreateBlendTreeNodeConstant(uint32_t blendValueID, uint32_t childCount, uint32_t* childIndices, float* blendTreeThresholdArray, memory::Allocator& alloc) + { + SETPROFILERLABEL(BlendTreeNodeConstant); + + BlendTreeNodeConstant *blendTreeNodeConstant = alloc.Construct<BlendTreeNodeConstant>(); + + blendTreeNodeConstant->m_BlendEventID = blendValueID; + blendTreeNodeConstant->m_ChildCount = childCount; + + blendTreeNodeConstant->m_ChildIndices = alloc.ConstructArray<uint32_t>(childCount); + memcpy(&blendTreeNodeConstant->m_ChildIndices[0], &childIndices[0], sizeof(uint32_t)*childCount); + + // Setup blend 1d data constant + blendTreeNodeConstant->m_BlendType = 0; + blendTreeNodeConstant->m_Blend1dData = alloc.Construct<Blend1dDataConstant>(); + Blend1dDataConstant& data = *blendTreeNodeConstant->m_Blend1dData; + + // Populate blend 1d data constant + data.m_ChildCount = childCount; + data.m_ChildThresholdArray = alloc.ConstructArray<float>(data.m_ChildCount); + memcpy(&data.m_ChildThresholdArray[0], &blendTreeThresholdArray[0], sizeof(float)*childCount); + + return blendTreeNodeConstant ; + } + + BlendTreeNodeConstant* CreateBlendTreeNodeConstant(uint32_t blendValueID, uint32_t blendValueYID, int blendType, uint32_t childCount, uint32_t* childIndices, Vector2f* blendTreePositionArray, memory::Allocator& alloc) + { + SETPROFILERLABEL(BlendTreeNodeConstant); + + BlendTreeNodeConstant *blendTreeNodeConstant = alloc.Construct<BlendTreeNodeConstant>(); + + blendTreeNodeConstant->m_BlendEventID = blendValueID; + blendTreeNodeConstant->m_BlendEventYID = blendValueYID; + blendTreeNodeConstant->m_ChildCount = childCount; + + blendTreeNodeConstant->m_ChildIndices = alloc.ConstructArray<uint32_t>(childCount); + memcpy(&blendTreeNodeConstant->m_ChildIndices[0], &childIndices[0], sizeof(uint32_t)*childCount); + + // Setup blend 2d data constant + blendTreeNodeConstant->m_BlendType = blendType; + blendTreeNodeConstant->m_Blend2dData = alloc.Construct<Blend2dDataConstant>(); + Blend2dDataConstant& data = *blendTreeNodeConstant->m_Blend2dData; + + // Populate blend 2d data constant + data.m_ChildCount = childCount; + data.m_ChildPositionArray = alloc.ConstructArray<Vector2f>(data.m_ChildCount); + memcpy(&data.m_ChildPositionArray[0], &blendTreePositionArray[0], sizeof(Vector2f)*childCount); + + if (blendType == 2 || blendType == 3) + { + // Populate blend 2d precomputed data for type 2 or 3 + if (blendType == 2) + { + data.m_ChildMagnitudeCount = childCount; + data.m_ChildMagnitudeArray = alloc.ConstructArray<float>(data.m_ChildMagnitudeCount); + } + data.m_ChildPairAvgMagInvCount = childCount * childCount; + data.m_ChildPairVectorCount = childCount * childCount; + data.m_ChildNeighborListCount = childCount; + data.m_ChildPairAvgMagInvArray = alloc.ConstructArray<float>(data.m_ChildPairAvgMagInvCount); + data.m_ChildPairVectorArray = alloc.ConstructArray<Vector2f>(data.m_ChildPairVectorCount); + data.m_ChildNeighborListArray = alloc.ConstructArray<MotionNeighborList>(data.m_ChildNeighborListCount); + PrecomputeFreeform (blendType, data, alloc); + } + + return blendTreeNodeConstant; + } + + BlendTreeNodeConstant* CreateBlendTreeNodeConstant(uint32_t clipID, float duration, bool mirror, float cycle, memory::Allocator& alloc) + { + SETPROFILERLABEL(BlendTreeNodeConstant); + BlendTreeNodeConstant *blendTreeNodeConstant = alloc.Construct<BlendTreeNodeConstant>(); + + blendTreeNodeConstant->m_ChildCount = 0; + blendTreeNodeConstant->m_ClipID = clipID; + blendTreeNodeConstant->m_Duration = duration; + blendTreeNodeConstant->m_Mirror = mirror; + blendTreeNodeConstant->m_CycleOffset = cycle; + + return blendTreeNodeConstant ; + } + + BlendTreeConstant* CreateBlendTreeConstant(BlendTreeNodeConstant** nodeArray, uint32_t nodeCount, memory::Allocator& alloc) + { + SETPROFILERLABEL(BlendTreeConstant); + + BlendTreeConstant *blendTreeConstant = alloc.Construct<BlendTreeConstant>(); + + blendTreeConstant->m_NodeCount = nodeCount; + blendTreeConstant->m_NodeArray = alloc.ConstructArray< OffsetPtr<BlendTreeNodeConstant> >(nodeCount); + + uint32_t i; + for(i = 0; i < nodeCount; i++) + blendTreeConstant->m_NodeArray[i] = nodeArray[i]; + + dynamic_array<int> blendValueIds; + GetAllBlendValue(blendTreeConstant, blendValueIds); + + blendTreeConstant->m_BlendEventArrayConstant = CreateValueArrayConstant(kFloatType, blendValueIds.size(), alloc); + + for(i = 0; i < blendValueIds.size(); i++) + { + blendTreeConstant->m_BlendEventArrayConstant->m_ValueArray[i].m_ID = blendValueIds[i]; + } + + return blendTreeConstant ; + } + + BlendTreeConstant* CreateBlendTreeConstant(uint32_t clipID, memory::Allocator& alloc) + { + SETPROFILERLABEL(BlendTreeConstant); + + BlendTreeConstant *blendTreeConstant = alloc.Construct<BlendTreeConstant>(); + blendTreeConstant->m_NodeCount = 1; + blendTreeConstant->m_NodeArray = alloc.ConstructArray< OffsetPtr<BlendTreeNodeConstant> >(1); + + blendTreeConstant->m_NodeArray[0] = CreateBlendTreeNodeConstant(clipID,1.0f,false,0,alloc); + + return blendTreeConstant; + + } + + void DestroyBlendTreeNodeConstant(BlendTreeNodeConstant * constant, memory::Allocator& alloc) + { + alloc.Deallocate(constant->m_ChildIndices); + + if (!constant->m_Blend1dData.IsNull ()) + { + alloc.Deallocate(constant->m_Blend1dData->m_ChildThresholdArray); + } + + if (!constant->m_Blend2dData.IsNull ()) + { + alloc.Deallocate(constant->m_Blend2dData->m_ChildPositionArray); + alloc.Deallocate(constant->m_Blend2dData->m_ChildMagnitudeArray); + alloc.Deallocate(constant->m_Blend2dData->m_ChildPairVectorArray); + alloc.Deallocate(constant->m_Blend2dData->m_ChildPairAvgMagInvArray); + + if (!constant->m_Blend2dData->m_ChildNeighborListArray.IsNull ()) + { + for (int i=0; i<constant->m_Blend2dData->m_ChildNeighborListCount; i++) + alloc.Deallocate(constant->m_Blend2dData->m_ChildNeighborListArray[i].m_NeighborArray); + alloc.Deallocate(constant->m_Blend2dData->m_ChildNeighborListArray); + } + } + + alloc.Deallocate(constant); + } + + void DestroyBlendTreeConstant(BlendTreeConstant * constant, memory::Allocator& alloc) + { + if(constant) + { + if(!constant->m_BlendEventArrayConstant.IsNull()) + { + DestroyValueArrayConstant(constant->m_BlendEventArrayConstant.Get(), alloc); + } + + for(uint32_t i = 0; i < constant->m_NodeCount; i++) + { + DestroyBlendTreeNodeConstant(constant->m_NodeArray[i].Get(), alloc); + } + + alloc.Deallocate(constant->m_NodeArray); + alloc.Deallocate(constant); + } + + } + + BlendTreeMemory* CreateBlendTreeMemory(BlendTreeConstant const* constant, memory::Allocator& alloc) + { + SETPROFILERLABEL(BlendTreeMemory); + BlendTreeMemory *blendTreeMemory = alloc.Construct<BlendTreeMemory>(); + + blendTreeMemory->m_NodeCount = GetLeafCount(*constant); + blendTreeMemory->m_NodeDurationArray = alloc.ConstructArray<float>(blendTreeMemory->m_NodeCount); + + return blendTreeMemory ; + } + + void DestroyBlendTreeMemory(BlendTreeMemory *memory, memory::Allocator& alloc) + { + if(memory) + { + alloc.Deallocate(memory->m_NodeDurationArray); + alloc.Deallocate(memory); + } + } + + BlendTreeInput* CreateBlendTreeInput(BlendTreeConstant const* constant, memory::Allocator& alloc) + { + SETPROFILERLABEL(BlendTreeInput); + + BlendTreeInput *blendTreeInput = alloc.Construct<BlendTreeInput>(); + + if(!constant->m_BlendEventArrayConstant.IsNull()) + blendTreeInput->m_BlendValueArray = CreateValueArray(constant->m_BlendEventArrayConstant.Get(), alloc); + + return blendTreeInput ; + } + + void DestroyBlendTreeInput(BlendTreeInput * input, memory::Allocator& alloc) + { + if(input) + { + if(input->m_BlendValueArray) + { + DestroyValueArray(input->m_BlendValueArray, alloc); + } + + alloc.Deallocate(input); + } + } + + BlendTreeOutput* CreateBlendTreeOutput(BlendTreeConstant const* constant, uint32_t maxBlendedClip, memory::Allocator& alloc) + { + SETPROFILERLABEL(BlendTreeOutput); + BlendTreeOutput *blendTreeOutput = alloc.Construct<BlendTreeOutput>(); + + blendTreeOutput->m_MaxBlendedClip = maxBlendedClip; + blendTreeOutput->m_OutputBlendArray = alloc.ConstructArray< BlendTreeNodeOutput >(maxBlendedClip); + + return blendTreeOutput ; + } + + void DestroyBlendTreeOutput(BlendTreeOutput *output, memory::Allocator& alloc) + { + if(output) + { + alloc.Deallocate(output->m_OutputBlendArray); + alloc.Deallocate(output); + } + } + + BlendTreeWorkspace* CreateBlendTreeWorkspace(BlendTreeConstant const* constant, memory::Allocator& alloc) + { + SETPROFILERLABEL(BlendTreeWorkspace); + + BlendTreeWorkspace *blendTreeWorkspace = alloc.Construct<BlendTreeWorkspace>(); + blendTreeWorkspace->m_BlendArray = alloc.ConstructArray<float>(constant->m_NodeCount); + // Optimize later to only have room for worst case number of immediate children instead of m_NodeCount: + blendTreeWorkspace->m_TempWeightArray = alloc.ConstructArray<float>(constant->m_NodeCount); + blendTreeWorkspace->m_TempCropArray = alloc.ConstructArray<int>(constant->m_NodeCount); + blendTreeWorkspace->m_ChildInputVectorArray = alloc.ConstructArray<Vector2f>(constant->m_NodeCount); + + return blendTreeWorkspace ; + } + + void DestroyBlendTreeWorkspace(BlendTreeWorkspace * workspace, memory::Allocator& alloc) + { + if(workspace != 0) + { + alloc.Deallocate(workspace->m_BlendArray); + alloc.Deallocate(workspace->m_TempWeightArray); + alloc.Deallocate(workspace->m_TempCropArray); + alloc.Deallocate(workspace->m_ChildInputVectorArray); + alloc.Deallocate(workspace); + } + } + + float WeightForIndex( const float* thresholdArray, mecanim::uint32_t count, mecanim::uint32_t index, float blend) + { + if( blend >= thresholdArray[index]) + { + if(index+1 == count) + { + return 1.0f; + } + else if(thresholdArray[index+1] < blend) + { + return 0.0f; + } + else + { + if(thresholdArray[index]-thresholdArray[index+1] != 0) + { + return (blend - thresholdArray[index+1]) / (thresholdArray[index]-thresholdArray[index+1]); + } + else + { + return thresholdArray[index]; + } + + } + } + else + { + if(index == 0) + { + return 1.0f; + } + else if(thresholdArray[index-1] > blend) + { + return 0.0f; + } + else + { + if(( thresholdArray[index]-thresholdArray[index-1]) != 0) + { + return (blend - thresholdArray[index-1]) / (thresholdArray[index]-thresholdArray[index-1]); + } + else + { + return thresholdArray[index]; + } + } + } + + } + + void GetWeightsSimpleDirectional (const Blend2dDataConstant& blendConstant, + float* weightArray, int* cropArray, Vector2f* workspaceBlendVectors, + float blendValueX, float blendValueY, bool preCompute = false) + { + // Get constants + const Vector2f* positionArray = blendConstant.m_ChildPositionArray.Get(); + mecanim::uint32_t count = blendConstant.m_ChildCount; + + if (weightArray == NULL || positionArray == NULL) + return; + + // Initialize all weights to 0 + for (int i=0; i<count; i++) + weightArray[i] = 0; + + // Handle fallback + if (count < 2) + { + if (count == 1) + weightArray[0] = 1; + return; + } + + Vector2f blendPosition = Vector2f (blendValueX, blendValueY); + + // Handle special case when sampled ecactly in the middle + if (blendPosition == Vector2f::zero) + { + // If we have a center motion, give that one all the weight + for (int i=0; i<count; i++) + { + if (positionArray[i] == Vector2f::zero) + { + weightArray[i] = 1; + return; + } + } + + // Otherwise divide weight evenly + float sharedWeight = 1.0f / count; + for (int i=0; i<count; i++) + weightArray[i] = sharedWeight; + return; + } + + int indexA = -1; + int indexB = -1; + int indexCenter = -1; + float maxDotForNegCross = -100000.0f; + float maxDotForPosCross = -100000.0f; + for (int i=0; i<count; i++) + { + if (positionArray[i] == Vector2f::zero) + { + if (indexCenter >= 0) + return; + indexCenter = i; + continue; + } + Vector2f posNormalized = Normalize (positionArray[i]); + float dot = Dot (posNormalized, blendPosition); + float cross = posNormalized.x * blendPosition.y - posNormalized.y * blendPosition.x; + if (cross > 0) + { + if (dot > maxDotForPosCross) + { + maxDotForPosCross = dot; + indexA = i; + } + } + else + { + if (dot > maxDotForNegCross) + { + maxDotForNegCross = dot; + indexB = i; + } + } + } + + float centerWeight = 0; + + if (indexA < 0 || indexB < 0) + { + // Fallback if sampling point is not inside a triangle + centerWeight = 1; + } + else + { + Vector2f a = positionArray[indexA]; + Vector2f b = positionArray[indexB]; + + // Calculate weights using barycentric coordinates + // (formulas from http://en.wikipedia.org/wiki/Barycentric_coordinate_system_%28mathematics%29 ) + float det = b.y*a.x - b.x*a.y; // Simplified from: (b.y-0)*(a.x-0) + (0-b.x)*(a.y-0); + float wA = (b.y*blendValueX - b.x*blendValueY) / det; // Simplified from: ((b.y-0)*(l.x-0) + (0-b.x)*(l.y-0)) / det; + float wB = (a.x*blendValueY - a.y*blendValueX) / det; // Simplified from: ((0-a.y)*(l.x-0) + (a.x-0)*(l.y-0)) / det; + centerWeight = 1 - wA - wB; + + // Clamp to be inside triangle + if (centerWeight < 0) + { + centerWeight = 0; + float sum = wA + wB; + wA /= sum; + wB /= sum; + } + else if (centerWeight > 1) + { + centerWeight = 1; + wA = 0; + wB = 0; + } + + // Give weight to the two vertices on the periphery that are closest + weightArray[indexA] = wA; + weightArray[indexB] = wB; + } + + if (indexCenter >= 0) + { + weightArray[indexCenter] = centerWeight; + } + else + { + // Give weight to all children when input is in the center + float sharedWeight = 1.0f / count; + for (int i=0; i<count; i++) + weightArray[i] += sharedWeight * centerWeight; + } + } + + const float kInversePI = 1 / kPI; + float GetWeightFreeformDirectional (const Blend2dDataConstant& blendConstant, Vector2f* workspaceBlendVectors, int i, int j, Vector2f blendPosition) + { + int pairIndex = i + j*blendConstant.m_ChildCount; + Vector2f vecIJ = blendConstant.m_ChildPairVectorArray[pairIndex]; + Vector2f vecIO = workspaceBlendVectors[i]; + vecIO.y *= blendConstant.m_ChildPairAvgMagInvArray[pairIndex]; + + if (blendConstant.m_ChildPositionArray[i] == Vector2f::zero) + vecIJ.x = workspaceBlendVectors[j].x; + else if (blendConstant.m_ChildPositionArray[j] == Vector2f::zero) + vecIJ.x = workspaceBlendVectors[i].x; + else if (vecIJ.x == 0 || blendPosition == Vector2f::zero) + vecIO.x = vecIJ.x; + + return 1 - Dot (vecIJ, vecIO) / SqrMagnitude (vecIJ); + } + + void GetWeightsFreeformDirectional (const Blend2dDataConstant& blendConstant, + float* weightArray, int* cropArray, Vector2f* workspaceBlendVectors, + float blendValueX, float blendValueY, bool preCompute = false) + { + // Get constants + const Vector2f* positionArray = blendConstant.m_ChildPositionArray.Get(); + mecanim::uint32_t count = blendConstant.m_ChildCount; + const float* constantMagnitudes = blendConstant.m_ChildMagnitudeArray.Get(); + const MotionNeighborList* constantChildNeighborLists = blendConstant.m_ChildNeighborListArray.Get(); + + Vector2f blendPosition = Vector2f (blendValueX, blendValueY); + float magO = Magnitude (blendPosition); + + if (blendPosition == Vector2f::zero) + { + for (int i=0; i<count; i++) + workspaceBlendVectors[i] = Vector2f (0, magO - constantMagnitudes[i]); + } + else + { + for (int i=0; i<count; i++) + { + if (positionArray[i] == Vector2f::zero) + workspaceBlendVectors[i] = Vector2f (0, magO - constantMagnitudes[i]); + else + { + float angle = Angle (positionArray[i], blendPosition); + if (positionArray[i].x * blendPosition.y - positionArray[i].y * blendPosition.x < 0) + angle = -angle; + workspaceBlendVectors[i] = Vector2f (angle, magO - constantMagnitudes[i]); + } + } + } + + if (preCompute) + { + for (int i=0; i<count; i++) + { + // Fade out over 180 degrees away from example + float value = 1 - abs (workspaceBlendVectors[i].x) * kInversePI; + cropArray[i] = -1; + for (int j=0; j<count; j++) + { + if (i==j) + continue; + + float newValue = GetWeightFreeformDirectional (blendConstant, workspaceBlendVectors, i, j, blendPosition); + + if (newValue <= 0) + { + value = 0; + cropArray[i] = -1; + break; + } + // Used for determining neighbors + if (newValue < value) + cropArray[i] = j; + value = min (value, newValue); + } + } + return; + } + + for (int i=0; i<count; i++) + { + // Fade out over 180 degrees away from example + float value = 1 - abs (workspaceBlendVectors[i].x) * kInversePI; + for (int jIndex=0; jIndex<constantChildNeighborLists[i].m_Count; jIndex++) + { + int j = constantChildNeighborLists[i].m_NeighborArray[jIndex]; + float newValue = GetWeightFreeformDirectional (blendConstant, workspaceBlendVectors, i, j, blendPosition); + if (newValue <= 0) + { + value = 0; + break; + } + value = min (value, newValue); + } + weightArray[i] = value; + } + + // Normalize weights + float summedWeight = 0; + for (int i=0; i<count; i++) + summedWeight += weightArray[i]; + + if (summedWeight > 0) + { + summedWeight = 1.0f / summedWeight; // Do division once instead of for every sample + for (int i=0; i<count; i++) + weightArray[i] *= summedWeight; + } + else + { + // Give weight to all children as fallback when no children have any weight. + // This happens when sampling in the center if no center motion is provided. + float evenWeight = 1.0f / count; + for (int i=0; i<count; i++) + weightArray[i] = evenWeight; + } + } + + void GetWeightsFreeformCartesian (const Blend2dDataConstant& blendConstant, + float* weightArray, int* cropArray, Vector2f* workspaceBlendVectors, + float blendValueX, float blendValueY, bool preCompute = false) + { + // Get constants + const Vector2f* positionArray = blendConstant.m_ChildPositionArray.Get(); + mecanim::uint32_t count = blendConstant.m_ChildCount; + const MotionNeighborList* constantChildNeighborLists = blendConstant.m_ChildNeighborListArray.Get(); + + Vector2f blendPosition = Vector2f (blendValueX, blendValueY); + for (int i=0; i<count; i++) + workspaceBlendVectors[i] = blendPosition - positionArray[i]; + + if (preCompute) + { + for (int i=0; i<count; i++) + { + cropArray[i] = -1; + Vector2f vecIO = workspaceBlendVectors[i]; + float value = 1; + for (int j=0; j<count; j++) + { + if (i==j) + continue; + + int pairIndex = i + j*blendConstant.m_ChildCount; + Vector2f vecIJ = blendConstant.m_ChildPairVectorArray[pairIndex]; + float newValue = 1 - Dot (vecIJ, vecIO) * blendConstant.m_ChildPairAvgMagInvArray[pairIndex]; + if (newValue <= 0) + { + value = 0; + cropArray[i] = -1; + break; + } + // Used for determining neighbors + if (newValue < value) + cropArray[i] = j; + value = min (value, newValue); + } + } + return; + } + + for (int i=0; i<count; i++) + { + Vector2f vecIO = workspaceBlendVectors[i]; + float value = 1; + for (int jIndex=0; jIndex<constantChildNeighborLists[i].m_Count; jIndex++) + { + int j = constantChildNeighborLists[i].m_NeighborArray[jIndex]; + if (i==j) + continue; + + int pairIndex = i + j*blendConstant.m_ChildCount; + Vector2f vecIJ = blendConstant.m_ChildPairVectorArray[pairIndex]; + float newValue = 1 - Dot (vecIJ, vecIO) * blendConstant.m_ChildPairAvgMagInvArray[pairIndex]; + if (newValue < 0) + { + value = 0; + break; + } + value = min (value, newValue); + } + weightArray[i] = value; + } + + // Normalize weights + float summedWeight = 0; + for (int i=0; i<count; i++) + summedWeight += weightArray[i]; + summedWeight = 1.0f / summedWeight; // Do division once instead of for every sample + for (int i=0; i<count; i++) + weightArray[i] *= summedWeight; + } + + void GetWeights1d (const Blend1dDataConstant& blendConstant, float* weightArray, float blendValue) + { + blendValue = math::clamp (blendValue, blendConstant.m_ChildThresholdArray[0], blendConstant.m_ChildThresholdArray[blendConstant.m_ChildCount-1]); + for (mecanim::uint32_t j = 0 ; j < blendConstant.m_ChildCount; j++) + weightArray[j] = WeightForIndex (blendConstant.m_ChildThresholdArray.Get (), blendConstant.m_ChildCount, j, blendValue); + } + + void GetWeights (const BlendTreeNodeConstant& nodeConstant, BlendTreeWorkspace &workspace, float* weightArray, float blendValueX, float blendValueY) + { + if (nodeConstant.m_BlendType == 0) + GetWeights1d (*nodeConstant.m_Blend1dData.Get(), weightArray, blendValueX); + else if (nodeConstant.m_BlendType == 1) + GetWeightsSimpleDirectional (*nodeConstant.m_Blend2dData.Get(), weightArray, workspace.m_TempCropArray, workspace.m_ChildInputVectorArray, blendValueX, blendValueY); + else if (nodeConstant.m_BlendType == 2) + GetWeightsFreeformDirectional (*nodeConstant.m_Blend2dData.Get(), weightArray, workspace.m_TempCropArray, workspace.m_ChildInputVectorArray, blendValueX, blendValueY); + else if (nodeConstant.m_BlendType == 3) + GetWeightsFreeformCartesian (*nodeConstant.m_Blend2dData.Get(), weightArray, workspace.m_TempCropArray, workspace.m_ChildInputVectorArray, blendValueX, blendValueY); + } + + uint32_t ComputeBlends(const BlendTreeConstant& constant, const BlendTreeInput &input, const BlendTreeMemory &memory, BlendTreeOutput &output, BlendTreeWorkspace &workspace) + { + uint32_t leafIndex = 0; + uint32_t currentOutputIndex = 0 ; + workspace.m_BlendArray[0] = 1; + + uint32_t i = 0; + for(i = 0 ; i < constant.m_NodeCount; i ++) + { + const BlendTreeNodeConstant* nodeConstant = constant.m_NodeArray[i].Get(); + + if(nodeConstant->m_ClipID != -1) + { + if(workspace.m_BlendArray[i] > 0) + { + float duration = IS_CONTENT_NEWER_OR_SAME(kUnityVersion4_3_a1) ? memory.m_NodeDurationArray[leafIndex] * nodeConstant->m_Duration : nodeConstant->m_Duration; + + output.m_OutputBlendArray[currentOutputIndex].m_ID = nodeConstant->m_ClipID; + output.m_OutputBlendArray[currentOutputIndex].m_BlendValue = workspace.m_BlendArray[i]; + output.m_OutputBlendArray[currentOutputIndex].m_Reverse = duration < 0; + output.m_OutputBlendArray[currentOutputIndex].m_CycleOffset = nodeConstant->m_CycleOffset; + output.m_OutputBlendArray[currentOutputIndex].m_Mirror = nodeConstant->m_Mirror; + + output.m_Duration += math::abs(duration) * workspace.m_BlendArray[i]; + currentOutputIndex++; + } + + leafIndex++; + } + else if(nodeConstant->m_ChildCount> 0) + { + if (nodeConstant->m_BlendType == 0) + { + // 1D blending + int32_t index = FindValueIndex(constant.m_BlendEventArrayConstant.Get(), nodeConstant->m_BlendEventID); + float blendValue; + input.m_BlendValueArray->ReadData(blendValue, constant.m_BlendEventArrayConstant->m_ValueArray[index].m_Index); + + GetWeights (*nodeConstant, workspace, workspace.m_TempWeightArray, blendValue, 0); + } + else if (nodeConstant->m_BlendType >= 1) + { + // 2D blending + int32_t indexX = FindValueIndex(constant.m_BlendEventArrayConstant.Get(), nodeConstant->m_BlendEventID); + int32_t indexY = FindValueIndex(constant.m_BlendEventArrayConstant.Get(), nodeConstant->m_BlendEventYID); + float blendValueX, blendValueY; + input.m_BlendValueArray->ReadData(blendValueX, constant.m_BlendEventArrayConstant->m_ValueArray[indexX].m_Index); + input.m_BlendValueArray->ReadData(blendValueY, constant.m_BlendEventArrayConstant->m_ValueArray[indexY].m_Index); + + GetWeights (*nodeConstant, workspace, workspace.m_TempWeightArray, blendValueX, blendValueY); + } + + for(mecanim::uint32_t j = 0 ; j < nodeConstant->m_ChildCount; j++) + { + float w = workspace.m_TempWeightArray[j]; + workspace.m_BlendArray[nodeConstant->m_ChildIndices[j]] = w * workspace.m_BlendArray[i]; + } + } + } + + return currentOutputIndex; + } + + void EvaluateBlendTree(const BlendTreeConstant& constant, const BlendTreeInput &input, const BlendTreeMemory &memory, BlendTreeOutput &output, BlendTreeWorkspace &workspace) + { + for(uint32_t i = 0 ; i < output.m_MaxBlendedClip ; i++) output.m_OutputBlendArray[i].m_ID = -1; + + output.m_Duration = 0; + uint32_t currentOutputIndex = 0; + + if(constant.m_NodeCount >0) + currentOutputIndex = ComputeBlends(constant, input, memory, output, workspace); + + if(currentOutputIndex == 0) + output.m_Duration = 1; + } + + + mecanim::uint32_t GetLeafCount(const BlendTreeConstant& constant) + { + mecanim::uint32_t leafCount = 0 ; + + for(int i = 0 ; i < constant.m_NodeCount ; i++) + { + if(constant.m_NodeArray[i]->m_ClipID != -1) + leafCount++; + } + + return leafCount; + } + + void FillLeafIDArray(const BlendTreeConstant& constant, uint32_t* leafIDArray) + { + uint32_t baseIndex = 0 ; + uint32_t i; + for(i = 0 ; i < constant.m_NodeCount ; i++) + { + if(constant.m_NodeArray[i]->m_ClipID != -1) + { + leafIDArray[baseIndex] = constant.m_NodeArray[i]->m_ClipID; + baseIndex++; + } + } + } + + mecanim::uint32_t GetMaxBlendCount(const BlendTreeConstant& constant, const BlendTreeNodeConstant& node) + { + uint32_t maxBlendCount = node.m_ClipID != -1 ? 1 : 0; + + // Blending occur between closest sibbling only + uint32_t current = 0; + uint32_t previous = 0; + + if(node.m_BlendType == 0 ) + { + for(int i = 0 ; i < node.m_ChildCount ; i++) + { + current = GetMaxBlendCount(constant, *constant.m_NodeArray[node.m_ChildIndices[i]]); + maxBlendCount = math::maximum( maxBlendCount, previous + current); + previous = current; + } + } + else + { + for(int i = 0 ; i < node.m_ChildCount ; i++) + { + maxBlendCount += GetMaxBlendCount(constant, *constant.m_NodeArray[node.m_ChildIndices[i]]); + } + } + + return maxBlendCount; + } + + mecanim::uint32_t GetMaxBlendCount(const BlendTreeConstant& constant) + { + uint32_t maxBlendCount = 0; + if(constant.m_NodeCount) + maxBlendCount = GetMaxBlendCount(constant, *constant.m_NodeArray[0]); + return maxBlendCount; + } + + +}// namespace animation + +}//namespace mecanim diff --git a/Runtime/mecanim/animation/blendtree.h b/Runtime/mecanim/animation/blendtree.h new file mode 100644 index 0000000..ee3f7a5 --- /dev/null +++ b/Runtime/mecanim/animation/blendtree.h @@ -0,0 +1,285 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/animation/curvedata.h" +#include "Runtime/mecanim/human/human.h" +#include "Runtime/mecanim/human/hand.h" +#include "Runtime/Math/Vector2.h" + +#include "Runtime/Serialize/Blobification/offsetptr.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Animation/MecanimArraySerialization.h" + +namespace mecanim +{ + +namespace animation +{ + struct MotionNeighborList + { + DEFINE_GET_TYPESTRING(MotionNeighborList) + + MotionNeighborList() : m_Count(0) + { + } + + uint32_t m_Count; + OffsetPtr<uint32_t> m_NeighborArray; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_Count); + MANUAL_ARRAY_TRANSFER2(uint32_t, m_NeighborArray, m_Count); + } + }; + + // Constant data for 1D blend node types - thresholds + struct Blend1dDataConstant + { + DEFINE_GET_TYPESTRING(Blend2dDataConstant) + + Blend1dDataConstant() : m_ChildCount(0) + { + } + + uint32_t m_ChildCount; + OffsetPtr<float> m_ChildThresholdArray; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_ChildCount); + MANUAL_ARRAY_TRANSFER2(float, m_ChildThresholdArray, m_ChildCount); + } + }; + + // Constant data for 2D blend node types - positions plus precomputed data to speed up blending + struct Blend2dDataConstant + { + DEFINE_GET_TYPESTRING(Blend2dDataConstant) + + Blend2dDataConstant() : m_ChildCount(0), m_ChildMagnitudeCount(0), m_ChildPairVectorCount(0), m_ChildPairAvgMagInvCount(0), m_ChildNeighborListCount(0) + { + } + + uint32_t m_ChildCount; + OffsetPtr<Vector2f> m_ChildPositionArray; + + uint32_t m_ChildMagnitudeCount; + OffsetPtr<float> m_ChildMagnitudeArray; // Used by type 2 + uint32_t m_ChildPairVectorCount; + OffsetPtr<Vector2f> m_ChildPairVectorArray; // Used by type 2, (3 TODO) + uint32_t m_ChildPairAvgMagInvCount; + OffsetPtr<float> m_ChildPairAvgMagInvArray; // Used by type 2 + uint32_t m_ChildNeighborListCount; + OffsetPtr<MotionNeighborList> m_ChildNeighborListArray; // Used by type 2, (3 TODO) + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_ChildCount); + MANUAL_ARRAY_TRANSFER2(Vector2f, m_ChildPositionArray, m_ChildCount); + + TRANSFER_BLOB_ONLY(m_ChildMagnitudeCount); + MANUAL_ARRAY_TRANSFER2(float, m_ChildMagnitudeArray, m_ChildMagnitudeCount); + TRANSFER_BLOB_ONLY(m_ChildPairVectorCount); + MANUAL_ARRAY_TRANSFER2(Vector2f, m_ChildPairVectorArray, m_ChildPairVectorCount); + TRANSFER_BLOB_ONLY(m_ChildPairAvgMagInvCount); + MANUAL_ARRAY_TRANSFER2(float, m_ChildPairAvgMagInvArray, m_ChildPairAvgMagInvCount); + TRANSFER_BLOB_ONLY(m_ChildNeighborListCount); + MANUAL_ARRAY_TRANSFER2(MotionNeighborList, m_ChildNeighborListArray, m_ChildNeighborListCount); + } + }; + + struct BlendTreeNodeConstant + { + DEFINE_GET_TYPESTRING(BlendTreeNodeConstant) + + BlendTreeNodeConstant(): m_BlendType(0), m_BlendEventID(-1), m_BlendEventYID(-1), m_ChildCount(0), m_ClipID(-1), m_Duration(0), m_CycleOffset(0), m_Mirror(false) + { + + } + + uint32_t m_BlendType; + + uint32_t m_BlendEventID; + uint32_t m_BlendEventYID; + uint32_t m_ChildCount; + OffsetPtr<uint32_t> m_ChildIndices; + + OffsetPtr<Blend1dDataConstant> m_Blend1dData; + OffsetPtr<Blend2dDataConstant> m_Blend2dData; + + uint32_t m_ClipID; // assert( m_ClipID != -1 && mClipBlendCount == 0) + float m_Duration; + float m_CycleOffset; + bool m_Mirror; + + // Unity 4.1 introduced 2D blendtrees. The data layout has been changed there. + template<class TransferFunction> + inline void Transfer_4_0_BackwardsCompatibility (TransferFunction& transfer) + { + if (transfer.IsOldVersion(1)) + { + if (m_Blend1dData.IsNull()) + { + mecanim::memory::ChainedAllocator* allocator = static_cast<mecanim::memory::ChainedAllocator*> (transfer.GetUserData()); + m_Blend1dData = allocator->Construct<Blend1dDataConstant>(); + } + + OffsetPtr<float>& m_ChildThresholdArray = m_Blend1dData->m_ChildThresholdArray; + MANUAL_ARRAY_TRANSFER2(float, m_ChildThresholdArray, m_Blend1dData->m_ChildCount); + } + } + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + transfer.SetVersion(2); + + TRANSFER(m_BlendType); + TRANSFER(m_BlendEventID); + TRANSFER(m_BlendEventYID); + + TRANSFER_BLOB_ONLY(m_ChildCount); + MANUAL_ARRAY_TRANSFER2(uint32_t, m_ChildIndices, m_ChildCount); + + TRANSFER(m_Blend1dData); + TRANSFER(m_Blend2dData); + + TRANSFER(m_ClipID); + TRANSFER(m_Duration); + + TRANSFER(m_CycleOffset); + TRANSFER(m_Mirror); + transfer.Align(); + + Transfer_4_0_BackwardsCompatibility(transfer); + + } + }; + + struct BlendTreeConstant + { + DEFINE_GET_TYPESTRING(BlendTreeConstant) + + BlendTreeConstant () :m_NodeCount(0) + { + } + + uint32_t m_NodeCount; + OffsetPtr< OffsetPtr<BlendTreeNodeConstant> > m_NodeArray; + + OffsetPtr<ValueArrayConstant> m_BlendEventArrayConstant; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_NodeCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<mecanim::animation::BlendTreeNodeConstant>, m_NodeArray, m_NodeCount); + + TRANSFER(m_BlendEventArrayConstant); + } + }; + + struct BlendTreeMemory + { + DEFINE_GET_TYPESTRING(BlendTreeMemory) + + BlendTreeMemory() : m_NodeCount(0) {} + + uint32_t m_NodeCount; + OffsetPtr<float> m_NodeDurationArray; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_NodeCount); + MANUAL_ARRAY_TRANSFER2(float, m_NodeDurationArray, m_NodeCount); + } + }; + + struct BlendTreeInput + { + BlendTreeInput() : m_BlendValueArray(0) + { + } + + ValueArray* m_BlendValueArray; + }; + + struct BlendTreeNodeOutput + { + BlendTreeNodeOutput() : m_BlendValue(0), m_ID(0), m_Reverse(false), m_Mirror(false), m_CycleOffset(0) + { + + } + + float m_BlendValue; + uint32_t m_ID; + bool m_Reverse; + bool m_Mirror; + float m_CycleOffset; + }; + + struct BlendTreeOutput + { + BlendTreeOutput() : m_Duration(0), + m_MaxBlendedClip(0) + {} + + BlendTreeNodeOutput* m_OutputBlendArray; + uint32_t m_MaxBlendedClip; + float m_Duration; + }; + + struct BlendTreeWorkspace + { + BlendTreeWorkspace() : m_BlendArray(0), m_TempWeightArray(0), m_TempCropArray(0), m_ChildInputVectorArray(0) + { + + } + + + float* m_BlendArray; + float* m_TempWeightArray; + int* m_TempCropArray; + Vector2f* m_ChildInputVectorArray; + }; + + void GetWeights (const BlendTreeNodeConstant& nodeConstant, BlendTreeWorkspace &workspace, float* weightArray, float blendValueX, float blendValueY); + + // Overload for creating 1D blend node + BlendTreeNodeConstant* CreateBlendTreeNodeConstant(uint32_t blendValueID, uint32_t childCount, uint32_t* childIndices, float* blendTreeThresholdArray, memory::Allocator& alloc); + // Overload for creating 2D blend node + BlendTreeNodeConstant* CreateBlendTreeNodeConstant(uint32_t blendValueID, uint32_t blendValueYID, int blendType, uint32_t childCount, uint32_t* childIndices, Vector2f* blendTreePositionArray, memory::Allocator& alloc); + // Overload for creating leaf blend node + BlendTreeNodeConstant* CreateBlendTreeNodeConstant(uint32_t clipID, float duration, bool mirror, float cycle, memory::Allocator& alloc); + + BlendTreeConstant* CreateBlendTreeConstant(BlendTreeNodeConstant** nodeArray, uint32_t nodeCount, memory::Allocator& alloc); + BlendTreeConstant* CreateBlendTreeConstant(uint32_t clipID, memory::Allocator& alloc); + void DestroyBlendTreeConstant(BlendTreeConstant * constant, memory::Allocator& alloc); + + BlendTreeMemory* CreateBlendTreeMemory(BlendTreeConstant const* constant, memory::Allocator& alloc); + void DestroyBlendTreeMemory(BlendTreeMemory *memory, memory::Allocator& alloc); + + BlendTreeInput* CreateBlendTreeInput(BlendTreeConstant const* constant, memory::Allocator& alloc); + void DestroyBlendTreeInput(BlendTreeInput * input, memory::Allocator& alloc); + + BlendTreeOutput* CreateBlendTreeOutput(BlendTreeConstant const* constant, uint32_t maxBlendedClip, memory::Allocator& alloc); + void DestroyBlendTreeOutput(BlendTreeOutput * output, memory::Allocator& alloc); + + BlendTreeWorkspace* CreateBlendTreeWorkspace(BlendTreeConstant const* constant, memory::Allocator& alloc); + void DestroyBlendTreeWorkspace(BlendTreeWorkspace * workspace, memory::Allocator& alloc); + + + void EvaluateBlendTree(const BlendTreeConstant& constant, const BlendTreeInput &input, const BlendTreeMemory &memory, BlendTreeOutput &output, BlendTreeWorkspace &workspace); + + mecanim::uint32_t GetLeafCount(const BlendTreeConstant& constant); + void FillLeafIDArray(const BlendTreeConstant& constant, uint32_t* leafIDArray); + mecanim::uint32_t GetMaxBlendCount(const BlendTreeConstant& constant); +} +} diff --git a/Runtime/mecanim/animation/clipmuscle.cpp b/Runtime/mecanim/animation/clipmuscle.cpp new file mode 100644 index 0000000..f6f53d2 --- /dev/null +++ b/Runtime/mecanim/animation/clipmuscle.cpp @@ -0,0 +1,1366 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/animation/clipmuscle.h" +#include "Runtime/mecanim/skeleton/skeleton.h" +#include "Runtime/mecanim/generic/stringtable.h" + +namespace mecanim +{ + +namespace animation +{ + struct MuscleIndexId + { + mecanim::uint32_t index; + mecanim::uint32_t id; + }; + + static mecanim::String s_ClipMuscleNameArray[s_ClipMuscleCurveCount]; + static MuscleIndexId s_ClipMuscleIndexIDArray[s_ClipMuscleCurveCount]; + + static void ClipMuscleNameArrayInit() + { + int i,j; + + i = 0; + s_ClipMuscleNameArray[i++] = "MotionT.x"; + s_ClipMuscleNameArray[i++] = "MotionT.y"; + s_ClipMuscleNameArray[i++] = "MotionT.z"; + + s_ClipMuscleNameArray[i++] = "MotionQ.x"; + s_ClipMuscleNameArray[i++] = "MotionQ.y"; + s_ClipMuscleNameArray[i++] = "MotionQ.z"; + s_ClipMuscleNameArray[i++] = "MotionQ.w"; + + s_ClipMuscleNameArray[i++] = "RootT.x"; + s_ClipMuscleNameArray[i++] = "RootT.y"; + s_ClipMuscleNameArray[i++] = "RootT.z"; + + s_ClipMuscleNameArray[i++] = "RootQ.x"; + s_ClipMuscleNameArray[i++] = "RootQ.y"; + s_ClipMuscleNameArray[i++] = "RootQ.z"; + s_ClipMuscleNameArray[i++] = "RootQ.w"; + + for(j=0;j<mecanim::human::kLastGoal;j++) + { + mecanim::String nameT(mecanim::human::BoneName(mecanim::human::s_HumanGoalInfo[j].m_Index)); + nameT += "T"; + mecanim::String nameTx = nameT; + nameTx += ".x"; + mecanim::String nameTy = nameT; + nameTy += ".y"; + mecanim::String nameTz = nameT; + nameTz += ".z"; + + mecanim::String nameQ(mecanim::human::BoneName(mecanim::human::s_HumanGoalInfo[j].m_Index)); + nameQ += "Q"; + mecanim::String nameQx = nameQ; + nameQx += ".x"; + mecanim::String nameQy = nameQ; + nameQy += ".y"; + mecanim::String nameQz = nameQ; + nameQz += ".z"; + mecanim::String nameQw = nameQ; + nameQw += ".w"; + + s_ClipMuscleNameArray[i++] = nameTx; + s_ClipMuscleNameArray[i++] = nameTy; + s_ClipMuscleNameArray[i++] = nameTz; + + s_ClipMuscleNameArray[i++] = nameQx; + s_ClipMuscleNameArray[i++] = nameQy; + s_ClipMuscleNameArray[i++] = nameQz; + s_ClipMuscleNameArray[i++] = nameQw; + } + + for(j=0;j<mecanim::human::kLastDoF;j++) + { + s_ClipMuscleNameArray[i++] = mecanim::human::MuscleName(j); + } + + for(int f=0;f<mecanim::hand::kLastFinger;++f) + { + for(int d=0;d<mecanim::hand::kLastFingerDoF;++d) + { + mecanim::String name("LeftHand."); + name += mecanim::hand::FingerName(f); + name += "."; + name += mecanim::hand::FingerDoFName(d); + + s_ClipMuscleNameArray[i++] = name; + } + } + + for(int f=0;f<mecanim::hand::kLastFinger;++f) + { + for(int d=0;d<mecanim::hand::kLastFingerDoF;++d) + { + mecanim::String name("RightHand."); + name += mecanim::hand::FingerName(f); + name += "."; + name += mecanim::hand::FingerDoFName(d); + + s_ClipMuscleNameArray[i++] = name; + } + } + } + + static bool MuscleIndexIdSortFunction(MuscleIndexId i, MuscleIndexId j) + { + return i.id < j.id; + } + + static void ClipMuscleIDArrayInit() + { + for(int i = 0; i < s_ClipMuscleCurveCount; i++) + { + s_ClipMuscleIndexIDArray[i].index = i; + s_ClipMuscleIndexIDArray[i].id = mecanim::processCRC32(GetMuscleCurveName(i)); + } + + std::sort(s_ClipMuscleIndexIDArray, s_ClipMuscleIndexIDArray + s_ClipMuscleCurveCount, MuscleIndexIdSortFunction); + } + + void InitializeMuscleClipTables () + { + ClipMuscleNameArrayInit(); + ClipMuscleIDArrayInit(); + } + + + mecanim::String const &GetMuscleCurveName(int32_t curveIndex) + { + Assert(s_ClipMuscleNameArray != NULL); + return s_ClipMuscleNameArray[curveIndex]; + } + + class MuscleIndexIdFindfunction + { + public: + + uint32_t mId; + + MuscleIndexIdFindfunction(uint32_t id) : mId(id) {} + + bool operator()(const MuscleIndexId &indexId) + { + return indexId.id == mId; + } + }; + + int32_t FindMuscleIndex(uint32_t id) + { + Assert(s_ClipMuscleIndexIDArray[0].id != 0); + + MuscleIndexId* end = s_ClipMuscleIndexIDArray + s_ClipMuscleCurveCount; + + const MuscleIndexId* found = std::find_if(s_ClipMuscleIndexIDArray, end, MuscleIndexIdFindfunction(id)); + if (found != end) + { + Assert(found->id == id); + return found->index; + } + else + return -1; + } + + int32_t GetMuscleCurveTQIndex(int32_t curveIndex) + { + return (curveIndex < (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount) ? curveIndex%s_ClipMuscleCurveTQCount : -1; + }; + + float GetXformCurveValue(math::xform const& x, int32_t index) + { + float ret = 0; + + if(index == 0) + { + ret = x.t.x().tofloat(); + } + else if(index == 1) + { + ret = x.t.y().tofloat(); + } + else if(index == 2) + { + ret = x.t.z().tofloat(); + } + else if(index == 3) + { + ret = x.q.x().tofloat(); + } + else if(index == 4) + { + ret = x.q.y().tofloat(); + } + else if(index == 5) + { + ret = x.q.z().tofloat(); + } + else if(index == 6) + { + ret = x.q.w().tofloat(); + } + + return ret; + } + + float GetMuscleCurveValue(human::HumanPose const& pose, math::xform const &motionX, int32_t curveIndex) + { + float ret = 0; + + if(curveIndex < s_ClipMuscleCurveTQCount) + { + ret = GetXformCurveValue(motionX,curveIndex); + } + else if(curveIndex < 2*s_ClipMuscleCurveTQCount) + { + ret = GetXformCurveValue(pose.m_RootX,curveIndex-s_ClipMuscleCurveTQCount); + } + else if(curveIndex < (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount) + { + int index = curveIndex - (2*s_ClipMuscleCurveTQCount); + int goalIndex = index / s_ClipMuscleCurveTQCount; + int xformIndex = index % s_ClipMuscleCurveTQCount; + + ret = GetXformCurveValue(pose.m_GoalArray[goalIndex].m_X,xformIndex); + } + else if(curveIndex < (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount + human::kLastDoF) + { + int dofIndex = curveIndex - (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount; + ret = pose.m_DoFArray[dofIndex]; + } + else if(curveIndex < (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount + human::kLastDoF + hand::s_DoFCount) + { + int dofIndex = curveIndex - ((2 + human::kLastGoal) * s_ClipMuscleCurveTQCount + human::kLastDoF); + ret = pose.m_LeftHandPose.m_DoFArray[dofIndex]; + } + else if(curveIndex < (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount + human::kLastDoF + 2 * hand::s_DoFCount) + { + int dofIndex = curveIndex - ((2 + human::kLastGoal) * s_ClipMuscleCurveTQCount + human::kLastDoF + hand::s_DoFCount); + ret = pose.m_RightHandPose.m_DoFArray[dofIndex]; + } + + return ret; + } + + bool GetMuscleCurveInMask(human::HumanPoseMask const& mask, int32_t curveIndex) + { + bool ret = false; + + if(curveIndex < s_ClipMuscleCurveTQCount) // root motion + { + ret = true; + } + else if(curveIndex < 2*s_ClipMuscleCurveTQCount) // root xform + { + ret = mask.test(human::kMaskRootIndex); + } + else if(curveIndex < (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount) + { + int index = curveIndex - (2*s_ClipMuscleCurveTQCount); + int goalIndex = index / s_ClipMuscleCurveTQCount; + + ret = mask.test(human::kMaskGoalStartIndex+goalIndex); + } + else if(curveIndex < (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount+ human::kLastDoF) + { + int dofIndex = curveIndex - (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount; + ret = mask.test(human::kMaskDoFStartIndex+dofIndex); + } + else if(curveIndex < (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount + human::kLastDoF + hand::s_DoFCount) + { + ret = mask.test(human::kMaskLeftHand); + } + else if(curveIndex < (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount + human::kLastDoF + 2 * hand::s_DoFCount) + { + ret = mask.test(human::kMaskRightHand); + } + + return ret; + } + + ClipMuscleConstant* CreateClipMuscleConstant(Clip * clip, memory::Allocator& alloc) + { + SETPROFILERLABEL(ClipMuscleConstant); + + ClipMuscleConstant *clipMuscle = alloc.Construct<ClipMuscleConstant>(); + + clipMuscle->m_Clip = clip; + clipMuscle->m_Mirror = false; + + clipMuscle->m_StartTime = 0; + clipMuscle->m_StopTime = 1; + + clipMuscle->m_Level = 0; + + clipMuscle->m_CycleOffset = 0; + clipMuscle->m_LoopTime = false; + clipMuscle->m_LoopBlend = false; + clipMuscle->m_LoopBlendOrientation = false; + clipMuscle->m_LoopBlendPositionY = false; + clipMuscle->m_LoopBlendPositionXZ = false; + clipMuscle->m_KeepOriginalOrientation = false; + clipMuscle->m_KeepOriginalPositionY = true; + clipMuscle->m_KeepOriginalPositionXZ = false; + clipMuscle->m_HeightFromFeet = false; + + clipMuscle->m_ValueArrayCount = GetClipCurveCount(*clip); + clipMuscle->m_ValueArrayDelta = alloc.ConstructArray<ValueDelta>(clipMuscle->m_ValueArrayCount); + + return clipMuscle; + } + + void DestroyClipMuscleConstant(ClipMuscleConstant * constant, memory::Allocator& alloc) + { + if(constant) + { + alloc.Deallocate(constant->m_ValueArrayDelta); + alloc.Deallocate(constant); + } + } + + void MotionOutputClear(MotionOutput *output) + { + output->m_DX.t = math::float4::zero(); + output->m_DX.q = math::quatIdentity(); + output->m_DX.s = math::float4::one(); + + output->m_MotionX.t = math::float4::zero(); + output->m_MotionX.q = math::quatIdentity(); + output->m_MotionX.s = math::float4::one(); + + output->m_MotionStartX.t = math::float4::zero(); + output->m_MotionStartX.q = math::quatIdentity(); + output->m_MotionStartX.s = math::float4::one(); + + output->m_MotionStopX.t = math::float4::zero(); + output->m_MotionStopX.q = math::quatIdentity(); + output->m_MotionStopX.s = math::float4::one(); + + output->m_PrevRootX.t = math::float4::zero(); + output->m_PrevRootX.q = math::quatIdentity(); + output->m_PrevRootX.s = math::float4::one(); + + output->m_PrevLeftFootX.t = math::float4::zero(); + output->m_PrevLeftFootX.q = math::quatIdentity(); + output->m_PrevLeftFootX.s = math::float4::one(); + + output->m_PrevRightFootX.t = math::float4::zero(); + output->m_PrevRightFootX.q = math::quatIdentity(); + output->m_PrevRightFootX.s = math::float4::one(); + + output->m_TargetX.t = math::float4::zero(); + output->m_TargetX.q = math::quatIdentity(); + output->m_TargetX.s = math::float4::one(); + + output->m_GravityWeight = 0; + } + + void MotionOutputCopy(MotionOutput *output, MotionOutput const *motion, bool hasRootMotion, bool isHuman, human::HumanPoseMask const &poseMask) + { + if(hasRootMotion || (isHuman && poseMask.test(human::kMaskRootIndex))) + { + output->m_DX = motion->m_DX; + output->m_GravityWeight = motion->m_GravityWeight; + } + + if(hasRootMotion) + { + output->m_MotionX = motion->m_MotionX; + output->m_MotionStartX = motion->m_MotionStartX; + output->m_MotionStopX = motion->m_MotionStopX; + } + + if(isHuman) + { + if(human::MaskHasLegs(poseMask)) + { + output->m_PrevRootX = motion->m_PrevRootX; + output->m_PrevLeftFootX = motion->m_PrevLeftFootX; + output->m_PrevRightFootX = motion->m_PrevRightFootX; + } + + if(poseMask.test(human::kMaskRootIndex)) + { + output->m_TargetX = motion->m_TargetX; + } + } + } + + void MotionOutputBlend(MotionOutput *output, MotionOutput **outputArray, float *weight, uint32_t count, bool hasRootMotion, bool isHuman, human::HumanPoseMask const &poseMask) + { + output->m_DX.t = math::float4::zero(); + output->m_DX.q = math::float4::zero(); + output->m_DX.s = math::float4::one(); + + output->m_MotionX.t = math::float4::zero(); + output->m_MotionX.q = math::float4::zero(); + output->m_MotionX.s = math::float4::one(); + + output->m_MotionStartX.t = math::float4::zero(); + output->m_MotionStartX.q = math::float4::zero(); + output->m_MotionStartX.s = math::float4::one(); + + output->m_MotionStopX.t = math::float4::zero(); + output->m_MotionStopX.q = math::float4::zero(); + output->m_MotionStopX.s = math::float4::one(); + + output->m_PrevRootX.t = math::float4::zero(); + output->m_PrevRootX.q = math::float4::zero(); + output->m_PrevRootX.s = math::float4::one(); + + output->m_PrevLeftFootX.t = math::float4::zero(); + output->m_PrevLeftFootX.q = math::float4::zero(); + output->m_PrevLeftFootX.s = math::float4::one(); + + output->m_PrevRightFootX.t = math::float4::zero(); + output->m_PrevRightFootX.q = math::float4::zero(); + output->m_PrevRightFootX.s = math::float4::one(); + + output->m_TargetX.t = math::float4::zero(); + output->m_TargetX.q = math::float4::zero(); + output->m_TargetX.s = math::float4::one(); + + output->m_GravityWeight = 0; + + float sumW = 0; + + for(int iter = 0; iter < count; iter++) + { + float w = weight[iter]; + + math::float1 w1(w); + + sumW += w; + + if(hasRootMotion || (isHuman && poseMask.test(human::kMaskRootIndex))) + { + output->m_DX.t += outputArray[iter]->m_DX.t*w1; + output->m_DX.q += math::cond(math::dot(output->m_DX.q,outputArray[iter]->m_DX.q) < math::float1::zero(),outputArray[iter]->m_DX.q * -w1,outputArray[iter]->m_DX.q * w1); + + output->m_GravityWeight += outputArray[iter]->m_GravityWeight*w; + } + + if(hasRootMotion) + { + output->m_MotionX.t += outputArray[iter]->m_MotionX.t*w1; + output->m_MotionX.q += math::cond(math::dot(output->m_MotionX.q,outputArray[iter]->m_MotionX.q) < math::float1::zero(),outputArray[iter]->m_MotionX.q * -w1,outputArray[iter]->m_MotionX.q * w1); + + output->m_MotionStartX.t += outputArray[iter]->m_MotionStartX.t*w1; + output->m_MotionStartX.q += math::cond(math::dot(output->m_MotionStartX.q,outputArray[iter]->m_MotionStartX.q) < math::float1::zero(),outputArray[iter]->m_MotionStartX.q * -w1,outputArray[iter]->m_MotionStartX.q * w1); + + output->m_MotionStopX.t += outputArray[iter]->m_MotionStopX.t*w1; + output->m_MotionStopX.q += math::cond(math::dot(output->m_MotionStopX.q,outputArray[iter]->m_MotionStopX.q) < math::float1::zero(),outputArray[iter]->m_MotionStopX.q * -w1,outputArray[iter]->m_MotionStopX.q * w1); + } + + if(isHuman) + { + if(human::MaskHasLegs(poseMask)) + { + output->m_PrevRootX.t += outputArray[iter]->m_PrevRootX.t*w1; + output->m_PrevRootX.q += math::cond(math::dot(output->m_PrevRootX.q,outputArray[iter]->m_PrevRootX.q) < math::float1::zero(),outputArray[iter]->m_PrevRootX.q * -w1,outputArray[iter]->m_PrevRootX.q * w1); + + output->m_PrevLeftFootX.t += outputArray[iter]->m_PrevLeftFootX.t*w1; + output->m_PrevLeftFootX.q += math::cond(math::dot(output->m_PrevLeftFootX.q,outputArray[iter]->m_PrevLeftFootX.q) < math::float1::zero(),outputArray[iter]->m_PrevLeftFootX.q * -w1,outputArray[iter]->m_PrevLeftFootX.q * w1); + + output->m_PrevRightFootX.t += outputArray[iter]->m_PrevRightFootX.t*w1; + output->m_PrevRightFootX.q += math::cond(math::dot(output->m_PrevRightFootX.q,outputArray[iter]->m_PrevRightFootX.q) < math::float1::zero(),outputArray[iter]->m_PrevRightFootX.q * -w1,outputArray[iter]->m_PrevRightFootX.q * w1); + } + + if(poseMask.test(human::kMaskRootIndex)) + { + output->m_TargetX.t += outputArray[iter]->m_TargetX.t*w1; + output->m_TargetX.q += math::cond(math::dot(output->m_TargetX.q,outputArray[iter]->m_TargetX.q) < math::float1::zero(),outputArray[iter]->m_TargetX.q * -w1,outputArray[iter]->m_TargetX.q * w1); + } + } + } + + math::float4 q(0,0,0,math::saturate(1.0f-sumW)); + + if(hasRootMotion || (isHuman && poseMask.test(human::kMaskRootIndex))) + { + output->m_DX.q = math::normalize(output->m_DX.q+q); + if(sumW > 0) output->m_GravityWeight /= sumW; + } + + if(hasRootMotion) + { + output->m_MotionX.q = math::normalize(output->m_MotionX.q+q); + output->m_MotionStartX.q = math::normalize(output->m_MotionStartX.q+q); + output->m_MotionStopX.q = math::normalize(output->m_MotionStopX.q+q); + } + + if(isHuman) + { + if(human::MaskHasLegs(poseMask)) + { + output->m_PrevRootX.q = math::normalize(output->m_PrevRootX.q+q); + output->m_PrevLeftFootX.q = math::normalize(output->m_PrevLeftFootX.q+q); + output->m_PrevRightFootX.q = math::normalize(output->m_PrevRightFootX.q+q); + } + + if(poseMask.test(human::kMaskRootIndex)) + { + output->m_TargetX.q = math::normalize(output->m_TargetX.q+q); + } + } + } + + void MotionAddAdditiveLayer(MotionOutput *output, MotionOutput const *motion, float weight, bool hasRootMotion, bool isHuman, human::HumanPoseMask const &poseMask) + { + math::float1 w(weight); + + if(hasRootMotion || (isHuman && poseMask.test(human::kMaskRootIndex))) + { + output->m_DX = math::xformMul(output->m_DX, math::xformWeight(motion->m_DX,w)); + output->m_GravityWeight = output->m_GravityWeight + motion->m_GravityWeight * weight; + } + + if(hasRootMotion) + { + output->m_MotionX = math::xformMul(output->m_MotionX, math::xformWeight(motion->m_MotionX,w)); + output->m_MotionStartX = math::xformMul(output->m_MotionStartX, math::xformWeight(motion->m_MotionStartX,w)); + output->m_MotionStopX = math::xformMul(output->m_MotionStopX, math::xformWeight(motion->m_MotionStopX,w)); + } + + if(isHuman) + { + if(human::MaskHasLegs(poseMask)) + { + output->m_PrevRootX = math::xformMul(output->m_PrevRootX, math::xformWeight(motion->m_PrevRootX,w)); + output->m_PrevLeftFootX = math::xformMul(output->m_PrevLeftFootX, math::xformWeight(motion->m_PrevLeftFootX,w)); + output->m_PrevRightFootX = math::xformMul(output->m_PrevRightFootX, math::xformWeight(motion->m_PrevRightFootX,w)); + } + + if(poseMask.test(human::kMaskRootIndex)) + { + output->m_TargetX = math::xformMul(output->m_TargetX, math::xformWeight(motion->m_TargetX,w)); + } + } + } + + void MotionAddOverrideLayer(MotionOutput *output, MotionOutput const *motion, float weight, bool hasRootMotion, bool isHuman, human::HumanPoseMask const &poseMask) + { + if(weight < 1.0f) + { + math::float1 w(weight); + + if(hasRootMotion || (isHuman && poseMask.test(human::kMaskRootIndex))) + { + output->m_DX = math::xformBlend(output->m_DX,motion->m_DX,w); + output->m_GravityWeight = math::lerp(output->m_GravityWeight,motion->m_GravityWeight,weight); + } + + if(hasRootMotion) + { + output->m_MotionX = math::xformBlend(output->m_MotionX,motion->m_MotionX,w); + output->m_MotionStartX = math::xformBlend(output->m_MotionStartX,motion->m_MotionStartX,w); + output->m_MotionStopX = math::xformBlend(output->m_MotionStopX,motion->m_MotionStopX,w); + } + + if(isHuman) + { + if(human::MaskHasLegs(poseMask)) + { + output->m_PrevRootX = math::xformBlend(output->m_PrevRootX,motion->m_PrevRootX,w); + output->m_PrevLeftFootX = math::xformBlend(output->m_PrevLeftFootX,motion->m_PrevLeftFootX,w); + output->m_PrevRightFootX = math::xformBlend(output->m_PrevRightFootX,motion->m_PrevRightFootX,w); + } + + if(poseMask.test(human::kMaskRootIndex)) + { + output->m_TargetX = math::xformBlend(output->m_TargetX,motion->m_TargetX,w); + } + } + } + else + { + MotionOutputCopy(output,motion,hasRootMotion,isHuman,poseMask); + } + } + + ClipMuscleInput* CreateClipMuscleInput(ClipMuscleConstant const* constant, memory::Allocator& alloc) + { + SETPROFILERLABEL(ClipMuscleInput); + + ClipMuscleInput *in = alloc.Construct<ClipMuscleInput>(); + return in; + } + + void DestroyClipMuscleInput(ClipMuscleInput * input, memory::Allocator& alloc) + { + if(input) + { + alloc.Deallocate(input); + } + } + + float ComputeClipTime(float normalizedTimeIn, float startTime, float stopTime, float cycleOffset, bool loop, bool reverse, float &normalizedTimeOut, float &timeInt) + { + float timeFrac = math::cond(loop,math::modf(normalizedTimeIn+cycleOffset,timeInt),math::saturate(normalizedTimeIn)); + normalizedTimeOut = math::cond(!reverse, timeFrac, 1.f - timeFrac); + return startTime+normalizedTimeOut*(stopTime-startTime); + } + + math::xform EvaluateMotion(ClipMuscleConstant const& constant, ClipMemory &memory, float time) + { + math::xform ret; + + float ATTRIBUTE_ALIGN(ALIGN4F) value[4]; + + ClipInput clipIn; + clipIn.m_Time = time; + + value[0] = constant.m_IndexArray[0] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[0]) : 0; + value[1] = constant.m_IndexArray[1] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[1]) : 0; + value[2] = constant.m_IndexArray[2] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[2]) : 0; + value[3] = 0.f; + + ret.t = math::load(value); + + value[0] = constant.m_IndexArray[3] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[3]) : 0; + value[1] = constant.m_IndexArray[4] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[4]) : 0; + value[2] = constant.m_IndexArray[5] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[5]) : 0; + value[3] = constant.m_IndexArray[6] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[6]) : 1; + + ret.q = math::normalize(math::load(value)); + + return ret; + } + + math::xform EvaluateRoot(ClipMuscleConstant const& constant, ClipMemory &memory, float time) + { + math::xform ret; + + float ATTRIBUTE_ALIGN(ALIGN4F) value[4]; + + ClipInput clipIn; + clipIn.m_Time = time; + + value[0] = constant.m_IndexArray[7] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[7]) : 0; + value[1] = constant.m_IndexArray[8] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[8]) : 0; + value[2] = constant.m_IndexArray[9] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[9]) : 0; + value[3] = 0.f; + + ret.t = math::load(value); + + value[0] = constant.m_IndexArray[10] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[10]) : 0; + value[1] = constant.m_IndexArray[11] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[11]) : 0; + value[2] = constant.m_IndexArray[12] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[12]) : 0; + value[3] = constant.m_IndexArray[13] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory, constant.m_IndexArray[13]) : 1; + + ret.q = math::normalize(math::load(value)); + + return ret; + } + + math::xform EvaluateGoal(ClipMuscleConstant const& constant, ClipMemory &memory, float time, int32_t goalIndex) + { + math::xform ret; + + float ATTRIBUTE_ALIGN(ALIGN4F) value[4]; + + const int32_t index = (2 + goalIndex) * s_ClipMuscleCurveTQCount; + + ClipInput clipIn; + clipIn.m_Time = time; + + value[0] = constant.m_IndexArray[index+0] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory,constant.m_IndexArray[index+0]) : 0; + value[1] = constant.m_IndexArray[index+1] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory,constant.m_IndexArray[index+1]) : 0; + value[2] = constant.m_IndexArray[index+2] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory,constant.m_IndexArray[index+2]) : 0; + value[3] = 0.f; + + ret.t = math::load(value); + + value[0] = constant.m_IndexArray[index+3] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory,constant.m_IndexArray[index+3]) : 0; + value[1] = constant.m_IndexArray[index+4] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory,constant.m_IndexArray[index+4]) : 0; + value[2] = constant.m_IndexArray[index+5] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory,constant.m_IndexArray[index+5]) : 0; + value[3] = constant.m_IndexArray[index+6] != -1 ? EvaluateClipAtIndex(constant.m_Clip.Get(), &clipIn, &memory,constant.m_IndexArray[index+6]) : 1; + + ret.q = math::normalize(math::load(value)); + + return ret; + } + + math::xform GetMotionX(const ClipMuscleConstant& constant, const ClipOutput &output) + { + math::xform ret; + + float ATTRIBUTE_ALIGN(ALIGN4F) value[4]; + + uint32_t currentCurveIndex = 0; + + if(constant.m_IndexArray[currentCurveIndex] != -1) value[0] = output.m_Values[constant.m_IndexArray[currentCurveIndex]]; else value[0] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[1] = output.m_Values[constant.m_IndexArray[currentCurveIndex]]; else value[1] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[2] = output.m_Values[constant.m_IndexArray[currentCurveIndex]]; else value[2] = 0; currentCurveIndex++; + value[3] = 0; + + ret.t = math::load(value); + + if(constant.m_IndexArray[currentCurveIndex] != -1) value[0] = output.m_Values[constant.m_IndexArray[currentCurveIndex]]; else value[0] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[1] = output.m_Values[constant.m_IndexArray[currentCurveIndex]]; else value[1] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[2] = output.m_Values[constant.m_IndexArray[currentCurveIndex]]; else value[2] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[3] = output.m_Values[constant.m_IndexArray[currentCurveIndex]]; else value[3] = 1.0f; currentCurveIndex++; + + ret.q = math::normalize(math::load(value)); + + ret.s = math::float4::one(); + + return ret; + } + + template<typename TYPE> + struct ValueAccessor + { + ValueAccessor(const TYPE* t) : m_Values(t) + {} + + const TYPE* m_Values; + + float operator[](int index) + { + return m_Values[index]; + } + }; + + template<> + struct ValueAccessor<ValueDelta> + { + ValueAccessor(const ValueDelta* t) : m_Values(t) + {} + + const ValueDelta* m_Values; + + float operator[](int index) + { + return m_Values[index].m_Start; + } + }; + + template<typename TYPE> + void GetHumanPose(const ClipMuscleConstant& constant, const TYPE* values, human::HumanPose &humanPose) + { + float ATTRIBUTE_ALIGN(ALIGN4F) value[4]; + + ValueAccessor<TYPE> accessor(values); + + uint32_t i,currentCurveIndex = s_ClipMotionCurveCount; + + if(constant.m_IndexArray[currentCurveIndex] != -1) value[0] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[0] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[1] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[1] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[2] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[2] = 0; currentCurveIndex++; + value[3] = 0; + + humanPose.m_RootX.t = math::load(value); + + if(constant.m_IndexArray[currentCurveIndex] != -1) value[0] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[0] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[1] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[1] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[2] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[2] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[3] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[3] = 1.0f; currentCurveIndex++; + + humanPose.m_RootX.q = math::normalize(math::load(value)); + humanPose.m_RootX.s = math::float4::one(); + + for(i = 0; i < human::kLastGoal; i++) + { + if(constant.m_IndexArray[currentCurveIndex] != -1) value[0] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[0] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[1] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[1] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[2] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[2] = 0; currentCurveIndex++; + value[3] = 0; + + humanPose.m_GoalArray[i].m_X.t = math::load(value); + + if(constant.m_IndexArray[currentCurveIndex] != -1) value[0] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[0] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[1] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[1] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[2] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[2] = 0; currentCurveIndex++; + if(constant.m_IndexArray[currentCurveIndex] != -1) value[3] = accessor[constant.m_IndexArray[currentCurveIndex]]; else value[3] = 1.0f; currentCurveIndex++; + + + humanPose.m_GoalArray[i].m_X.q = math::load(value); + humanPose.m_GoalArray[i].m_X.q = math::normalize(humanPose.m_GoalArray[i].m_X.q); + } + + for(i = 0 ; i < human::kLastDoF; i++) + { + if(constant.m_IndexArray[currentCurveIndex] != -1) humanPose.m_DoFArray[i] = accessor[constant.m_IndexArray[currentCurveIndex]]; + else humanPose.m_DoFArray[i] = 0; + currentCurveIndex++; + } + + for(i = 0 ; i < hand::s_DoFCount; i++) + { + if(constant.m_IndexArray[currentCurveIndex] != -1) humanPose.m_LeftHandPose.m_DoFArray[i] = accessor[constant.m_IndexArray[currentCurveIndex]]; + else humanPose.m_LeftHandPose.m_DoFArray[i] = 0; + currentCurveIndex++; + } + + for(i = 0 ; i < hand::s_DoFCount; i++) + { + if(constant.m_IndexArray[currentCurveIndex] != -1) humanPose.m_RightHandPose.m_DoFArray[i] = accessor[constant.m_IndexArray[currentCurveIndex]]; + else humanPose.m_RightHandPose.m_DoFArray[i] = 0; + currentCurveIndex++; + } + } + + void GetHumanPose(const ClipMuscleConstant& constant, const float* values, human::HumanPose &humanPose) + { + GetHumanPose<float>(constant, values, humanPose); + } + + void GetHumanPose(const ClipMuscleConstant& constant, const ValueDelta* values, human::HumanPose &humanPose) + { + GetHumanPose<ValueDelta>(constant, values, humanPose); + } + + + // root motion extraction WIP + void EvaluateClipRootMotionDeltaX(const ClipMuscleConstant& constant, const ClipMuscleInput &input, MotionOutput &output, ClipMemory &memory) + { + output.m_DX = math::xformIdentity(); + output.m_MotionX = math::xformIdentity(); + + if(constant.m_IndexArray[0] != -1) + { + float cycleOffset = constant.m_CycleOffset + input.m_CycleOffset; + + float prevTime; + float prevTimeFrac = math::cond(constant.m_LoopTime,math::modf(input.m_PreviousTime+cycleOffset,prevTime),math::saturate(input.m_PreviousTime)); + prevTimeFrac = math::cond(!input.m_Reverse, prevTimeFrac, 1.f - prevTimeFrac); + prevTime = constant.m_StartTime+prevTimeFrac*(constant.m_StopTime-constant.m_StartTime); + + float currentTime; + float lTimeFrac = math::cond(constant.m_LoopTime,math::modf(input.m_Time+cycleOffset,currentTime),math::saturate(input.m_Time)); + lTimeFrac = math::cond(!input.m_Reverse, lTimeFrac, 1.f - lTimeFrac); + currentTime = constant.m_StartTime+lTimeFrac*(constant.m_StopTime-constant.m_StartTime); + + math::xform refStartX(constant.m_MotionStartX); + math::xform refStopX(constant.m_MotionStopX); + math::xform prevRefX(EvaluateMotion(constant,memory,prevTime)); + math::xform refX(EvaluateMotion(constant,memory,currentTime)); + + refStartX.q = math::quatProjOnYPlane(refStartX.q); + refStopX.q = math::quatProjOnYPlane(refStopX.q); + prevRefX.q = math::quatProjOnYPlane(prevRefX.q); + refX.q = math::quatProjOnYPlane(refX.q); + + math::float4 refOffsetT = math::float4(0,constant.m_Level,0,0); + math::float4 refOffsetQ = math::qtan2Quat(math::float4(0,math::halfTan(math::radians(constant.m_OrientationOffsetY)),0,1)); + + if(constant.m_KeepOriginalPositionY) + { + refOffsetT.y() -= refStartX.t.y(); + } + + if(constant.m_KeepOriginalPositionXZ) + { + refOffsetT.x() -= refStartX.t.x(); + refOffsetT.z() -= refStartX.t.z(); + } + + if(constant.m_KeepOriginalOrientation) + { + refOffsetQ = math::normalize(math::quatMul(refOffsetQ,math::quatConj(refStartX.q))); + } + + refStartX.t = refStartX.t + refOffsetT; + refStopX.t = refStopX.t + refOffsetT; + prevRefX.t = prevRefX.t + refOffsetT; + refX.t = refX.t + refOffsetT; + + refStartX.q = math::normalize(math::quatMul(refOffsetQ,refStartX.q)); + refStopX.q = math::normalize(math::quatMul(refOffsetQ,refStopX.q)); + prevRefX.q = math::normalize(math::quatMul(refOffsetQ,prevRefX.q)); + refX.q = math::normalize(math::quatMul(refOffsetQ,refX.q)); + + if(constant.m_LoopBlendOrientation) + { + refStopX.q = refStartX.q; + prevRefX.q = refStartX.q; + refX.q = refStartX.q; + } + + if(constant.m_LoopBlendPositionY) + { + refStopX.t.y() = refStartX.t.y(); + prevRefX.t.y() = refStartX.t.y(); + refX.t.y() = refStartX.t.y(); + } + + if(constant.m_LoopBlendPositionXZ) + { + refStartX.t.x() = refStartX.t.x(); + refStopX.t.x() = refStartX.t.x();; + prevRefX.t.x() = refStartX.t.x();; + refX.t.x() = refStartX.t.x();; + + refStartX.t.z() = refStartX.t.z(); + refStopX.t.z() = refStartX.t.z();; + prevRefX.t.z() = refStartX.t.z();; + refX.t.z() = refStartX.t.z();; + } + + output.m_MotionX = refX; + output.m_MotionStartX = refStartX; + output.m_MotionStopX = refStopX; + + if(constant.m_LoopTime) + { + float deltaTime = math::abs(lTimeFrac - prevTimeFrac); + + if(deltaTime > 0.5f) + { + if(lTimeFrac < prevTimeFrac) + { + output.m_DX = math::xformInvMulNS(prevRefX,math::xformMul(refStopX,math::xformInvMulNS(refStartX,refX))); + } + else if(lTimeFrac > prevTimeFrac) + { + output.m_DX = math::xformInvMulNS(prevRefX,math::xformMul(refStartX,math::xformInvMulNS(refStopX,refX))); + } + } + else if(lTimeFrac != prevTimeFrac) + { + output.m_DX = math::xformInvMulNS(prevRefX,refX); + } + } + else + { + if(input.m_PreviousTime != input.m_Time) + { + output.m_DX = math::xformInvMulNS(prevRefX,refX); + } + } + + output.m_DX.q = math::quat2Qtan(output.m_DX.q); + + output.m_DX.q.x() = 0; + output.m_DX.q.z() = 0; + + if(constant.m_LoopBlendPositionY) + { + output.m_DX.t.y() = 0; + } + + if(constant.m_LoopBlendPositionXZ) + { + output.m_DX.t.x() = 0; + output.m_DX.t.z() = 0; + } + + if(constant.m_LoopBlendOrientation) + { + output.m_DX.q.y() = 0; + } + + output.m_DX.q = math::qtan2Quat(output.m_DX.q); + } + } + + void EvaluateClipMusclePrevTime(const ClipMuscleConstant& constant, const ClipMuscleInput &input, MotionOutput &output, ClipMemory &memory) + { + float prevTimeInt; + float prevTimeFrac; + float prevTime = ComputeClipTime(input.m_PreviousTime,constant.m_StartTime,constant.m_StopTime,constant.m_CycleOffset+input.m_CycleOffset,constant.m_LoopTime,input.m_Reverse,prevTimeFrac,prevTimeInt); + + output.m_PrevRootX = EvaluateRoot(constant,memory,prevTime); + output.m_PrevLeftFootX = EvaluateGoal(constant,memory,prevTime,human::kLeftFootGoal); + output.m_PrevRightFootX = EvaluateGoal(constant,memory,prevTime,human::kRightFootGoal); + } + + void EvaluateClipMuscle(const ClipMuscleConstant& constant, const ClipMuscleInput &input, const float *valuesOutput, MotionOutput &motionOutput, human::HumanPose &humanPose, ClipMemory &memory) + { + float cycleOffset = constant.m_CycleOffset+input.m_CycleOffset; + float prevTimeInt; + float prevTimeFrac; + ComputeClipTime(input.m_PreviousTime,constant.m_StartTime,constant.m_StopTime,cycleOffset,constant.m_LoopTime,input.m_Reverse,prevTimeFrac,prevTimeInt); + + ClipInput in; + float timeFrac; + float currentTimeInt; + in.m_Time = ComputeClipTime(input.m_Time,constant.m_StartTime,constant.m_StopTime,cycleOffset,constant.m_LoopTime,input.m_Reverse,timeFrac,currentTimeInt); + + bool mirror = (constant.m_Mirror || input.m_Mirror) && !(constant.m_Mirror && input.m_Mirror); + + math::xform stopX = math::xformMulInv(constant.m_StartX,constant.m_DeltaPose.m_RootX); + + math::xform prevRootX = motionOutput.m_PrevRootX ; + math::xform prevLeftFootX = motionOutput.m_PrevLeftFootX; + math::xform prevRightFootX = motionOutput.m_PrevRightFootX; + + GetHumanPose(constant,valuesOutput,humanPose); + math::xform rootX = humanPose.m_RootX; + + math::xform refStartX(constant.m_StartX.t,math::quatProjOnYPlane(constant.m_StartX.q),constant.m_StartX.s); + math::xform refStopX(stopX.t,math::quatProjOnYPlane(stopX.q),stopX.s); + math::xform prevRefX(prevRootX.t,math::quatProjOnYPlane(prevRootX.q),prevRootX.s); + math::xform refX(rootX.t,math::quatProjOnYPlane(rootX.q),rootX.s); + + // todo: target stuff most likely breaks curve cache + float targetTime = constant.m_StopTime; + float targetTimeFrac = 1; + math::xform targetRootX = stopX; + math::xform targetRefX = refStopX; + math::xform targetGoalX = math::xformIdentity(); + + float targetTimeInt = 0; + if(input.m_TargetTime != 1) + { + targetTime = ComputeClipTime(input.m_TargetTime,constant.m_StartTime,constant.m_StopTime,cycleOffset,constant.m_LoopTime,input.m_Reverse,targetTimeFrac,targetTimeInt); + + targetRootX = EvaluateRoot(constant,memory,targetTime); + targetRefX = math::xform(targetRootX.t,math::quatProjOnYPlane(targetRootX.q),targetRootX.s); + } + + int32_t targetGoalIndex = input.m_TargetIndex - kTargetLeftFoot; + + if(targetGoalIndex >= 0) + { + targetGoalIndex = mirror ? ((targetGoalIndex % 2) ? targetGoalIndex-1 : targetGoalIndex+1) : targetGoalIndex; + targetGoalX = EvaluateGoal(constant,memory,targetTime,targetGoalIndex); + } + ////////////////////////////////////////// + + if(constant.m_HeightFromFeet) + { + math::xform refLeftFootStartX = math::xformMul(constant.m_StartX,constant.m_LeftFootStartX); + math::xform refRightFootStartX = math::xformMul(constant.m_StartX,constant.m_RightFootStartX); + + math::xform refLeftFootStopX = math::xformMul(stopX,math::xformMulInv(constant.m_LeftFootStartX,constant.m_DeltaPose.m_GoalArray[human::kLeftFootGoal].m_X)); + math::xform refRightFootStopX = math::xformMul(stopX,math::xformMulInv(constant.m_RightFootStartX,constant.m_DeltaPose.m_GoalArray[human::kRightFootGoal].m_X)); + + math::xform refLeftFootPrevX = math::xformMul(prevRootX,prevLeftFootX); + math::xform refRightFootPrevX = math::xformMul(prevRootX,prevRightFootX); + + math::xform refLeftFootX = math::xformMul(rootX,humanPose.m_GoalArray[human::kLeftFootGoal].m_X); + math::xform refRightFootX = math::xformMul(rootX,humanPose.m_GoalArray[human::kRightFootGoal].m_X); + + math::xform refTargetLeftFootX = math::xformMul(targetRootX,EvaluateGoal(constant,memory,targetTime,human::kLeftFootGoal)); + math::xform refTargetRightFootX = math::xformMul(targetRootX,EvaluateGoal(constant,memory,targetTime,human::kRightFootGoal)); + + refStartX.t.y() = math::minimum(refStartX.t,math::minimum(refLeftFootStartX.t,refRightFootStartX.t)).y(); + refStopX.t.y() = math::minimum(refStopX.t,math::minimum(refLeftFootStopX.t,refRightFootStopX.t)).y(); + prevRefX.t.y() = math::minimum(prevRefX.t,math::minimum(refLeftFootPrevX.t,refRightFootPrevX.t)).y(); + refX.t.y() = math::minimum(refX.t,math::minimum(refLeftFootX.t,refRightFootX.t)).y(); + targetRefX.t.y() = math::minimum(targetRefX.t,math::minimum(refTargetLeftFootX.t,refTargetRightFootX.t)).y(); + } + + math::float4 refOffsetT = math::float4(0,constant.m_Level,0,0); + math::float4 refOffsetQ = math::qtan2Quat(math::float4(0,math::halfTan(math::radians(constant.m_OrientationOffsetY)),0,1)); + + if(constant.m_KeepOriginalPositionY) + { + refOffsetT.y() -= refStartX.t.y(); + } + + if(constant.m_KeepOriginalPositionXZ) + { + refOffsetT.x() -= refStartX.t.x(); + refOffsetT.z() -= refStartX.t.z(); + } + + if(constant.m_KeepOriginalOrientation) + { + refOffsetQ = math::normalize(math::quatMul(refOffsetQ,math::quatConj(refStartX.q))); + } + + refStartX.t = refStartX.t + refOffsetT; + refStopX.t = refStopX.t + refOffsetT; + prevRefX.t = prevRefX.t + refOffsetT; + refX.t = refX.t + refOffsetT; + targetRefX.t = targetRefX.t + refOffsetT; + + refStartX.q = math::normalize(math::quatMul(refOffsetQ,refStartX.q)); + refStopX.q = math::normalize(math::quatMul(refOffsetQ,refStopX.q)); + prevRefX.q = math::normalize(math::quatMul(refOffsetQ,prevRefX.q)); + refX.q = math::normalize(math::quatMul(refOffsetQ,refX.q)); + targetRefX.q = math::normalize(math::quatMul(refOffsetQ,targetRefX.q)); + + if(constant.m_LoopBlendOrientation) + { + refStopX.q = refStartX.q; + prevRefX.q = refStartX.q; + refX.q = refStartX.q; + targetRefX.q = refStartX.q; + } + + if(constant.m_LoopBlendPositionY) + { + refStopX.t.y() = refStartX.t.y(); + prevRefX.t.y() = refStartX.t.y(); + refX.t.y() = refStartX.t.y(); + targetRefX.t.y() = refStartX.t.y(); + } + + if(constant.m_LoopBlendPositionXZ) + { + refStartX.t.x() = refStartX.t.x(); + refStopX.t.x() = refStartX.t.x();; + prevRefX.t.x() = refStartX.t.x();; + refX.t.x() = refStartX.t.x();; + targetRefX.t.x() = refStartX.t.x();; + + refStartX.t.z() = refStartX.t.z(); + refStopX.t.z() = refStartX.t.z();; + prevRefX.t.z() = refStartX.t.z();; + refX.t.z() = refStartX.t.z();; + targetRefX.t.z() = refStartX.t.z();; + } + + prevRootX = math::xformInvMulNS(prevRefX,prevRootX); + rootX = math::xformInvMulNS(refX,rootX); + targetRootX = math::xformInvMulNS(targetRefX,targetRootX); + + if(constant.m_LoopTime && constant.m_LoopBlend) + { + human::HumanPose deltaPose; + human::HumanPoseWeight(deltaPose,constant.m_DeltaPose,timeFrac); + human::HumanPoseAdd(humanPose,humanPose,deltaPose); + + prevLeftFootX = math::xformMul(prevLeftFootX,math::xformWeight(constant.m_DeltaPose.m_GoalArray[human::kLeftFootGoal].m_X,math::float1(prevTimeFrac))); + prevRightFootX = math::xformMul(prevRightFootX,math::xformWeight(constant.m_DeltaPose.m_GoalArray[human::kRightFootGoal].m_X,math::float1(prevTimeFrac))); + + math::xform rootDeltaX = math::xformInvMulNS(math::xformInvMulNS(refStopX,stopX),math::xformInvMulNS(refStartX,constant.m_StartX)); + math::xform prevRootDeltaX = math::xformWeight(rootDeltaX,math::float1(prevTimeFrac)); + math::xform targetRootDeltaX = math::xformWeight(rootDeltaX,math::float1(targetTimeFrac)); + rootDeltaX = math::xformWeight(rootDeltaX,math::float1(timeFrac)); + prevRootX = math::xformMul(prevRootX,prevRootDeltaX); + rootX = math::xformMul(rootX,rootDeltaX); + targetRootX = math::xformMul(targetRootX,targetRootDeltaX); + + math::xform cycleDelta; + for(int i = 0 ; i < targetTimeInt - currentTimeInt; i++) + { + if(i == 0) cycleDelta = math::xformInvMulNS(refStartX,refStopX); + targetRefX = math::xformMul(targetRefX,cycleDelta); + } + if(targetGoalIndex >= 0) targetGoalX = math::xformMul(targetGoalX,math::xformWeight(constant.m_DeltaPose.m_GoalArray[targetGoalIndex].m_X,math::float1(targetTimeFrac))); + } + + humanPose.m_RootX = rootX; + + for(int i = 0; i < human::kLastGoal; i++) + { + humanPose.m_GoalArray[i].m_X = math::xformMul(rootX,humanPose.m_GoalArray[i].m_X); + } + + motionOutput.m_PrevLeftFootX = math::xformMul(prevRootX,prevLeftFootX); + motionOutput.m_PrevRightFootX = math::xformMul(prevRootX,prevRightFootX); + + motionOutput.m_DX = math::xformIdentity(); + motionOutput.m_MotionX = math::xformIdentity(); + + if(constant.m_LoopTime) + { + float deltaTime = math::abs(timeFrac - prevTimeFrac); + + if(deltaTime > 0.5f) + { + if(timeFrac < prevTimeFrac) + { + motionOutput.m_DX = math::xformInvMulNS(prevRefX,math::xformMul(refStopX,math::xformInvMulNS(refStartX,refX))); + } + else if(timeFrac > prevTimeFrac) + { + motionOutput.m_DX = math::xformInvMulNS(prevRefX,math::xformMul(refStartX,math::xformInvMulNS(refStopX,refX))); + } + } + else if(timeFrac != prevTimeFrac) + { + motionOutput.m_DX = math::xformInvMulNS(prevRefX,refX); + } + } + else + { + if(input.m_PreviousTime != input.m_Time) + { + motionOutput.m_DX = math::xformInvMulNS(prevRefX,refX); + } + } + + motionOutput.m_DX.q = math::quat2Qtan(motionOutput.m_DX.q); + + motionOutput.m_DX.q.x() = 0; + motionOutput.m_DX.q.z() = 0; + + if(constant.m_LoopBlendPositionY) + { + motionOutput.m_DX.t.y() = 0; + } + + if(constant.m_LoopBlendPositionXZ) + { + motionOutput.m_DX.t.x() = 0; + motionOutput.m_DX.t.z() = 0; + } + + if(constant.m_LoopBlendOrientation) + { + motionOutput.m_DX.q.y() = 0; + } + + motionOutput.m_DX.q = math::qtan2Quat(motionOutput.m_DX.q); + motionOutput.m_DX.s = math::float4::one(); + motionOutput.m_TargetX = math::xformInvMulNS(refX,targetRefX); + + if(mirror) + { + human::HumanPoseMirror(humanPose,humanPose); + + motionOutput.m_DX = math::mirror(motionOutput.m_DX); + + math::xform x = math::mirror(motionOutput.m_PrevLeftFootX); + motionOutput.m_PrevLeftFootX = math::mirror(motionOutput.m_PrevRightFootX); + motionOutput.m_PrevRightFootX = x; + + constant_float4(offsetQY,0,1,0,0); + constant_float4(offsetQZ,0,0,1,0); + + motionOutput.m_PrevLeftFootX.q = math::normalize(math::quatMul(motionOutput.m_PrevLeftFootX.q,offsetQY)); + motionOutput.m_PrevRightFootX.q = math::normalize(math::quatMul(motionOutput.m_PrevRightFootX.q,offsetQY)); + + motionOutput.m_TargetX = math::mirror(motionOutput.m_TargetX); + + if(input.m_TargetIndex > kTargetReference) + { + targetRootX = math::mirror(targetRootX); + + if(input.m_TargetIndex > kTargetRoot) + { + targetGoalX = math::mirror(targetGoalX); + targetGoalX.q = math::normalize(math::quatMul(targetGoalX.q,math::cond(math::bool4(input.m_TargetIndex > kTargetRightFoot),offsetQZ,offsetQY))); + } + } + } + + if(input.m_TargetIndex > kTargetReference) + { + motionOutput.m_TargetX = math::xformMul(motionOutput.m_TargetX,targetRootX); + + if(input.m_TargetIndex > kTargetRoot) + { + motionOutput.m_TargetX = math::xformMul(motionOutput.m_TargetX,targetGoalX); + } + } + + motionOutput.m_MotionX = refX; + } + + void ComputeClipMuscleDeltaPose(ClipMuscleConstant const& constant, float startTime, float stopTime, human::HumanPose &deltaPose, math::xform &startX, math::xform &leftFootStartX, math::xform &rightFootStartX, memory::Allocator& alloc) + { + ClipInput in; + human::HumanPose poseStart; + human::HumanPose poseStop; + + ClipOutput *clipOutStart = CreateClipOutput(constant.m_Clip.Get(),alloc); + ClipOutput *clipOutStop = CreateClipOutput(constant.m_Clip.Get(),alloc); + ClipMemory *mem = CreateClipMemory(constant.m_Clip.Get(), alloc); + + in.m_Time = startTime; + EvaluateClip(constant.m_Clip.Get(),&in,mem,clipOutStart); + GetHumanPose(constant,clipOutStart->m_Values,poseStart); + + in.m_Time = stopTime; + EvaluateClip(constant.m_Clip.Get(),&in,mem,clipOutStop); + GetHumanPose(constant,clipOutStop->m_Values,poseStop); + + human::HumanPoseSub(deltaPose,poseStart,poseStop); + + startX = poseStart.m_RootX; + leftFootStartX = poseStart.m_GoalArray[human::kLeftFootGoal].m_X; + rightFootStartX = poseStart.m_GoalArray[human::kRightFootGoal].m_X; + + DestroyClipOutput(clipOutStart,alloc); + DestroyClipOutput(clipOutStop,alloc); + DestroyClipMemory(mem,alloc); + } + + void InitClipMuscleDeltaValues(ClipMuscleConstant& constant) + { + mecanim::memory::MecanimAllocator alloc(kMemTempAlloc); + + ClipInput in; + ClipOutput *outStart = CreateClipOutput(constant.m_Clip.Get(),alloc); + ClipOutput *outStop = CreateClipOutput(constant.m_Clip.Get(),alloc); + ClipMemory *mem = CreateClipMemory(constant.m_Clip.Get(), alloc); + + in.m_Time = constant.m_StartTime; + EvaluateClip(constant.m_Clip.Get(),&in,mem,outStart); + + in.m_Time = constant.m_StopTime; + EvaluateClip(constant.m_Clip.Get(),&in,mem,outStop); + + constant.m_MotionStartX = GetMotionX(constant,*outStart); + constant.m_MotionStopX = GetMotionX(constant,*outStop); + + for(int valueIter = 0; valueIter < constant.m_ValueArrayCount; valueIter++) + { + constant.m_ValueArrayDelta[valueIter].m_Start = outStart->m_Values[valueIter]; + constant.m_ValueArrayDelta[valueIter].m_Stop = outStop->m_Values[valueIter]; + } + + DestroyClipOutput(outStart,alloc); + DestroyClipOutput(outStop,alloc); + DestroyClipMemory(mem,alloc); + } + + void InitClipMuscleDeltaPose(ClipMuscleConstant& constant) + { + mecanim::memory::MecanimAllocator alloc(kMemTempAlloc); + ComputeClipMuscleDeltaPose(constant,constant.m_StartTime,constant.m_StopTime,constant.m_DeltaPose,constant.m_StartX,constant.m_LeftFootStartX,constant.m_RightFootStartX,alloc); + } + + void InitClipMuscleAverageSpeed(ClipMuscleConstant& constant, int steps) + { + mecanim::memory::MecanimAllocator alloc(kMemTempAlloc); + + ClipMemory *mem = CreateClipMemory(constant.m_Clip.Get(), alloc); + + float period = (constant.m_StopTime - constant.m_StartTime) / float(steps); + float time = constant.m_StartTime; + + math::xform prevRootX; + + math::float4 speed = math::float4::zero(); + float angularSpeed = 0; + + for(int i = 0; i <= steps; i++) + { + math::xform rootX = EvaluateRoot(constant,*mem,time); + + math::float4 qYOffset = math::qtan2Quat(math::float4(0,math::halfTan(math::radians(constant.m_OrientationOffsetY)),0,1)); + + if(constant.m_KeepOriginalOrientation) + { + qYOffset = math::normalize(math::quatMul(qYOffset,math::quatConj(constant.m_StartX.q))); + } + + rootX.q = math::normalize(math::quatMul(qYOffset,math::quatProjOnYPlane(math::cond(math::bool4(constant.m_LoopBlendOrientation),constant.m_StartX.q,rootX.q)))); + + if(i > 0) + { + math::xform dx = math::xformInvMul(prevRootX,rootX); + if(constant.m_Mirror) dx = math::mirror(dx); + math::float4 dxdof = math::doubleAtan(math::quat2Qtan(dx.q)); + angularSpeed += dxdof.y().tofloat() / period; + speed = speed + dx.t / math::float1(period); + } + + prevRootX = rootX; + time += period; + } + + constant.m_AverageAngularSpeed = angularSpeed / float(steps); + + constant.m_AverageSpeed = speed / math::float1(steps); + constant.m_AverageSpeed = math::cond(math::bool4(constant.m_LoopBlendPositionXZ,constant.m_LoopBlendPositionY,constant.m_LoopBlendPositionXZ,true),math::float4::zero(),constant.m_AverageSpeed); + + DestroyClipMemory(mem,alloc); + } + + size_t GetClipCurveCount(const ClipMuscleConstant& constant) + { + return GetClipCurveCount(*constant.m_Clip); + } +} + +} diff --git a/Runtime/mecanim/animation/clipmuscle.h b/Runtime/mecanim/animation/clipmuscle.h new file mode 100644 index 0000000..e2bb685 --- /dev/null +++ b/Runtime/mecanim/animation/clipmuscle.h @@ -0,0 +1,264 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/Math/Simd/float4.h" +#include "Runtime/mecanim/animation/curvedata.h" +#include "Runtime/mecanim/human/human.h" +#include "Runtime/mecanim/human/hand.h" + +#include "Runtime/Serialize/Blobification/offsetptr.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Animation/MecanimArraySerialization.h" + +namespace mecanim +{ + +namespace animation +{ + const int32_t s_ClipMuscleCurveTQCount = 7; + const int32_t s_ClipMuscleDoFBegin = (2 + human::kLastGoal) * s_ClipMuscleCurveTQCount; + const int32_t s_ClipMotionCurveCount = 7; + const int32_t s_ClipMuscleCurveCount = s_ClipMotionCurveCount + (1 + human::kLastGoal) * 7 + human::kLastDoF + 2 * hand::s_DoFCount; + + int32_t FindMuscleIndex(uint32_t id); + mecanim::String const& GetMuscleCurveName(int32_t curveIndex); + float GetXformCurveValue(math::xform const& x, int32_t index); + float GetMuscleCurveValue(human::HumanPose const& pose, math::xform const &motionX, int32_t curveIndex); + bool GetMuscleCurveInMask(human::HumanPoseMask const& mask, int32_t curveIndex); + int32_t GetMuscleCurveTQIndex(int32_t curveIndex); + + void InitializeMuscleClipTables (); + + enum { kTargetReference, kTargetRoot, kTargetLeftFoot, kTargetRightFoot, kTargetLeftHand, kTargetRightHand }; + + struct ValueDelta + { + DEFINE_GET_TYPESTRING(ValueDelta) + + float m_Start; + float m_Stop; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_Start); + TRANSFER(m_Stop); + } + }; + + // Constant + // @TODO: create a smaller constant to use with non humanoid clips. There is too much overhead here + struct ClipMuscleConstant + { + DEFINE_GET_TYPESTRING(ClipMuscleConstant) + + ClipMuscleConstant() : + m_StartX(math::xformIdentity()), + m_LeftFootStartX(math::xformIdentity()), + m_RightFootStartX(math::xformIdentity()), + m_MotionStartX(math::xformIdentity()), + m_MotionStopX(math::xformIdentity()), + m_ValueArrayCount(0), + m_AverageSpeed(math::float4::zero()), + m_Mirror(false), + m_StartTime(0), + m_StopTime(1), + m_LoopTime(false), + m_LoopBlend(false), + m_LoopBlendOrientation(false), + m_LoopBlendPositionXZ(false), + m_LoopBlendPositionY(false), + m_KeepOriginalOrientation(false), + m_KeepOriginalPositionY(true), + m_KeepOriginalPositionXZ(false), + m_HeightFromFeet(false), + m_OrientationOffsetY(0), + m_Level(0), + m_CycleOffset(0), + m_AverageAngularSpeed(0) + { + int32_t i; + for(i = 0; i < s_ClipMuscleCurveCount; i++) + { + m_IndexArray[i] = -1; + } + } + + human::HumanPose m_DeltaPose; + + math::xform m_StartX; + math::xform m_LeftFootStartX; + math::xform m_RightFootStartX; + + math::xform m_MotionStartX; + math::xform m_MotionStopX; + + math::float4 m_AverageSpeed; + + OffsetPtr<Clip> m_Clip; + + float m_StartTime; + float m_StopTime; + float m_OrientationOffsetY; + float m_Level; + float m_CycleOffset; + float m_AverageAngularSpeed; + + int32_t m_IndexArray[s_ClipMuscleCurveCount]; + + uint32_t m_ValueArrayCount; + OffsetPtr<ValueDelta> m_ValueArrayDelta; + + bool m_Mirror; + bool m_LoopTime; + bool m_LoopBlend; + bool m_LoopBlendOrientation; + bool m_LoopBlendPositionY; + bool m_LoopBlendPositionXZ; + + bool m_KeepOriginalOrientation; + bool m_KeepOriginalPositionY; + bool m_KeepOriginalPositionXZ; + bool m_HeightFromFeet; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + transfer.SetVersion(2); + + TRANSFER(m_DeltaPose); + TRANSFER(m_StartX); + TRANSFER(m_LeftFootStartX); + TRANSFER(m_RightFootStartX); + TRANSFER(m_MotionStartX); + TRANSFER(m_MotionStopX); + TRANSFER(m_AverageSpeed); + TRANSFER(m_Clip); + TRANSFER(m_StartTime); + TRANSFER(m_StopTime); + TRANSFER(m_OrientationOffsetY); + TRANSFER(m_Level); + TRANSFER(m_CycleOffset); + + TRANSFER(m_AverageAngularSpeed); + + STATIC_ARRAY_TRANSFER(mecanim::int32_t, m_IndexArray, s_ClipMuscleCurveCount); + + TRANSFER_BLOB_ONLY(m_ValueArrayCount); + MANUAL_ARRAY_TRANSFER2(ValueDelta, m_ValueArrayDelta, m_ValueArrayCount); + + TRANSFER(m_Mirror); + + TRANSFER(m_LoopTime); + TRANSFER(m_LoopBlend); + + m_LoopTime = transfer.IsVersionSmallerOrEqual(1) ? m_LoopBlend : m_LoopTime; + + TRANSFER(m_LoopBlendOrientation); + TRANSFER(m_LoopBlendPositionY); + TRANSFER(m_LoopBlendPositionXZ); + + TRANSFER(m_KeepOriginalOrientation); + TRANSFER(m_KeepOriginalPositionY); + TRANSFER(m_KeepOriginalPositionXZ); + TRANSFER(m_HeightFromFeet); + + transfer.Align(); + } + }; + + // Input + struct ClipMuscleInput + { + ClipMuscleInput() : m_Time(0), + m_PreviousTime(0), + m_TargetIndex(kTargetReference), + m_TargetTime(1), m_Reverse(false), m_Mirror(false), m_CycleOffset(0){} + + float m_Time; + float m_PreviousTime; + + int32_t m_TargetIndex; + float m_TargetTime; + bool m_Reverse; + bool m_Mirror; + float m_CycleOffset; + }; + + // Output + struct MotionOutput + { + DEFINE_GET_TYPESTRING(MotionOutput) + + MotionOutput() : m_DX(math::xformIdentity()), + m_MotionX(math::xformIdentity()), + m_MotionStartX(math::xformIdentity()), + m_MotionStopX(math::xformIdentity()), + m_PrevRootX(math::xformIdentity()), + m_PrevLeftFootX(math::xformIdentity()), + m_PrevRightFootX(math::xformIdentity()), + m_TargetX(math::xformIdentity()), + m_GravityWeight(0) {} + + math::xform m_DX; + math::xform m_MotionX; + math::xform m_MotionStartX; + math::xform m_MotionStopX; + + math::xform m_PrevRootX; + math::xform m_PrevLeftFootX; + math::xform m_PrevRightFootX; + + math::xform m_TargetX; + + float m_GravityWeight; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_DX); + TRANSFER(m_MotionX); + TRANSFER(m_MotionStartX); + TRANSFER(m_MotionStopX); + TRANSFER(m_PrevRootX); + TRANSFER(m_PrevLeftFootX); + TRANSFER(m_PrevRightFootX); + TRANSFER(m_TargetX); + TRANSFER(m_GravityWeight); + }; + }; + + void MotionOutputClear(MotionOutput *output); + void MotionOutputCopy(MotionOutput *output, MotionOutput const *motion, bool hasRootMotion, bool isHuman, human::HumanPoseMask const &poseMask); + void MotionOutputBlend(MotionOutput *output, MotionOutput **outputArray, float *weigh, uint32_t count, bool hasRootMotion, bool isHuman, human::HumanPoseMask const &poseMask); + void MotionAddAdditiveLayer(MotionOutput *output, MotionOutput const *motion, float weight, bool hasRootMotion, bool isHuman, human::HumanPoseMask const &poseMask); + void MotionAddOverrideLayer(MotionOutput *output, MotionOutput const *motion, float weight, bool hasRootMotion, bool isHuman, human::HumanPoseMask const &poseMask); + + ClipMuscleConstant* CreateClipMuscleConstant(Clip * clip, memory::Allocator& alloc); + void DestroyClipMuscleConstant(ClipMuscleConstant * constant, memory::Allocator& alloc); + + ClipMuscleInput* CreateClipMuscleInput(ClipMuscleConstant const* constant, memory::Allocator& alloc); + void DestroyClipMuscleInput(ClipMuscleInput * input, memory::Allocator& alloc); + + float ComputeClipTime(float normalizedTimeIn, float startTime, float stopTime, float cycleOffset, bool loop, bool reverse, float &nomralizedTimeOut, float &timeInt); + + void EvaluateClipRootMotionDeltaX(const ClipMuscleConstant& constant, const ClipMuscleInput &input, MotionOutput &output, ClipMemory &memory); + + void EvaluateClipMusclePrevTime(const ClipMuscleConstant& constant, const ClipMuscleInput &input, MotionOutput &output, ClipMemory &memory); + void EvaluateClipMuscle(const ClipMuscleConstant& constant, const ClipMuscleInput &input, const float *valuesOutput, MotionOutput &motionOutput, human::HumanPose &humanPose, ClipMemory &memory); + + void ComputeClipMuscleDeltaPose(ClipMuscleConstant const &constant, float startTime, float stopTime, human::HumanPose &deltaPose, math::xform &startX, math::xform &leftFootStartX, math::xform &rightFootStartX, memory::Allocator& alloc); + + void GetHumanPose(const ClipMuscleConstant& constant, const float* values, human::HumanPose &humanPose); + void GetHumanPose(const ClipMuscleConstant& constant, const ValueDelta* values, human::HumanPose &humanPose); + + void InitClipMuscleDeltaValues(ClipMuscleConstant& constant); + void InitClipMuscleDeltaPose(ClipMuscleConstant& constant); + void InitClipMuscleAverageSpeed(ClipMuscleConstant& constant, int steps = 20); + + void InitMuscleClipIndexArray(ClipMuscleConstant& constant, memory::Allocator& alloc); + size_t GetClipCurveCount(const ClipMuscleConstant& constant); +} +} diff --git a/Runtime/mecanim/animation/constantclip.cpp b/Runtime/mecanim/animation/constantclip.cpp new file mode 100644 index 0000000..1fd07cf --- /dev/null +++ b/Runtime/mecanim/animation/constantclip.cpp @@ -0,0 +1,34 @@ +#include "UnityPrefix.h" +#include "constantclip.h" +#include "Runtime/mecanim/memory.h" + +namespace mecanim +{ + namespace animation + { + void SampleClip (const ConstantClip& curveData, uint32_t outputCount, float* output) + { + DebugAssert(outputCount <= curveData.curveCount); + + memcpy(output, curveData.data.Get(), outputCount * sizeof(float)); + } + + float SampleClipAtIndex (const ConstantClip& curveData, int index) + { + Assert(index < curveData.curveCount); + + return curveData.data[index]; + } + + void DestroyConstantClip (ConstantClip& clip, memory::Allocator& alloc) + { + alloc.Deallocate(clip.data); + } + + void CreateConstantClip (ConstantClip& clip, size_t curveCount, memory::Allocator& alloc) + { + clip.curveCount = curveCount; + clip.data = alloc.ConstructArray<float> (curveCount); + } + } +} diff --git a/Runtime/mecanim/animation/constantclip.h b/Runtime/mecanim/animation/constantclip.h new file mode 100644 index 0000000..a7de648 --- /dev/null +++ b/Runtime/mecanim/animation/constantclip.h @@ -0,0 +1,36 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/Serialize/Blobification/offsetptr.h" + + +namespace mecanim +{ +namespace animation +{ +struct ConstantClip +{ + UInt32 curveCount; + OffsetPtr<float> data; + + ConstantClip () : curveCount (0) { } + + DEFINE_GET_TYPESTRING(ConstantClip) + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(curveCount); + MANUAL_ARRAY_TRANSFER2(float, data, curveCount); + } +}; + +// Sample functions +void SampleClip (const ConstantClip& curveData, uint32_t outputCount, float* output); +float SampleClipAtIndex (const ConstantClip& curveData, int index); + +void DestroyConstantClip (ConstantClip& clip, memory::Allocator& alloc); +void CreateConstantClip (ConstantClip& clip, size_t curveCount, memory::Allocator& alloc); + +} +}
\ No newline at end of file diff --git a/Runtime/mecanim/animation/curvedata.cpp b/Runtime/mecanim/animation/curvedata.cpp new file mode 100644 index 0000000..210bbe8 --- /dev/null +++ b/Runtime/mecanim/animation/curvedata.cpp @@ -0,0 +1,118 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/animation/curvedata.h" + +namespace mecanim +{ + +namespace animation +{ + Clip* CreateClipSimple(uint32_t count, memory::Allocator& alloc) + { + SETPROFILERLABEL(Clip); + + // Allocate data. + Clip* clip = alloc.Construct<Clip>(); + return clip; + } + + + void DestroyClip(Clip* clip, memory::Allocator& alloc) + { + if(clip) + { + DestroyStreamedClip(clip->m_StreamedClip, alloc); + DestroyDenseClip(clip->m_DenseClip, alloc); + + alloc.Deallocate(clip); + } + } + + ClipMemory* CreateClipMemory(Clip const* clip, memory::Allocator& alloc) + { + return CreateClipMemory(clip, GetClipCurveCount(*clip), alloc); + } + + ClipMemory* CreateClipMemory(Clip const* clip, int32_t totalUsedCurves, memory::Allocator& alloc) + { + SETPROFILERLABEL(ClipMemory); + + ClipMemory* mem = alloc.Construct<ClipMemory>(); + mem->m_ConstantClipValueCount = totalUsedCurves - (GetClipCurveCount(*clip) - clip->m_ConstantClip.curveCount); + Assert(mem->m_ConstantClipValueCount >= 0 && mem->m_ConstantClipValueCount <= clip->m_ConstantClip.curveCount); + + CreateStreamedClipMemory(clip->m_StreamedClip, mem->m_StreamedClipCache, alloc); + + return mem; + } + + void DestroyClipMemory(ClipMemory* memory, memory::Allocator& alloc) + { + if(memory) + { + DestroyStreamedClipMemory(memory->m_StreamedClipCache, alloc); + alloc.Deallocate(memory); + } + } + + ClipOutput* CreateClipOutput(Clip const* clip, memory::Allocator& alloc) + { + return CreateClipOutput(GetClipCurveCount(*clip), alloc); + } + + ClipOutput* CreateClipOutput(int32_t totalUsedCurves, memory::Allocator& alloc) + { + SETPROFILERLABEL(ClipOutput); + + ClipOutput* out = alloc.Construct<ClipOutput>(); + out->m_Values = alloc.ConstructArray<float> (totalUsedCurves); + + return out; + } + + void DestroyClipOutput(ClipOutput* output, memory::Allocator& alloc) + { + if(output) + { + alloc.Deallocate(output->m_Values); + alloc.Deallocate(output); + } + } + + float EvaluateClipAtIndex(Clip const* clip, ClipInput const* input, ClipMemory* memory, uint32_t index) + { + if (index < clip->m_StreamedClip.curveCount) + return SampleClipAtIndex(clip->m_StreamedClip, memory->m_StreamedClipCache, index, input->m_Time); + index -= clip->m_StreamedClip.curveCount; + + if (index < clip->m_DenseClip.m_CurveCount) + return SampleClipAtIndex(clip->m_DenseClip, index, input->m_Time); + index -= clip->m_DenseClip.m_CurveCount; + + return SampleClipAtIndex(clip->m_ConstantClip, index); + } + + void EvaluateClip(Clip const* clip, ClipInput const* input, ClipMemory* memory, ClipOutput* output ) + { + float* outputData = output->m_Values; + + if (clip->m_StreamedClip.curveCount != 0) + { + SampleClip(clip->m_StreamedClip, memory->m_StreamedClipCache, input->m_Time, outputData); + outputData += clip->m_StreamedClip.curveCount; + } + + if (clip->m_DenseClip.m_CurveCount != 0) + { + SampleClip(clip->m_DenseClip, input->m_Time, outputData); + outputData += clip->m_DenseClip.m_CurveCount; + } + + if (memory->m_ConstantClipValueCount != 0) + { + // Constant clips are not sampling based on the total curve count + SampleClip(clip->m_ConstantClip, memory->m_ConstantClipValueCount, outputData); + } + } +} + +} diff --git a/Runtime/mecanim/animation/curvedata.h b/Runtime/mecanim/animation/curvedata.h new file mode 100644 index 0000000..0244684 --- /dev/null +++ b/Runtime/mecanim/animation/curvedata.h @@ -0,0 +1,82 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/generic/valuearray.h" + +#include "Runtime/Serialize/Blobification/offsetptr.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Animation/MecanimArraySerialization.h" +#include "streamedclip.h" +#include "denseclip.h" +#include "constantclip.h" + +namespace mecanim +{ + +namespace animation +{ + struct Clip + { + DEFINE_GET_TYPESTRING(Clip) + + Clip() {} + + StreamedClip m_StreamedClip; + DenseClip m_DenseClip; + ConstantClip m_ConstantClip; + + OffsetPtr<ValueArrayConstant> m_DeprecatedBinding; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER (m_StreamedClip); + TRANSFER (m_DenseClip); + TRANSFER (m_ConstantClip); + + transfer.Transfer(m_DeprecatedBinding, "m_Binding"); + } + }; + + struct ClipInput + { + float m_Time; + }; + + struct ClipMemory + { + StreamedClipMemory m_StreamedClipCache; + int m_ConstantClipValueCount; + }; + + struct ClipOutput + { + public: + float* m_Values; + + }; + + Clip* CreateClipSimple(uint32_t count, memory::Allocator& alloc); + void DestroyClip(Clip* clip, memory::Allocator& alloc); + + ClipMemory* CreateClipMemory(Clip const* clip, memory::Allocator& alloc); + ClipMemory* CreateClipMemory(Clip const* clip, int32_t totalUsedCurves, memory::Allocator& alloc); + void DestroyClipMemory(ClipMemory* memory, memory::Allocator& alloc); + + ClipOutput* CreateClipOutput(int32_t usedConstantCurves, memory::Allocator& alloc); + ClipOutput* CreateClipOutput(Clip const* clip, memory::Allocator& alloc); + + void DestroyClipOutput(ClipOutput* output, memory::Allocator& alloc); + + float EvaluateClipAtIndex(Clip const* clip, ClipInput const* input, ClipMemory* memory, uint32_t index); + void EvaluateClip(Clip const* clip, ClipInput const* input, ClipMemory* memory, ClipOutput* output); + + inline size_t GetClipCurveCount (const Clip& clip) { return clip.m_StreamedClip.curveCount + clip.m_DenseClip.m_CurveCount + clip.m_ConstantClip.curveCount; } + inline bool IsValidClip (const Clip& clip) { return GetClipCurveCount (clip) != 0; } + + +} // namespace animation + +} diff --git a/Runtime/mecanim/animation/damp.cpp b/Runtime/mecanim/animation/damp.cpp new file mode 100644 index 0000000..c769108 --- /dev/null +++ b/Runtime/mecanim/animation/damp.cpp @@ -0,0 +1,24 @@ +#include "UnityPrefix.h" +#include "Runtime/Math/Simd/math.h" +#include "Runtime/mecanim/animation/damp.h" + +namespace mecanim +{ + +namespace dynamics +{ + void ScalDamp::Evaluate(float value, float deltaTime) + { + m_Value = math::cond(m_DampTime > 0, m_Value + (value - m_Value) * math::abs(deltaTime) / (m_DampTime + math::abs(deltaTime)), value); + } + + void VectorDamp::Evaluate(math::float4 const& value, float deltaTime) + { + math::float1 dt(deltaTime); + math::float1 dampTime(m_DampTime); + + m_Value = math::cond( math::bool4(m_DampTime > 0), m_Value + (value - m_Value) * math::abs(dt) / (dampTime + math::abs(dt)), value); + } +} + +} diff --git a/Runtime/mecanim/animation/damp.h b/Runtime/mecanim/animation/damp.h new file mode 100644 index 0000000..319f6e6 --- /dev/null +++ b/Runtime/mecanim/animation/damp.h @@ -0,0 +1,42 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/Math/Simd/float4.h" + +namespace mecanim +{ + +namespace dynamics +{ + class ScalDamp + { + public: + + float m_DampTime; + float m_Value; + + ScalDamp() { Reset(); } + + void Reset() { m_DampTime = 0; m_Value = 0; } + void Evaluate(float value, float deltaTime); + }; + + + class VectorDamp + { + public: + + float m_DampTime; + math::float4 m_Value; + + VectorDamp() { Reset(); } + + void Reset() { m_DampTime = 0; m_Value = math::float4::zero(); } + void Evaluate(math::float4 const& value, float deltaTime); + }; + +} // namespace dynamics + +} diff --git a/Runtime/mecanim/animation/denseclip.cpp b/Runtime/mecanim/animation/denseclip.cpp new file mode 100644 index 0000000..3c19e47 --- /dev/null +++ b/Runtime/mecanim/animation/denseclip.cpp @@ -0,0 +1,63 @@ +#include "UnityPrefix.h" +#include "denseclip.h" +#include "Runtime/Math/Simd/math.h" +namespace mecanim +{ +namespace animation +{ + +static void BlendArray(const float* lhs, const float* rhs, size_t size, float t, float* output) +{ + for (int i=0;i<size;i++) + output[i] = math::lerp(lhs[i], rhs[i], t); +} + + +static void PrepareBlendValues (const DenseClip& clip, float time, float*& lhs, float*& rhs, float& u) +{ + time -= clip.m_BeginTime; + + float index; + u = math::modf(time * clip.m_SampleRate, index); + + int lhsIndex = (int)index; + int rhsIndex = lhsIndex + 1; + lhsIndex = math::maximum(0, lhsIndex); + lhsIndex = math::minimum(clip.m_FrameCount-1, lhsIndex); + + rhsIndex = math::maximum(0, rhsIndex); + rhsIndex = math::minimum(clip.m_FrameCount-1, rhsIndex); + + lhs = const_cast<float*> (&clip.m_SampleArray[lhsIndex * clip.m_CurveCount]); + rhs = const_cast<float*> (&clip.m_SampleArray[rhsIndex * clip.m_CurveCount]); +} + +void SampleClip (const DenseClip& clip, float time, float* output) +{ + float u; + float* lhsPtr; + float* rhsPtr; + PrepareBlendValues(clip, time, lhsPtr, rhsPtr, u); + + BlendArray(lhsPtr, rhsPtr, clip.m_CurveCount, u, output); +} + +float SampleClipAtIndex (const DenseClip& clip, int curveIndex, float time) +{ + float u; + float* lhsPtr; + float* rhsPtr; + PrepareBlendValues(clip, time, lhsPtr, rhsPtr, u); + + return math::lerp(lhsPtr[curveIndex], rhsPtr[curveIndex], u); +} + + +// Creation & Destruction +void DestroyDenseClip (DenseClip& clip, memory::Allocator& alloc) +{ + alloc.Deallocate(clip.m_SampleArray); +} + +} +} diff --git a/Runtime/mecanim/animation/denseclip.h b/Runtime/mecanim/animation/denseclip.h new file mode 100644 index 0000000..e3175f1 --- /dev/null +++ b/Runtime/mecanim/animation/denseclip.h @@ -0,0 +1,47 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/Serialize/Blobification/offsetptr.h" + + +namespace mecanim +{ + namespace animation + { + struct DenseClip + { + int m_FrameCount; + uint32_t m_CurveCount; + float m_SampleRate; + float m_BeginTime; + + uint32_t m_SampleArraySize; + OffsetPtr<float> m_SampleArray; + + + DenseClip () : m_FrameCount (0),m_CurveCount(0),m_SampleRate(0.0F),m_SampleArraySize(0),m_BeginTime(0.0F) { } + + DEFINE_GET_TYPESTRING(DenseClip) + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_FrameCount); + TRANSFER(m_CurveCount); + TRANSFER(m_SampleRate); + TRANSFER(m_BeginTime); + + TRANSFER_BLOB_ONLY(m_SampleArraySize); + MANUAL_ARRAY_TRANSFER2(float, m_SampleArray, m_SampleArraySize); + } + }; + + // Sample functions + void SampleClip (const DenseClip& curveData, float time, float* output); + float SampleClipAtIndex (const DenseClip& curveData, int index, float time); + + + // Creation & Destruction + void DestroyDenseClip (DenseClip& clip, memory::Allocator& alloc); + } +}
\ No newline at end of file diff --git a/Runtime/mecanim/animation/poseblender.cpp b/Runtime/mecanim/animation/poseblender.cpp new file mode 100644 index 0000000..ccbdf8f --- /dev/null +++ b/Runtime/mecanim/animation/poseblender.cpp @@ -0,0 +1,228 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/skeleton/skeleton.h" + +#include "Runtime/mecanim/graph/plugbinder.h" +#include "Runtime/mecanim/animation/common.h" +#include "Runtime/mecanim/animation/poseblender.h" + +namespace mecanim +{ + +namespace animation +{ + namespace + { + void SetPoseBlenderValue(graph::GraphPlug const* plug, uint32_t index, PoseBlenderInput const* input, graph::EvaluationGraphWorkspace* graphMemory) + { + float value; + plug->ReadData(&value, graphMemory->m_EvaluationInfo); + input->m_WeightArray[index] += value; + } + } + + PoseBlenderConstant* CreatePoseBlenderConstant(skeleton::Skeleton * skeleton, skeleton::SkeletonPose* defaultPose, skeleton::SkeletonPose** arrayPose, uint32_t count, graph::Graph* graph, memory::Allocator& alloc) + { + PoseBlenderConstant *cst = alloc.Construct<PoseBlenderConstant>(); + + cst->m_Skeleton = skeleton; + cst->m_SkeletonPoseCount = count; + + cst->m_DefaultSkeletonPose = defaultPose; + cst->m_SkeletonPoseArray = alloc.ConstructArray<skeleton::SkeletonPose*>(count); + cst->m_PosesID = alloc.ConstructArray<uint32_t>(count); + cst->m_HasGraph = graph != 0; + cst->m_Graph = graph; + + uint32_t i; + for(i=0;i<count;i++) + { + cst->m_SkeletonPoseArray[i] = arrayPose[i]; + } + + if(cst->m_HasGraph) + { + cst->m_ValuesConstant = CreateValueArrayConstant(kFloatType, cst->m_Graph->m_InEdgesCount, alloc); + for(i=0;i<cst->m_Graph->m_InEdgesCount;++i) + { + cst->m_ValuesConstant->m_ValueArray[i].m_ID = cst->m_Graph->m_InEdges[i].m_Binding; + } + } + + return cst; + } + + void DestroyPoseBlenderConstant(PoseBlenderConstant * constant, memory::Allocator& alloc) + { + if(constant) + { + alloc.Deallocate(constant->m_PosesID); + alloc.Deallocate(constant->m_SkeletonPoseArray); + alloc.Deallocate(constant); + } + } + + void InitializePoseBlenderConstant(PoseBlenderConstant * constant, graph::GraphFactory const& factory, memory::Allocator& alloc) + { + if(constant->m_Graph) + { + constant->m_EvaluationGraph = graph::CreateEvaluationGraph(constant->m_Graph, factory, alloc); + } + } + + void ClearPoseBlenderConstant(PoseBlenderConstant * constant, memory::Allocator& alloc) + { + if(constant->m_EvaluationGraph) + { + graph::DestroyEvaluationGraph(constant->m_EvaluationGraph, alloc); + constant->m_EvaluationGraph = 0; + } + } + + PoseBlenderInput* CreatePoseBlenderInput(PoseBlenderConstant const* constant, memory::Allocator& alloc) + { + PoseBlenderInput *in = alloc.Construct<PoseBlenderInput>(); + + if(constant->m_ValuesConstant) + { + in->m_Values = CreateValueArray(constant->m_ValuesConstant, alloc); + } + in->m_WeightPoseCount = constant->m_SkeletonPoseCount; + in->m_SkeletonPose = skeleton::CreateSkeletonPose(constant->m_Skeleton, alloc); + in->m_WeightArray = alloc.ConstructArray<float>(constant->m_SkeletonPoseCount); + + uint32_t i; + for(i=0;i<in->m_WeightPoseCount;i++) + { + in->m_WeightArray[i] = 0.f; + } + + return in; + } + + void DestroyPoseBlenderInput(PoseBlenderInput * input, memory::Allocator& alloc) + { + if(input) + { + skeleton::DestroySkeletonPose(input->m_SkeletonPose, alloc); + alloc.Deallocate(input->m_WeightArray); + alloc.Deallocate(input); + } + } + + PoseBlenderWorkspace* CreatePoseBlenderWorkspace(PoseBlenderConstant const* constant, memory::Allocator& alloc) + { + PoseBlenderWorkspace *ws = alloc.Construct<PoseBlenderWorkspace>(); + ws->m_SkeletonPose = skeleton::CreateSkeletonPose(constant->m_Skeleton, alloc); + + return ws; + } + void DestroyPoseBlenderWorkspace(PoseBlenderWorkspace * workspace, memory::Allocator& alloc) + { + if(workspace) + { + skeleton::DestroySkeletonPose(workspace->m_SkeletonPose, alloc); + alloc.Deallocate(workspace); + } + } + + PoseBlenderMemory* CreatePoseBlenderMemory(PoseBlenderConstant const* constant, memory::Allocator& alloc) + { + PoseBlenderMemory* mem = alloc.Construct<PoseBlenderMemory>(); + + if(constant->m_EvaluationGraph) + { + mem->m_GraphWS = graph::CreateEvaluationGraphWorkspace(constant->m_EvaluationGraph, alloc); + mem->m_InputPoseBlenderBindingCount = 0; + uint32_t i,j; + for(i=0;i<constant->m_EvaluationGraph->m_Output->mPlugCount;i++) + { + graph::GraphPlug const* plug = constant->m_EvaluationGraph->m_Output->mPlugArray[i]; + for(j=0;j<constant->m_SkeletonPoseCount;j++) + { + if( plug->m_ID == constant->m_PosesID[j]) + { + mem->m_InputPoseBlenderBindingCount++; + } + } + } + if(mem->m_InputPoseBlenderBindingCount) + { + mem->m_InputPoseBlenderBinding = alloc.ConstructArray<SetPoseBlenderInput2>(mem->m_InputPoseBlenderBindingCount); + uint32_t k = 0; + for(i=0;i<constant->m_EvaluationGraph->m_Output->mPlugCount;i++) + { + graph::GraphPlug const* plug = constant->m_EvaluationGraph->m_Output->mPlugArray[i]; + for(j=0;j<constant->m_SkeletonPoseCount;j++) + { + if( plug->m_ID == constant->m_PosesID[j]) + { + mem->m_InputPoseBlenderBinding[k++] = bind( SetPoseBlenderValue, plug, j); + } + } + } + } + } + + return mem; + } + + void DestroyPoseBlenderMemory(PoseBlenderMemory* memory, memory::Allocator& alloc) + { + if(memory) + { + graph::DestroyEvaluationGraphWorkspace(memory->m_GraphWS, alloc); + alloc.Deallocate(memory->m_InputPoseBlenderBinding); + alloc.Deallocate(memory); + } + } + + + PoseBlenderOutput* CreatePoseBlenderOutput(PoseBlenderConstant const* constant, memory::Allocator& alloc) + { + PoseBlenderOutput *out = alloc.Construct<PoseBlenderOutput>(); + out->m_SkeletonPose = skeleton::CreateSkeletonPose(constant->m_Skeleton, alloc); + + return out; + } + + void DestroyPoseBlenderOutput(PoseBlenderOutput * output, memory::Allocator& alloc) + { + if(output) + { + skeleton::DestroySkeletonPose(output->m_SkeletonPose, alloc); + alloc.Deallocate(output); + } + } + + void EvaluatePoseBlender(PoseBlenderConstant const * constant, PoseBlenderInput const * input, PoseBlenderOutput * output, PoseBlenderMemory* memory, PoseBlenderWorkspace * workspace) + { + if(constant->m_EvaluationGraph) + { + memory->m_GraphWS->m_EvaluationInfo.m_EvaluationId++; + + // Value Array is constructed based on graph so they should be sync + uint32_t i, count = constant->m_ValuesConstant->m_Count; + for(i=0;i<count;i++) + { + SetPlugValue(&constant->m_EvaluationGraph->m_Input->GetPlug(i), i, input->m_Values, memory->m_GraphWS); + } + + count = memory->m_InputPoseBlenderBindingCount; + for(i=0;i<count;i++) + { + memory->m_InputPoseBlenderBinding[i](input, memory->m_GraphWS); + } + } + + skeleton::SkeletonPoseCopy(input->m_SkeletonPose, output->m_SkeletonPose); + uint32_t i; + for(i = 0; i < constant->m_SkeletonPoseCount; i++) + { + skeleton::SkeletonPoseSub(constant->m_SkeletonPoseArray[i], input->m_SkeletonPose, workspace->m_SkeletonPose); + skeleton::SkeletonPoseWeight(workspace->m_SkeletonPose, math::float1(input->m_WeightArray[i]), workspace->m_SkeletonPose); + skeleton::SkeletonPoseAdd(output->m_SkeletonPose, workspace->m_SkeletonPose, output->m_SkeletonPose); + } + } +} + +} diff --git a/Runtime/mecanim/animation/poseblender.h b/Runtime/mecanim/animation/poseblender.h new file mode 100644 index 0000000..4bd683e --- /dev/null +++ b/Runtime/mecanim/animation/poseblender.h @@ -0,0 +1,113 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/object.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/bind.h" + +#include "Runtime/mecanim/generic/valuearray.h" +#include "Runtime/mecanim/graph/plug.h" +#include "Runtime/mecanim/graph/graph.h" + +// forwards + +namespace mecanim +{ + +namespace skeleton {struct Skeleton; struct SkeletonPose;} + +namespace animation +{ + // Constant + struct PoseBlenderConstant + { + PoseBlenderConstant() + :m_Skeleton(0), + m_DefaultSkeletonPose(0), + m_SkeletonPoseArray(0), + m_PosesID(0), + m_SkeletonPoseCount(0), + m_Graph(0), + m_ValuesConstant(0), + m_HasGraph(false), + m_EvaluationGraph(0) + { + } + + skeleton::Skeleton* m_Skeleton; + skeleton::SkeletonPose* m_DefaultSkeletonPose; + skeleton::SkeletonPose** m_SkeletonPoseArray; + uint32_t* m_PosesID; + graph::Graph* m_Graph; + ValueArrayConstant const* m_ValuesConstant; + bool m_HasGraph; + + uint32_t m_SkeletonPoseCount; + + graph::EvaluationGraph* m_EvaluationGraph; + }; + + struct PoseBlenderInput + { + PoseBlenderInput(): m_Values(0), m_WeightArray(0), m_SkeletonPose(0), m_WeightPoseCount(0){} + + ValueArray* m_Values; + float* m_WeightArray; + skeleton::SkeletonPose* m_SkeletonPose; + uint32_t m_WeightPoseCount; + }; + + struct PoseBlenderWorkspace + { + PoseBlenderWorkspace(): m_SkeletonPose(0){} + skeleton::SkeletonPose* m_SkeletonPose; + }; + + typedef binder2<function<void (graph::GraphPlug const*, uint32_t, PoseBlenderInput const*, graph::EvaluationGraphWorkspace*)>::ptr, + graph::GraphPlug const*, + uint32_t> + SetPoseBlenderInput2; + + struct PoseBlenderMemory + { + PoseBlenderMemory() : m_InputPoseBlenderBindingCount(0), m_InputPoseBlenderBinding(0), m_GraphWS(0) + { + } + + uint32_t m_InputPoseBlenderBindingCount; + SetPoseBlenderInput2* m_InputPoseBlenderBinding; + + graph::EvaluationGraphWorkspace* m_GraphWS; + }; + + + struct PoseBlenderOutput : public Object + { + PoseBlenderOutput():m_SkeletonPose(0){} + + skeleton::SkeletonPose* m_SkeletonPose; + }; + + PoseBlenderConstant* CreatePoseBlenderConstant(skeleton::Skeleton * skeleton, skeleton::SkeletonPose* defaultPose, skeleton::SkeletonPose** arrayPose, uint32_t count, graph::Graph* graph, memory::Allocator& alloc); + void DestroyPoseBlenderConstant(PoseBlenderConstant * constant, memory::Allocator& alloc); + + void InitializePoseBlenderConstant(PoseBlenderConstant * constant, graph::GraphFactory const& factory, memory::Allocator& alloc); + void ClearPoseBlenderConstant(PoseBlenderConstant * constant, memory::Allocator& alloc); + + PoseBlenderInput* CreatePoseBlenderInput(PoseBlenderConstant const* constant, memory::Allocator& alloc); + void DestroyPoseBlenderInput(PoseBlenderInput * input, memory::Allocator& alloc); + + PoseBlenderWorkspace* CreatePoseBlenderWorkspace(PoseBlenderConstant const* constant, memory::Allocator& alloc); + void DestroyPoseBlenderWorkspace(PoseBlenderWorkspace * workspace, memory::Allocator& alloc); + + PoseBlenderMemory* CreatePoseBlenderMemory(PoseBlenderConstant const* constant, memory::Allocator& alloc); + void DestroyPoseBlenderMemory(PoseBlenderMemory* amemory, memory::Allocator& alloc); + + PoseBlenderOutput* CreatePoseBlenderOutput(PoseBlenderConstant const* constant, memory::Allocator& alloc); + void DestroyPoseBlenderOutput(PoseBlenderOutput * output, memory::Allocator& alloc); + + void EvaluatePoseBlender(PoseBlenderConstant const * constant, PoseBlenderInput const * input, PoseBlenderOutput * output, PoseBlenderMemory* memory, PoseBlenderWorkspace * workspace); +} + +} diff --git a/Runtime/mecanim/animation/streamedclip.cpp b/Runtime/mecanim/animation/streamedclip.cpp new file mode 100644 index 0000000..8b27785 --- /dev/null +++ b/Runtime/mecanim/animation/streamedclip.cpp @@ -0,0 +1,196 @@ +#include "UnityPrefix.h" +#include "streamedclip.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/Utilities/Prefetch.h" +#include "Runtime/Math/Simd/math.h" + +namespace mecanim +{ +namespace animation +{ + + +inline static float EvaluateCache (const StreamedCacheItem& cache, float sampleTime) +{ + float t = sampleTime - cache.time; + return (t * (t * (t * cache.coeff[0] + cache.coeff[1]) + cache.coeff[2])) + cache.coeff[3]; +} + +inline static void EvaluateMultipleCaches ( const StreamedCacheItem& cache0, + const StreamedCacheItem& cache1, + const StreamedCacheItem& cache2, + const StreamedCacheItem& cache3, + float sampleTime, float* output) +{ + const math::float4 time(sampleTime); + + const math::float4 cachetime(cache0.time, cache1.time, cache2.time, cache3.time); + const math::float4 dt = time - cachetime; + + const math::float4 coeffs0(cache0.coeff[0], cache1.coeff[0], cache2.coeff[0], cache3.coeff[0]); + const math::float4 coeffs1(cache0.coeff[1], cache1.coeff[1], cache2.coeff[1], cache3.coeff[1]); + const math::float4 coeffs2(cache0.coeff[2], cache1.coeff[2], cache2.coeff[2], cache3.coeff[2]); + const math::float4 coeffs3(cache0.coeff[3], cache1.coeff[3], cache2.coeff[3], cache3.coeff[3]); + + ATTRIBUTE_ALIGN(ALIGN4F) float v[4]; + math::store(dt * (dt * (dt * coeffs0 + coeffs1) + coeffs2) + coeffs3, v); + + output[0] = v[0]; + output[1] = v[1]; + output[2] = v[2]; + output[3] = v[3]; +} + +static void EvaluateCaches (const StreamedClipMemory& cache, float sampleTime, float* output) +{ + const StreamedCacheItem* caches = &cache.caches[0]; + Prefetch(caches); + Prefetch(caches+2); + Prefetch(caches+4); + Prefetch(caches+6); + Prefetch(caches+8); + + int i = 0; + for ( ; i+4 <= cache.cacheCount; i+=4, caches+=4 ) + { + Prefetch(caches+10); + Prefetch(caches+12); + const StreamedCacheItem& item0 = *(caches); + const StreamedCacheItem& item1 = *(caches+1); + const StreamedCacheItem& item2 = *(caches+2); + const StreamedCacheItem& item3 = *(caches+3); + EvaluateMultipleCaches(item0, item1, item2, item3, sampleTime, &output[i]); + } + + for ( ; i<cache.cacheCount; ++i, ++caches) + { + const StreamedCacheItem& item = *caches; + output[i] = EvaluateCache(item, sampleTime); + } + +} + +static void RewindCache (StreamedClipMemory& cache) +{ + cache.time = -std::numeric_limits<float>::infinity(); + cache.readByteOffset = 0; +} + +static inline void ConsumeCurveTimeData (const CurveTimeData* __restrict curveData, StreamedCacheItem* __restrict caches) +{ + float time = curveData->time; + const CurveKey* __restrict keys = reinterpret_cast<const CurveKey*> (curveData + 1); + int count = curveData->count; + + Prefetch(keys,0); + Prefetch(keys+3); + + int curveIndex = keys[0].curveIndex; + float coeff0 = keys[0].coeff[0]; + float coeff1 = keys[0].coeff[1]; + float coeff2 = keys[0].coeff[2]; + float coeff3 = keys[0].coeff[3]; + + for (int i=1;i<count;i++) + { + Prefetch(keys+3+i); + + StreamedCacheItem& activeCache = caches[curveIndex]; + activeCache.time = time; + activeCache.coeff[0] = coeff0; + activeCache.coeff[1] = coeff1; + activeCache.coeff[2] = coeff2; + activeCache.coeff[3] = coeff3; + curveIndex = keys[i].curveIndex; + coeff0 = keys[i].coeff[0]; + coeff1 = keys[i].coeff[1]; + coeff2 = keys[i].coeff[2]; + coeff3 = keys[i].coeff[3]; + } + StreamedCacheItem& activeCache = caches[curveIndex]; + activeCache.time = time; + activeCache.coeff[0] = coeff0; + activeCache.coeff[1] = coeff1; + activeCache.coeff[2] = coeff2; + activeCache.coeff[3] = coeff3; +} + +static void SeekClipForward (const UInt8* curveData, float time, StreamedClipMemory& cache) +{ + int readByteOffset = cache.readByteOffset; + const CurveTimeData* data = reinterpret_cast<const CurveTimeData*> (curveData + readByteOffset); + + while (time >= data->time) + { + // Consume the data and apply it to the cache + ConsumeCurveTimeData(data, cache.caches); + + // Seek forward by the consumed data + readByteOffset += sizeof(CurveTimeData) + data->count * sizeof(CurveKey); + + data = reinterpret_cast<const CurveTimeData*> (curveData + readByteOffset); + } + + // Synchronize cached time & offset + cache.time = time; + cache.readByteOffset = readByteOffset; +} + +void SeekClip (const StreamedClip& curveData, StreamedClipMemory& cache, float time) +{ + Assert(cache.cacheCount == curveData.curveCount); + + // No seeking is necessary, we are exactly at the same cached time + // (Happens due to SampleClipAtIndex) + // @TODO: it would be best to remove that and instead seperate root motion data from other data + if (time == cache.time) + return; + + // Seeking backwards is not supported. Jump the beginning of curve. + if (time < cache.time) + RewindCache(cache); + + // Seek and make sure the curve cache is up to date + const UInt8* stream = reinterpret_cast<const UInt8*> (curveData.data.Get()); + SeekClipForward(stream, time, cache); +} + +void SampleClip (const StreamedClip& curveData, StreamedClipMemory& cache, float time, float* output) +{ + SeekClip(curveData, cache, time); + + // Evaluate the cache and write sampled values to output + EvaluateCaches(cache, time, output); +} + +float SampleClipAtIndex (const StreamedClip& curveData, StreamedClipMemory& cache, int index, float time) +{ + Assert(index < curveData.curveCount); + + SeekClip(curveData, cache, time); + + return EvaluateCache(cache.caches[index], time); +} + +void CreateStreamedClipMemory(const StreamedClip& clip, StreamedClipMemory& mem, memory::Allocator& alloc) +{ + SETPROFILERLABEL(StreamedClipMemory); + + mem.caches = alloc.ConstructArray<StreamedCacheItem>(clip.curveCount); + mem.cacheCount = clip.curveCount; + + RewindCache(mem); +} + +void DestroyStreamedClipMemory (StreamedClipMemory& memory, memory::Allocator& alloc) +{ + alloc.Deallocate(memory.caches); +} + +void DestroyStreamedClip (StreamedClip& clip, memory::Allocator& alloc) +{ + alloc.Deallocate(clip.data); +} + +} +} diff --git a/Runtime/mecanim/animation/streamedclip.h b/Runtime/mecanim/animation/streamedclip.h new file mode 100644 index 0000000..93e1fc7 --- /dev/null +++ b/Runtime/mecanim/animation/streamedclip.h @@ -0,0 +1,93 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/Serialize/Blobification/offsetptr.h" + + +namespace mecanim +{ +namespace animation +{ + +// StreamedClip is a stream oriented clip format. +// It stores hermite coefficients directly. Multiple curves are stored in the same stream. Keys for all curves are sorted by time. +// This drastically reduces cache misses. In the best case we can sample a clip with a single cache miss. Clip data is read completely linearly. + +// Instead of laying out the animationclip by curves containing any array of keys. +// We sort all keys of all curves by time and each key has an index. +// This is a streamed format basically CurveTimeData defines the time value and the number of keys at this time. +// Then CurveTimeData.count CurveKey elements will follow. + +// time = 0 +// CurveTimeData(4) ... CurveKey . CurveKey . CurveKey . CurveKey + +// time = 0.2 +// CurveTimeData(1) ... CurveKey + +// Sampling is separated into two functions. +// 1. Seeking, it is responsible for updating the cached in the StreamedClipMemory to ensure each curve Index has an up to date time and hermite cofficients. +// 2. Evaluating the caches. This simply evaluates the hermite caches + +// See StreamedClipBuilder.cpp and AnimationClip.cpp on how to build the streamed clip data. + +struct StreamedCacheItem +{ + float time; + float coeff[4]; +}; + +struct StreamedClipMemory +{ + StreamedCacheItem* caches; + int cacheCount; + float time; + UInt32 readByteOffset; +}; + +struct StreamedClip +{ + uint32_t dataSize; + OffsetPtr<uint32_t> data; + UInt32 curveCount; + + + StreamedClip () : curveCount (0),dataSize(0) { } + + DEFINE_GET_TYPESTRING(StreamedClip) + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(dataSize); + MANUAL_ARRAY_TRANSFER2(uint32_t, data, dataSize); + TRANSFER(curveCount); + } +}; + +struct CurveTimeData +{ + float time; + UInt32 count; + // This is implicitly followed by CurveKey in the stream +}; + +struct CurveKey +{ + int curveIndex; + float coeff[4]; +}; + + +// Sample functions +void SampleClip (const StreamedClip& curveData, StreamedClipMemory& cache, float time, float* output); +float SampleClipAtIndex (const StreamedClip& curveData, StreamedClipMemory& cache, int index, float time); + + +// Creation & Destruction +// StreamedClipBuilder.h actually creates the streamed clip from an array of AnimationCurve. +void CreateStreamedClipMemory (const StreamedClip& clip, StreamedClipMemory& memory, memory::Allocator& alloc); +void DestroyStreamedClipMemory (StreamedClipMemory& memory, memory::Allocator& alloc); +void DestroyStreamedClip (StreamedClip& clip, memory::Allocator& alloc); + +} +}
\ No newline at end of file diff --git a/Runtime/mecanim/bind.h b/Runtime/mecanim/bind.h new file mode 100644 index 0000000..5e1fd07 --- /dev/null +++ b/Runtime/mecanim/bind.h @@ -0,0 +1,833 @@ +#pragma once + + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/types.h" + +// Value binders +// inspiration come from C++ template, The complete guide +// +// Bound function parameters to a specific value. +// We do support first and second parameter bounding. +// +// + +namespace mecanim +{ + template<typename rt, typename p1 = void, + typename p2 = void, + typename p3 = void, + typename p4 = void, + typename p5 = void, + typename p6 = void> + class function_ptr_type + { + public: + enum {numParams = 6}; + typedef rt (*function_type)(p1, p2, p3, p4, p5, p6); + }; + + // partial specialization for 5 parameter + template<typename rt, typename p1, + typename p2, + typename p3, + typename p4, + typename p5> + class function_ptr_type<rt, p1, p2, p3, p4, p5, void> + { + public: + enum {numParams = 5}; + typedef rt (*function_type)(p1, p2, p3, p4, p5); + }; + + // partial specialization for 4 parameter + template<typename rt, typename p1, + typename p2, + typename p3, + typename p4> + class function_ptr_type<rt, p1, p2, p3, p4, void, void> + { + public: + enum {numParams = 4}; + typedef rt (*function_type)(p1, p2, p3, p4); + }; + + // partial specialization for 3 parameter + template<typename rt, typename p1, + typename p2, + typename p3> + class function_ptr_type<rt, p1, p2, p3, void, void, void> + { + public: + enum {numParams = 3}; + typedef rt (*function_type)(p1, p2, p3); + }; + + // partial specialization for 2 parameter + template<typename rt, typename p1, + typename p2> + class function_ptr_type<rt, p1, p2, void, void, void, void> + { + public: + enum {numParams = 2}; + typedef rt (*function_type)(p1, p2); + }; + + // partial specialization for 1 parameter + template<typename rt, typename p1> + class function_ptr_type<rt, p1, void, void, void, void, void> + { + public: + enum {numParams = 1}; + typedef rt (*function_type)(p1); + }; + + // partial specialization for 0 parameter + template<typename rt> + class function_ptr_type<rt, void, void, void, void, void, void> + { + public: + enum {numParams = 0}; + typedef rt (*function_type)(); + }; + + template<typename T> + class forwardparam + { + public: + // class type are passed by copy, + // thus invoke the copy constructor + // In this case we should use a const reference + typedef T type; + }; + + template<> + class forwardparam<void> + { + private: + class empty{}; + public: + typedef empty type; + }; + + // The class function_ptr encapsulate a function pointer, + // passing function call argument can have a side effect + // if the corresponding parameter has a class type, it's copy constructor + // is invoked. To avoid this extra cost we need to change + // the class forwardparam, in this case a reference to the corresponding + // const class should be use. + template<typename rt, typename p1 = void, + typename p2 = void, + typename p3 = void, + typename p4 = void, + typename p5 = void, + typename p6 = void> + class function_ptr + { + private: + typedef typename function_ptr_type<rt, p1, p2, p3, p4, p5, p6>::function_type function_type; + function_type mFuntionPtr; + public: + enum { numParams = function_ptr_type<rt, p1, p2, p3, p4, p5, p6>::numParams }; + typedef rt return_type; + typedef p1 parameter_type1; + typedef p2 parameter_type2; + typedef p3 parameter_type3; + typedef p4 parameter_type4; + typedef p5 parameter_type5; + typedef p6 parameter_type6; + + function_ptr():mFuntionPtr(0) {} + function_ptr(function_type ptr):mFuntionPtr(ptr) {} + + return_type operator()() { return mFuntionPtr(); } + return_type operator()(typename forwardparam<parameter_type1>::type a1) + { + return mFuntionPtr(a1); + } + return_type operator()(typename forwardparam<parameter_type1>::type a1, + typename forwardparam<parameter_type2>::type a2) + { + return mFuntionPtr(a1, a2); + } + return_type operator()(typename forwardparam<parameter_type1>::type a1, + typename forwardparam<parameter_type2>::type a2, + typename forwardparam<parameter_type3>::type a3) + { + return mFuntionPtr(a1, a2, a3); + } + return_type operator()(typename forwardparam<parameter_type1>::type a1, + typename forwardparam<parameter_type2>::type a2, + typename forwardparam<parameter_type3>::type a3, + typename forwardparam<parameter_type4>::type a4) + { + return mFuntionPtr(a1, a2, a3, a4); + } + return_type operator()(typename forwardparam<parameter_type1>::type a1, + typename forwardparam<parameter_type2>::type a2, + typename forwardparam<parameter_type3>::type a3, + typename forwardparam<parameter_type4>::type a4, + typename forwardparam<parameter_type5>::type a5) + { + return mFuntionPtr(a1, a2, a3, a4, a5); + } + return_type operator()(typename forwardparam<parameter_type1>::type a1, + typename forwardparam<parameter_type2>::type a2, + typename forwardparam<parameter_type3>::type a3, + typename forwardparam<parameter_type4>::type a4, + typename forwardparam<parameter_type5>::type a5, + typename forwardparam<parameter_type6>::type a6) + { + return mFuntionPtr(a1, a2, a3, a4, a5, a6); + } + }; + + template<typename signature> class function; + + template<typename rt> + class function<rt (void)> + { + public: + enum {numParams = 0}; + typedef rt return_type; + typedef rt (*function_type)(); + typedef function_ptr<rt,void,void,void,void,void,void> ptr; + }; + + template<typename rt, typename p1> + class function<rt (p1)> + { + public: + enum {numParams = 1}; + typedef rt return_type; + typedef p1 parameter_type1; + typedef rt (*function_type) (p1); + typedef function_ptr<rt, p1,void,void,void,void,void> ptr; + }; + + template<typename rt, typename p1, typename p2> + class function<rt (p1, p2)> + { + public: + enum {numParams = 2}; + typedef rt return_type; + typedef p1 parameter_type1; + typedef p2 parameter_type2; + typedef rt (*function_type) (p1, p2); + typedef function_ptr<rt, p1, p2,void,void,void,void> ptr; + }; + + template<typename rt, typename p1, typename p2, typename p3> + class function<rt (p1, p2, p3)> + { + public: + enum {numParams = 3}; + typedef rt return_type; + typedef p1 parameter_type1; + typedef p2 parameter_type2; + typedef p3 parameter_type3; + typedef rt (*function_type) (p1, p2, p3); + typedef function_ptr<rt, p1, p2, p3,void,void,void> ptr; + }; + + template<typename rt, typename p1, typename p2, typename p3, typename p4> + class function<rt (p1, p2, p3, p4)> + { + public: + enum {numParams = 4}; + typedef rt return_type; + typedef p1 parameter_type1; + typedef p2 parameter_type2; + typedef p3 parameter_type3; + typedef p4 parameter_type4; + typedef rt (*function_type) (p1, p2, p3, p4); + typedef function_ptr<rt, p1, p2, p3, p4,void,void> ptr; + }; + + template<typename rt, typename p1, typename p2, typename p3, typename p4, typename p5> + class function<rt (p1, p2, p3, p4, p5)> + { + public: + enum {numParams = 5}; + typedef rt return_type; + typedef p1 parameter_type1; + typedef p2 parameter_type2; + typedef p3 parameter_type3; + typedef p4 parameter_type4; + typedef p5 parameter_type5; + typedef rt (*function_type) (p1, p2, p3, p4, p5); + typedef function_ptr<rt, p1, p2, p3, p4, p5,void> ptr; + }; + + template<typename rt, typename p1, typename p2, typename p3, typename p4, typename p5, typename p6> + class function<rt (p1, p2, p3, p4, p5, p6)> + { + public: + enum {numParams = 6}; + typedef rt return_type; + typedef p1 parameter_type1; + typedef p2 parameter_type2; + typedef p3 parameter_type3; + typedef p4 parameter_type4; + typedef p5 parameter_type5; + typedef p6 parameter_type6; + typedef rt (*function_type) (p1, p2, p3, p4, p5, p6); + typedef function_ptr<rt, p1, p2, p3, p4, p5, p6> ptr; + }; + + template <typename t> class bound_value_1 + { + public: + typedef t value_type; + bound_value_1(){} + bound_value_1(value_type v):value(v){} + value_type get(){return value;} + private: + value_type value; + }; + template <typename t> class bound_value_2 + { + public: + typedef t value_type; + bound_value_2(){} + bound_value_2(value_type v):value(v){} + value_type get(){return value;} + private: + value_type value; + }; + template <typename t> class bound_value_3 + { + public: + typedef t value_type; + bound_value_3(){} + bound_value_3(value_type v):value(v){} + value_type get(){return value;} + private: + value_type value; + }; + template <typename t> class bound_value_4 + { + public: + typedef t value_type; + bound_value_4(){} + bound_value_4(value_type v):value(v){} + value_type get(){return value;} + private: + value_type value; + }; + template <typename t> class bound_value_5 + { + public: + typedef t value_type; + bound_value_5(){} + bound_value_5(value_type v):value(v){} + value_type get(){return value;} + private: + value_type value; + }; + template <typename t> class bound_value_6 + { + public: + typedef t value_type; + bound_value_6(){} + bound_value_6(value_type v):value(v){} + value_type get(){return value;} + private: + value_type value; + }; + + + template<typename ft, typename p1> class binder1 : private ft, private bound_value_1<p1> + { + public: + enum { numParams = ft::numParams - 1 }; + + typedef typename ft::return_type return_type; + typedef typename forwardparam<typename ft::parameter_type2>::type parameter_type1; + typedef typename forwardparam<typename ft::parameter_type3>::type parameter_type2; + typedef typename forwardparam<typename ft::parameter_type4>::type parameter_type3; + typedef typename forwardparam<typename ft::parameter_type5>::type parameter_type4; + typedef typename forwardparam<typename ft::parameter_type6>::type parameter_type5; + + binder1(ft f, bound_value_1<p1> const& value1) + :ft(f), + bound_value_1<p1>(value1) + { + } + + binder1(ft f, p1 const& value1) + :ft(f), + bound_value_1<p1>(bound_value_1<p1>(value1)) + { + } + + return_type operator()() + { + return ft::operator()(bound_value_1<p1>::get()); + } + + return_type operator()(parameter_type1 a1) + { + return ft::operator()(bound_value_1<p1>::get(), a1); + } + + return_type operator()(parameter_type1 a1, parameter_type2 a2) + { + return ft::operator()(bound_value_1<p1>::get(), a1, a2); + } + + return_type operator()(parameter_type1 a1, parameter_type2 a2, parameter_type3 a3) + { + return ft::operator()(bound_value_1<p1>::get(), a1, a2, a3); + } + + return_type operator()(parameter_type1 a1, parameter_type2 a2, parameter_type3 a3, parameter_type4 a4) + { + return ft::operator()(bound_value_1<p1>::get(), a1, a2, a3, a4); + } + + return_type operator()(parameter_type1 a1, parameter_type2 a2, parameter_type3 a3, parameter_type4 a4, parameter_type5 a5) + { + return ft::operator()(bound_value_1<p1>::get(), a1, a2, a3, a4, a5); + } + }; + + + template<typename ft, typename p1, typename p2> class binder2 : private ft, private bound_value_1<p1>, private bound_value_2<p2> + { + public: + enum { numParams = ft::numParams - 2 }; + + typedef typename ft::return_type return_type; + typedef typename forwardparam<typename ft::parameter_type3>::type parameter_type1; + typedef typename forwardparam<typename ft::parameter_type4>::type parameter_type2; + typedef typename forwardparam<typename ft::parameter_type5>::type parameter_type3; + typedef typename forwardparam<typename ft::parameter_type6>::type parameter_type4; + + binder2() + { + } + + binder2(ft f, bound_value_1<p1> const& value1, bound_value_2<p2> const& value2) + :ft(f), + bound_value_1<p1>(value1), + bound_value_2<p2>(value2) + { + } + + binder2(ft f, p1 const& value1, p2 const& value2) + :ft(f), + bound_value_1<p1>(bound_value_1<p1>(value1)), + bound_value_2<p2>(bound_value_2<p2>(value2)) + { + } + + return_type operator()() + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get()); + } + + return_type operator()(parameter_type1 a1) + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), a1); + } + + return_type operator()(parameter_type1 a1, parameter_type2 a2) + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), a1, a2); + } + + return_type operator()(parameter_type1 a1, parameter_type2 a2, parameter_type3 a3) + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), a1, a2, a3); + } + + return_type operator()(parameter_type1 a1, parameter_type2 a2, parameter_type3 a3, parameter_type4 a4) + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), a1, a2, a3, a4); + } + }; + + template<typename ft, + typename p1, + typename p2, + typename p3> + class binder3 : private ft, + private bound_value_1<p1>, + private bound_value_2<p2>, + private bound_value_3<p3> + { + public: + enum { numParams = ft::numParams - 3 }; + + typedef typename forwardparam<typename ft::parameter_type4>::type parameter_type1; + typedef typename forwardparam<typename ft::parameter_type5>::type parameter_type2; + typedef typename forwardparam<typename ft::parameter_type6>::type parameter_type3; + + typedef typename ft::return_type return_type; + + binder3() + { + } + + binder3(ft f, bound_value_1<p1> const& value1, bound_value_2<p2> const& value2, + bound_value_3<p3> const& value3) + :ft(f), + bound_value_1<p1>(value1), + bound_value_2<p2>(value2), + bound_value_3<p3>(value3) + { + } + + binder3(ft f, p1 const& value1, p2 const& value2, p3 const& value3) + :ft(f), + bound_value_1<p1>(bound_value_1<p1>(value1)), + bound_value_2<p2>(bound_value_2<p2>(value2)), + bound_value_3<p3>(bound_value_3<p3>(value3)) + { + } + + return_type operator()() + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), + bound_value_3<p3>::get()); + } + + return_type operator()(parameter_type1 a1) + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), + bound_value_3<p3>::get(), a1); + } + + return_type operator()(parameter_type1 a1, parameter_type2 a2) + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), + bound_value_3<p3>::get(), a1, a2); + } + + return_type operator()(parameter_type1 a1, parameter_type2 a2, parameter_type3 a3) + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), + bound_value_3<p3>::get(), a1, a2, a3); + } + }; + + + template<typename ft, + typename p1, + typename p2, + typename p3, + typename p4> + class binder4 : private ft, + private bound_value_1<p1>, + private bound_value_2<p2>, + private bound_value_3<p3>, + private bound_value_4<p4> + { + public: + enum { numParams = ft::numParams - 4 }; + + typedef typename forwardparam<typename ft::parameter_type5>::type parameter_type1; + typedef typename forwardparam<typename ft::parameter_type6>::type parameter_type2; + + typedef typename ft::return_type return_type; + + binder4() + { + } + + binder4(ft f, bound_value_1<p1> const& value1, bound_value_2<p2> const& value2, + bound_value_3<p3> const& value3, bound_value_4<p4> const& value4) + :ft(f), + bound_value_1<p1>(value1), + bound_value_2<p2>(value2), + bound_value_3<p3>(value3), + bound_value_4<p4>(value4) + { + } + + binder4(ft f, p1 const& value1, p2 const& value2, p3 const& value3, p4 const& value4) + :ft(f), + bound_value_1<p1>(bound_value_1<p1>(value1)), + bound_value_2<p2>(bound_value_2<p2>(value2)), + bound_value_3<p3>(bound_value_3<p3>(value3)), + bound_value_4<p4>(bound_value_4<p4>(value4)) + { + } + + return_type operator()() + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), + bound_value_3<p3>::get(), bound_value_4<p4>::get()); + } + + return_type operator()(parameter_type1 a1) + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), + bound_value_3<p3>::get(), bound_value_4<p4>::get(), a1); + } + + return_type operator()(parameter_type1 a1, parameter_type2 a2) + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), + bound_value_3<p3>::get(), bound_value_4<p4>::get(), a1, a2); + } + }; + + + template<typename ft, + typename p1, + typename p2, + typename p3, + typename p4, + typename p5> + class binder5 : private ft, + private bound_value_1<p1>, + private bound_value_2<p2>, + private bound_value_3<p3>, + private bound_value_4<p4>, + private bound_value_5<p5> + { + public: + enum { numParams = ft::numParams - 5 }; + + typedef typename forwardparam<typename ft::parameter_type6>::type parameter_type1; + + typedef typename ft::return_type return_type; + + binder5() + { + } + + binder5(ft f, bound_value_1<p1> const& value1, bound_value_2<p2> const& value2, + bound_value_3<p3> const& value3, bound_value_4<p4> const& value4, + bound_value_5<p5> const& value5) + :ft(f), + bound_value_1<p1>(value1), + bound_value_2<p2>(value2), + bound_value_3<p3>(value3), + bound_value_4<p4>(value4), + bound_value_5<p5>(value5) + { + } + + binder5(ft f, p1 const& value1, p2 const& value2, p3 const& value3, p4 const& value4, p5 const& value5) + :ft(f), + bound_value_1<p1>(bound_value_1<p1>(value1)), + bound_value_2<p2>(bound_value_2<p2>(value2)), + bound_value_3<p3>(bound_value_3<p3>(value3)), + bound_value_4<p4>(bound_value_4<p4>(value4)), + bound_value_5<p5>(bound_value_5<p5>(value5)) + { + } + + return_type operator()() + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), + bound_value_3<p3>::get(), bound_value_4<p4>::get(), + bound_value_5<p5>::get()); + } + + return_type operator()(parameter_type1 a1) + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), + bound_value_3<p3>::get(), bound_value_4<p4>::get(), + bound_value_5<p5>::get(), a1); + } + }; + + template<typename ft, + typename p1, + typename p2, + typename p3, + typename p4, + typename p5, + typename p6> + class binder6 : private ft, + private bound_value_1<p1>, + private bound_value_2<p2>, + private bound_value_3<p3>, + private bound_value_4<p4>, + private bound_value_5<p5>, + private bound_value_6<p6> + { + public: + enum { numParams = ft::numParams - 6 }; + + typedef typename ft::return_type return_type; + + binder6() + { + } + + binder6(ft f, bound_value_1<p1> const& value1, bound_value_2<p2> const& value2, + bound_value_3<p3> const& value3, bound_value_4<p4> const& value4, + bound_value_5<p5> const& value5, bound_value_6<p6> const& value6) + :ft(f), + bound_value_1<p1>(value1), + bound_value_2<p2>(value2), + bound_value_3<p3>(value3), + bound_value_4<p4>(value4), + bound_value_5<p5>(value5), + bound_value_6<p6>(value6) + { + } + + binder6(ft f, p1 const& value1, p2 const& value2, p3 const& value3, p4 const& value4, p5 const& value5, p6 const& value6) + :ft(f), + bound_value_1<p1>(bound_value_1<p1>(value1)), + bound_value_2<p2>(bound_value_2<p2>(value2)), + bound_value_3<p3>(bound_value_3<p3>(value3)), + bound_value_4<p4>(bound_value_4<p4>(value4)), + bound_value_5<p5>(bound_value_5<p5>(value5)), + bound_value_6<p6>(bound_value_6<p6>(value6)) + { + } + + return_type operator()() + { + return ft::operator()(bound_value_1<p1>::get(), bound_value_2<p2>::get(), + bound_value_3<p3>::get(), bound_value_4<p4>::get(), + bound_value_5<p5>::get(), bound_value_6<p6>::get()); + } + }; + + template<typename rt, typename a1> + binder1<function_ptr<rt, a1>, a1 > bind(rt (*function)(a1), a1 value1) + { + typedef function_ptr<rt, a1> function_type; + return binder1<function_type, a1 >(function, value1); + } + template<typename rt, typename a1, typename a2> + binder1<function_ptr<rt, a1, a2>, a1 > bind(rt (*function)(a1, a2), a1 value1) + { + typedef function_ptr<rt, a1, a2> function_type; + return binder1<function_type, a1 >(function, value1); + } + template<typename rt, typename a1, typename a2, typename a3> + binder1<function_ptr<rt, a1, a2, a3>, a1 > bind(rt (*function)(a1, a2,a3), a1 value1) + { + typedef function_ptr<rt, a1, a2, a3> function_type; + return binder1<function_type, a1 >(function, value1); + } + template<typename rt, typename a1, typename a2, typename a3, typename a4> + binder1<function_ptr<rt, a1, a2, a3, a4>, a1 > bind(rt (*function)(a1, a2, a3, a4), a1 value1) + { + typedef function_ptr<rt, a1, a2, a3, a4> function_type; + return binder1<function_type, a1 >(function, value1); + } + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5> + binder1<function_ptr<rt, a1, a2, a3, a4, a5>, a1 > bind(rt (*function)(a1, a2, a3, a4, a5), a1 value1) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5> function_type; + return binder1<function_type, a1 >(function, value1); + } + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5, typename a6> + binder1<function_ptr<rt, a1, a2, a3, a4, a5, a6>, a1 > bind(rt (*function)(a1, a2, a3, a4, a5, a6), a1 value1) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5, a6> function_type; + return binder1<function_type, a1 >(function, value1); + } + + template<typename rt, typename a1, typename a2> + binder2<function_ptr<rt, a1, a2>, a1, a2 > bind(rt (*function)(a1, a2), a1 value1, a2 value2) + { + typedef function_ptr<rt, a1, a2> function_type; + return binder2<function_type, a1, a2 >(function, value1, value2); + } + template<typename rt, typename a1, typename a2, typename a3> + binder2<function_ptr<rt, a1, a2, a3>, a1, a2 > bind(rt (*function)(a1, a2, a3), a1 value1, a2 value2) + { + typedef function_ptr<rt, a1, a2, a3> function_type; + return binder2<function_type, a1, a2 >(function, value1, value2); + } + template<typename rt, typename a1, typename a2, typename a3, typename a4> + binder2<function_ptr<rt, a1, a2, a3, a4>, a1, a2 > bind(rt (*function)(a1, a2, a3, a4), a1 value1, a2 value2) + { + typedef function_ptr<rt, a1, a2, a3, a4> function_type; + return binder2<function_type, a1, a2 >(function, value1, value2); + } + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5> + binder2<function_ptr<rt, a1, a2, a3, a4, a5>, a1, a2 > bind(rt (*function)(a1, a2, a3, a4, a5), a1 value1, a2 value2) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5> function_type; + return binder2<function_type, a1, a2 >(function, value1, value2); + } + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5, typename a6> + binder2<function_ptr<rt, a1, a2, a3, a4, a5, a6>, a1, a2 > bind(rt (*function)(a1, a2, a3, a4, a5, a6), a1 value1, a2 value2) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5, a6> function_type; + return binder2<function_type, a1, a2 >(function, value1, value2); + } + + + template<typename rt, typename a1, typename a2, typename a3> + binder3<function_ptr<rt, a1, a2, a3>, a1, a2, a3 > bind(rt (*function)(a1, a2, a3), a1 value1, a2 value2, a3 value3) + { + typedef function_ptr<rt, a1, a2, a3> function_type; + return binder3<function_type, a1, a2, a3>(function, value1, value2, value3); + } + + template<typename rt, typename a1, typename a2, typename a3, typename a4> + binder3<function_ptr<rt, a1, a2, a3, a4>, a1, a2, a3 > bind(rt (*function)(a1, a2, a3, a4), a1 value1, a2 value2, a3 value3) + { + typedef function_ptr<rt, a1, a2, a3, a4> function_type; + return binder3<function_type, a1, a2, a3>(function, value1, value2, value3); + } + + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5> + binder3<function_ptr<rt, a1, a2, a3, a4, a5>, a1, a2, a3 > bind(rt (*function)(a1, a2, a3, a4, a5), a1 value1, a2 value2, a3 value3) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5> function_type; + return binder3<function_type, a1, a2, a3>(function, value1, value2, value3); + } + + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5, typename a6> + binder3<function_ptr<rt, a1, a2, a3, a4, a5, a6>, a1, a2, a3 > bind(rt (*function)(a1, a2, a3, a4, a5, a6), a1 value1, a2 value2, a3 value3) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5, a6> function_type; + return binder3<function_type, a1, a2, a3>(function, value1, value2, value3); + } + + template<typename rt, typename a1, typename a2, typename a3, typename a4> + binder4<function_ptr<rt, a1, a2, a3, a4>, a1, a2, a3, a4 > bind(rt (*function)(a1, a2, a3, a4), a1 value1, a2 value2, a3 value3, a4 value4) + { + typedef function_ptr<rt, a1, a2, a3, a4> function_type; + return binder4<function_type, a1, a2, a3, a4>(function, value1, value2, value3, value4); + } + + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5> + binder4<function_ptr<rt, a1, a2, a3, a4, a5>, a1, a2, a3, a4 > bind(rt (*function)(a1, a2, a3, a4, a5), a1 value1, a2 value2, a3 value3, a4 value4) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5> function_type; + return binder4<function_type, a1, a2, a3, a4>(function, value1, value2, value3, value4); + } + + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5, typename a6> + binder4<function_ptr<rt, a1, a2, a3, a4, a5, a6>, a1, a2, a3, a4 > bind(rt (*function)(a1, a2, a3, a4, a5, a6), a1 value1, a2 value2, a3 value3, a4 value4) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5, a6> function_type; + return binder4<function_type, a1, a2, a3, a4>(function, value1, value2, value3, value4); + } + + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5> + binder5<function_ptr<rt, a1, a2, a3, a4, a5>, a1, a2, a3, a4, a5 > bind(rt (*function)(a1, a2, a3, a4, a5), a1 value1, a2 value2, a3 value3, a4 value4, a5 value5) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5> function_type; + return binder5<function_type, a1, a2, a3, a4, a5>(function, value1, value2, value3, value4, value5); + } + + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5, typename a6> + binder5<function_ptr<rt, a1, a2, a3, a4, a5, a6>, a1, a2, a3, a4, a5 > bind(rt (*function)(a1, a2, a3, a4, a5, a6), a1 value1, a2 value2, a3 value3, a4 value4, a5 value5) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5, a6> function_type; + return binder5<function_type, a1, a2, a3, a4, a5>(function, value1, value2, value3, value4, value5); + } + + template<typename rt, typename a1, typename a2, typename a3, typename a4, typename a5, typename a6> + binder6<function_ptr<rt, a1, a2, a3, a4, a5, a6>, a1, a2, a3, a4, a5, a6 > bind(rt (*function)(a1, a2, a3, a4, a5, a6), a1 value1, a2 value2, a3 value3, a4 value4, a5 value5, a6 value6) + { + typedef function_ptr<rt, a1, a2, a3, a4, a5, a6> function_type; + return binder6<function_type, a1, a2, a3, a4, a5, a6 >(function, value1, value2, value3, value4, value5, value6); + } +} diff --git a/Runtime/mecanim/bitset.h b/Runtime/mecanim/bitset.h new file mode 100644 index 0000000..841e269 --- /dev/null +++ b/Runtime/mecanim/bitset.h @@ -0,0 +1,155 @@ +#pragma once + + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/types.h" + +namespace mecanim +{ + template <uint32_t Bits> class bitset + { + public: + typedef uint32_t type; + enum { + digits = Bits, + // parameters for packing bits into words + Bitsperword = (int)(8 * sizeof(type)), // bits in each word + Words = (int)(Bits == 0 ? 0 : (Bits - 1) / Bitsperword) + }; + + bitset(type value=0) + { + init(value); + } + + bitset<Bits>& set() + { + // set all bits true + init((type)~0); + return (*this); + } + + bitset<Bits>& set(type pos, bool val = true) + { + // set bit at _Pos to _Val + if (pos < Bits) + { + if (val) + mArray[pos / Bitsperword] |= (type)1 << pos % Bitsperword; + else + mArray[pos / Bitsperword] &= ~((type)1 << pos % Bitsperword); + } + return (*this); + } + + bitset<Bits>& reset() + { + // set all bits false + init(); + return (*this); + } + + bitset<Bits>& reset(type pos) + { + // set bit at pos to false + return set(pos, false); + } + + bitset<Bits>& flip() + { + // flip all bits + for (int pos = Words; 0 <= pos; --pos) + mArray[pos] = (type) ~mArray[pos]; + + trim(); + return *this; + } + + bitset<Bits>& flip(type pos) + { + // flip bit at pos + if (pos < Bits) + mArray[pos / Bitsperword] ^= (type) 1 << pos % Bitsperword; + return (*this); + } + + size_t count() const + { + // count number of set bits + static char Bitsperhex[] = "\0\1\1\2\1\2\2\3\1\2\2\3\2\3\3\4"; + type val = 0; + for (int pos = Words; 0 <= pos; --pos) + for (type Wordval = mArray[pos]; Wordval != 0; Wordval >>= 4) + val += Bitsperhex[Wordval & 0xF]; + return val; + } + + type size() const + { + // return size of bitset + return (Bits); + } + + bool test(uint32_t pos) const + { + // test if bit at pos is set + if (pos < Bits) + return ((mArray[pos / Bitsperword] & ((type)1 << pos % Bitsperword)) != 0); + return false; + } + + bool any() const + { + // test if any bits are set + for (int pos = Words; 0 <= pos; --pos) + if (mArray[pos] != 0) + return true; + return false; + } + + bool none() const + { + // test if no bits are set + return !any(); + } + + bool operator==(const bitset<Bits>& right) const + { + // test for bitset equality + for (int pos = Words; 0 <= pos; --pos) + if (mArray[pos] != right.word(pos)) + return false; + return true; + } + + type const& word(uint32_t pos)const + { + // get word at pos + return mArray[pos]; + } + + type& word(uint32_t pos) + { + // get word at pos + return mArray[pos]; + } + protected: + void init(type value = 0) + { + // set all words to value + for (int pos = Words; 0 <= pos; --pos) + mArray[pos] = value; + if (value != 0) + trim(); + } + + void trim() + { + // clear any trailing bits in last word + if (Bits % Bitsperword != 0) + mArray[Words] &= ((type)1 << Bits % Bitsperword) - 1; + } + + type mArray[Words+1]; + }; +} diff --git a/Runtime/mecanim/defs.h b/Runtime/mecanim/defs.h new file mode 100644 index 0000000..734dc10 --- /dev/null +++ b/Runtime/mecanim/defs.h @@ -0,0 +1,116 @@ +#pragma once + +/*** +* defs.h - definitions/declarations for some commonly standard declaration +* +* +* Purpose: +* This file defines the following ma keywords: +* RESTRICT +* MECANIM_FORCE_INLINE +* STATIC_INLINE +* ATTRIBUTE_ALIGN(a) +* EXPLICIT_TYPENAME +* EXPLICIT_TEMPLATE +* DLL_IMPORT +* DLL_EXPORT +* DECLARE_C +* +****/ + +#if defined(__INTEL_COMPILER) || defined(__ICL) + #include <cstddef> + #define RESTRICT __restrict + #define MECANIM_FORCE_INLINE __forceinline + #define ATTRIBUTE_ALIGN(a) __declspec(align(a)) + #define EXPLICIT_TEMPLATE template + #define DLL_IMPORT __declspec(dllimport) + #define DLL_EXPORT __declspec(dllexport) + #define ALIGN4F 16 + #define DECLARE_C __cdecl + +#elif defined(_MSC_VER) + #include <cstddef> + #define RESTRICT __restrict + #define MECANIM_FORCE_INLINE __forceinline + #define ATTRIBUTE_ALIGN(a) __declspec(align(a)) + #define EXPLICIT_TEMPLATE template + #define DLL_IMPORT __declspec(dllimport) + #define DLL_EXPORT __declspec(dllexport) + #define ALIGN4F 16 + #define DECLARE_C __cdecl + + #pragma warning( disable : 4996) + +#elif defined(__GNUC__) + #include <cstddef> + #if ((__GNUC__ >= 3) && (__GNUC_MINOR__ >= 1)) || (__GNUC__ >= 4) + #ifdef _DEBUG + #ifndef MECANIM_FORCE_INLINE + #define MECANIM_FORCE_INLINE inline + #endif + #define STATIC_INLINE inline + #else + #ifndef MECANIM_FORCE_INLINE + #define MECANIM_FORCE_INLINE inline __attribute__((always_inline)) + #endif + #define STATIC_INLINE inline __attribute__((always_inline)) + #endif + #else + #define STATIC_INLINE extern inline + #endif + + #define ATTRIBUTE_ALIGN(a) __attribute__ ((aligned(a))) + #define ALIGN4F 16 + + #if ((__GNUC__ >= 3) && (__GNUC_MINOR__ >= 4)) || (__GNUC__ >= 4) + #define EXPLICIT_TEMPLATE template + #endif +#endif + +#if defined(__GNUC__) && ((__GNUC__ <= 4) && (__GNUC_MINOR__ <= 2)) + #define TEMPLATE_SPEC(L, R) template<L,R> +#else + #define TEMPLATE_SPEC(L, R) template<> +#endif + +#ifndef RESTRICT + #define RESTRICT +#endif + +#ifndef MECANIM_FORCE_INLINE + #define MECANIM_FORCE_INLINE inline +#endif + +#ifndef STATIC_INLINE + #define STATIC_INLINE static inline +#endif + +#ifndef ATTRIBUTE_ALIGN + #define ATTRIBUTE_ALIGN(a) +#endif + +#ifndef EXPLICIT_TYPENAME + #define EXPLICIT_TYPENAME typename +#endif + +#ifndef EXPLICIT_TEMPLATE + #define EXPLICIT_TEMPLATE +#endif + +#ifndef DLL_IMPORT + #define DLL_IMPORT +#endif + +#ifndef DLL_EXPORT + #define DLL_EXPORT +#endif + +#ifndef ALIGN4F + #define ALIGN4F 16 +#endif + +#ifndef DECLARE_C + #define DECLARE_C +#endif + diff --git a/Runtime/mecanim/generic/crc32.h b/Runtime/mecanim/generic/crc32.h new file mode 100644 index 0000000..ca1797b --- /dev/null +++ b/Runtime/mecanim/generic/crc32.h @@ -0,0 +1,208 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/string.h" + +namespace mecanim +{ + template < std::size_t Bits > struct reflector + { + typedef typename mecanim::uint_t<Bits>::value_type value_type; + static value_type reflect( value_type x ); + }; + + // Function that reflects its argument + template < std::size_t Bits > typename reflector<Bits>::value_type reflector<Bits>::reflect( typename reflector<Bits>::value_type x) + { + value_type reflection = 0; + value_type const one = 1; + + for ( std::size_t i = 0 ; i < Bits ; ++i, x >>= 1 ) + { + if ( x & one ) + { + reflection |= ( one << (Bits - 1u - i) ); + } + } + + return reflection; + } + + template <uint32_t TruncPoly> struct crc32_table_t + { + static const uint32_t byte_combos = (1ul << CHAR_BIT); + + typedef uint32_t value_type; + typedef value_type table_type[byte_combos]; + + static void init_table(); + + static table_type table; + static bool isInitialized; + }; + + template <uint32_t TruncPoly> typename crc32_table_t<TruncPoly>::table_type crc32_table_t<TruncPoly>::table = { 0 }; + template <uint32_t TruncPoly> bool crc32_table_t<TruncPoly>::isInitialized = false; + + // Populate CRC lookup table + template<uint32_t TruncPoly> void crc32_table_t<TruncPoly>::init_table() + { + // factor-out constants to avoid recalculation + value_type const fast_hi_bit = 1ul << ( 32 - 1u ); + unsigned char const byte_hi_bit = 1u << (CHAR_BIT - 1u); + + // loop over every possible dividend value + unsigned char dividend = 0; + do + { + value_type remainder = 0; + + // go through all the dividend's bits + for ( unsigned char mask = byte_hi_bit ; mask ; mask >>= 1 ) + { + // check if divisor fits + if ( dividend & mask ) + { + remainder ^= fast_hi_bit; + } + + // do polynominal division + if ( remainder & fast_hi_bit ) + { + remainder <<= 1; + remainder ^= TruncPoly; + } + else + { + remainder <<= 1; + } + } + + table[ reflector<CHAR_BIT>::reflect(dividend) ] = reflector<32>::reflect( remainder ); + } + while ( ++dividend ); + + isInitialized = true; + } + + class crc32 + { + public: + // Type + typedef uint32_t value_type; + + // Constants for the template parameters + static const std::size_t bit_count = 32; + static const value_type truncated_polynominal = 0x04C11DB7; + static const value_type initial_remainder = 0xFFFFFFFF; + static const value_type final_xor_value = 0xFFFFFFFF; + + // Constructor + explicit crc32( value_type init_rem = crc32::initial_remainder ):rem(reflector<bit_count>::reflect(init_rem)){} + + inline void process_block(void const *bytes_begin, void const *bytes_end); + inline void process_block_skip2(void const * bytes_begin, void const * bytes_end); + + inline void process_bytes(void const *buffer, std::size_t byte_count ); + inline void process_bytes_skip2(void const * buffer, std::size_t byte_count); + + inline value_type checksum() const; + + typedef crc32_table_t<truncated_polynominal> crc32_table_type; + + protected: + + inline value_type get_truncated_polynominal() const{ return truncated_polynominal; } + inline value_type get_initial_remainder() const{return initial_remainder;} + inline value_type get_final_xor_value() const{return final_xor_value;} + + static unsigned char index( value_type rem, unsigned char x ){ return static_cast<unsigned char>(x ^ rem); } + + // Shift out the remainder's highest byte + static value_type shift( value_type rem ){ return rem >> CHAR_BIT; } + + // Member data + value_type rem; + }; + + inline void crc32::process_block(void const * bytes_begin, void const * bytes_end) + { + Assert(crc32_table_type::isInitialized); + + // Recompute the CRC for each byte passed + for ( unsigned char const * p = static_cast<unsigned char const *>(bytes_begin) ; p < bytes_end ; ++p ) + { + // Compare the new byte with the remainder's higher bits to + // get the new bits, shift out the remainder's current higher + // bits, and update the remainder with the polynominal division + // of the new bits. + unsigned char const byte_index = index( rem, *p ); + rem = shift( rem ); + rem ^= crc32_table_type::table[ byte_index ]; + } + } + + inline void crc32::process_block_skip2(void const * bytes_begin, void const * bytes_end) + { + Assert(crc32_table_type::isInitialized); + unsigned char const * p; + +#if UNITY_BIG_ENDIAN + p = static_cast<unsigned char const *>(bytes_begin) + 1; +#else + p = static_cast<unsigned char const *>(bytes_begin); +#endif + + // Recompute the CRC for every second byte passed. This is useful for hashing a UTF16 string that is known to actually be an ascii string. + for ( ; p < bytes_end ; p += 2 ) + { + // Compare the new byte with the remainder's higher bits to + // get the new bits, shift out the remainder's current higher + // bits, and update the remainder with the polynominal division + // of the new bits. + unsigned char const byte_index = index( rem, *p ); + rem = shift( rem ); + rem ^= crc32_table_type::table[ byte_index ]; + } + } + + + inline void crc32::process_bytes(void const * buffer, std::size_t byte_count) + { + unsigned char const * const b = static_cast<unsigned char const *>( buffer ); + process_block( b, b + byte_count ); + } + + inline void crc32::process_bytes_skip2(void const * buffer, std::size_t byte_count) + { + unsigned char const * const b = static_cast<unsigned char const *>( buffer ); + process_block_skip2( b, b + byte_count ); + } + + inline crc32::value_type crc32::checksum() const + { + return ( rem ^ get_final_xor_value() ); + } + + static inline int processCRC32(String const& string) + { + crc32 result; + result.process_bytes(string.c_str(), string.size()); + return result.checksum(); + } + + static inline int processCRC32(char const* string) + { + crc32 result; + result.process_bytes(string, strlen(string)); + return result.checksum(); + } + + static inline int processCRC32UTF16Ascii(unsigned short const* string, std::size_t stringLength) + { + crc32 result; + result.process_bytes_skip2(string, stringLength * 2); + return result.checksum(); + } +} diff --git a/Runtime/mecanim/generic/stringtable.cpp b/Runtime/mecanim/generic/stringtable.cpp new file mode 100644 index 0000000..85cf025 --- /dev/null +++ b/Runtime/mecanim/generic/stringtable.cpp @@ -0,0 +1,131 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/generic/stringtable.h" +#include "Runtime/mecanim/generic/crc32.h" + +#define DEFINE_KEYWORD(s) table[mecanim::e##s] = mecanim::ReserveKeyword(mecanim::processCRC32(#s), #s) + +namespace +{ + mecanim::ReserveKeyword* InitTable() + { + static mecanim::ReserveKeyword table[mecanim::eLastString]; + + DEFINE_KEYWORD(T); + DEFINE_KEYWORD(Q); + DEFINE_KEYWORD(S); + DEFINE_KEYWORD(A); + DEFINE_KEYWORD(B); + DEFINE_KEYWORD(C); + DEFINE_KEYWORD(D); + DEFINE_KEYWORD(E); + DEFINE_KEYWORD(X); + DEFINE_KEYWORD(Y); + DEFINE_KEYWORD(Z); + DEFINE_KEYWORD(W); + DEFINE_KEYWORD(Result); + DEFINE_KEYWORD(Min); + DEFINE_KEYWORD(Max); + DEFINE_KEYWORD(Value); + DEFINE_KEYWORD(MinMin); + DEFINE_KEYWORD(MinMax); + DEFINE_KEYWORD(MaxMin); + DEFINE_KEYWORD(MaxMax); + DEFINE_KEYWORD(In); + DEFINE_KEYWORD(Out); + DEFINE_KEYWORD(RangeA); + DEFINE_KEYWORD(RangeB); + DEFINE_KEYWORD(RangeC); + DEFINE_KEYWORD(RangeD); + DEFINE_KEYWORD(RangeE); + DEFINE_KEYWORD(WeightA); + DEFINE_KEYWORD(WeightB); + DEFINE_KEYWORD(WeightC); + DEFINE_KEYWORD(WeightD); + DEFINE_KEYWORD(WeightE); + DEFINE_KEYWORD(OutA); + DEFINE_KEYWORD(OutB); + DEFINE_KEYWORD(OutC); + DEFINE_KEYWORD(OutD); + DEFINE_KEYWORD(OutE); + DEFINE_KEYWORD(Num); + DEFINE_KEYWORD(Den); + DEFINE_KEYWORD(Rem); + DEFINE_KEYWORD(DampTime); + DEFINE_KEYWORD(DeltaTime); + DEFINE_KEYWORD(PreviousValue); + DEFINE_KEYWORD(GravityWeight); + DEFINE_KEYWORD(SrcRefX); + DEFINE_KEYWORD(DstRefX); + DEFINE_KEYWORD(SrcPivotX); + DEFINE_KEYWORD(DstPivotX); + DEFINE_KEYWORD(RefWeight); + DEFINE_KEYWORD(PivotWeight); + DEFINE_KEYWORD(XI); + DEFINE_KEYWORD(XO); + DEFINE_KEYWORD(Condition); + DEFINE_KEYWORD(StateTime); + DEFINE_KEYWORD(StateSpeed); + DEFINE_KEYWORD(StateExitTime); + DEFINE_KEYWORD(DoTransition); + DEFINE_KEYWORD(NextStateStartTime); + DEFINE_KEYWORD(TransitionDuration); + DEFINE_KEYWORD(TransitionOffset); + DEFINE_KEYWORD(TransitionStartTime); + DEFINE_KEYWORD(StateMachineWeight); + DEFINE_KEYWORD(TransitionTime); + DEFINE_KEYWORD(BlendWeight); + DEFINE_KEYWORD(StateWeight); + DEFINE_KEYWORD(StabilizeFeet); + DEFINE_KEYWORD(RootX); + table[mecanim::eLeftFootWeightT] = mecanim::ReserveKeyword(mecanim::processCRC32("LeftFoot.WeightT"), "LeftFoot.WeightT"); + table[mecanim::eLeftFootWeightR] = mecanim::ReserveKeyword(mecanim::processCRC32("LeftFoot.WeightR"), "LeftFoot.WeightR"); + table[mecanim::eRightFootWeightT] = mecanim::ReserveKeyword(mecanim::processCRC32("RightFoot.WeightT"), "RightFoot.WeightT"); + table[mecanim::eRightFootWeightR] = mecanim::ReserveKeyword(mecanim::processCRC32("RightFoot.WeightR"), "RightFoot.WeightR"); + DEFINE_KEYWORD(ComputeSource); + DEFINE_KEYWORD(LookAt); + DEFINE_KEYWORD(LeftFootX); + DEFINE_KEYWORD(RightFootX); + DEFINE_KEYWORD(LeftFootSpeedT); + DEFINE_KEYWORD(LeftFootSpeedQ); + DEFINE_KEYWORD(RightFootSpeedT); + DEFINE_KEYWORD(RightFootSpeedQ); + DEFINE_KEYWORD(LeftFootStableT); + DEFINE_KEYWORD(LeftFootStableQ); + DEFINE_KEYWORD(RightFootStableT); + DEFINE_KEYWORD(RightFootStableQ); + DEFINE_KEYWORD(RootSpeedT); + DEFINE_KEYWORD(RootSpeedQ); + DEFINE_KEYWORD(RootStableT); + DEFINE_KEYWORD(RootStableQ); + DEFINE_KEYWORD(LeftFootProjX); + DEFINE_KEYWORD(RightFootProjX); + DEFINE_KEYWORD(PlantFeet); + DEFINE_KEYWORD(LeftFootSafeX); + DEFINE_KEYWORD(RightFootSafeX); + DEFINE_KEYWORD(PositionX); + DEFINE_KEYWORD(PositionY); + DEFINE_KEYWORD(PositionZ); + DEFINE_KEYWORD(QuaternionX); + DEFINE_KEYWORD(QuaternionY); + DEFINE_KEYWORD(QuaternionZ); + DEFINE_KEYWORD(QuaternionW); + DEFINE_KEYWORD(ScaleX); + DEFINE_KEYWORD(ScaleY); + DEFINE_KEYWORD(ScaleZ); + DEFINE_KEYWORD(DynamicCurve); + return table; + } +} + +namespace mecanim +{ + ReserveKeyword* ReserveKeywordTable() + { + static ReserveKeyword* s_Table = InitTable(); + return s_Table; + } + uint32_t CRCKey(eString id) + { + return ReserveKeywordTable()[id].m_ID; + } + } diff --git a/Runtime/mecanim/generic/stringtable.h b/Runtime/mecanim/generic/stringtable.h new file mode 100644 index 0000000..3bf01f5 --- /dev/null +++ b/Runtime/mecanim/generic/stringtable.h @@ -0,0 +1,128 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/types.h" + + +namespace mecanim +{ + enum eString + { + eT, + eQ, + eS, + eA, + eB, + eC, + eD, + eE, + eX, + eY, + eZ, + eW, + eResult, + eMin, + eMax, + eValue, + eMinMin, + eMinMax, + eMaxMin, + eMaxMax, + eIn, + eOut, + eRangeA, + eRangeB, + eRangeC, + eRangeD, + eRangeE, + eWeightA, + eWeightB, + eWeightC, + eWeightD, + eWeightE, + eOutA, + eOutB, + eOutC, + eOutD, + eOutE, + eNum, + eDen, + eRem, + eDampTime, + eDeltaTime, + ePreviousValue, + eGravityWeight, + eSrcRefX, + eDstRefX, + eSrcPivotX, + eDstPivotX, + eRefWeight, + ePivotWeight, + eXI, + eXO, + eCondition, + eStateTime, + eStateSpeed, + eStateExitTime, + eDoTransition, + eNextStateStartTime, + eTransitionDuration, + eTransitionOffset, + eTransitionStartTime, + eStateMachineWeight, + eTransitionTime, + eBlendWeight, + eStateWeight, + eStabilizeFeet, + eRootX, + eLeftFootWeightT, + eLeftFootWeightR, + eRightFootWeightT, + eRightFootWeightR, + eComputeSource, + eLookAt, + eLeftFootX, + eRightFootX, + eLeftFootSpeedT, + eLeftFootSpeedQ, + eRightFootSpeedT, + eRightFootSpeedQ, + eLeftFootStableT, + eLeftFootStableQ, + eRightFootStableT, + eRightFootStableQ, + eRootSpeedT, + eRootSpeedQ, + eRootStableT, + eRootStableQ, + eLeftFootProjX, + eRightFootProjX, + ePlantFeet, + eLeftFootSafeX, + eRightFootSafeX, + ePositionX, + ePositionY, + ePositionZ, + eQuaternionX, + eQuaternionY, + eQuaternionZ, + eQuaternionW, + eScaleX, + eScaleY, + eScaleZ, + eDynamicCurve, + eLastString + }; + + struct ReserveKeyword + { + ReserveKeyword():m_ID(0),m_Keyword(0){} + ReserveKeyword(uint32_t id, char const* keyword):m_ID(id),m_Keyword(keyword){} + + uint32_t m_ID; + char const* m_Keyword ; + }; + + ReserveKeyword* ReserveKeywordTable(); + uint32_t CRCKey(eString id); +} diff --git a/Runtime/mecanim/generic/typetraits.h b/Runtime/mecanim/generic/typetraits.h new file mode 100644 index 0000000..1f9aa25 --- /dev/null +++ b/Runtime/mecanim/generic/typetraits.h @@ -0,0 +1,47 @@ +#pragma once + +#include "Runtime/mecanim/types.h" +#include "Runtime/Math/Simd/float4.h" +#include "Runtime/Math/Simd/xform.h" +#include "Runtime/Math/Simd/bool4.h" + +namespace mecanim +{ + enum ValueType + { + kFloatType = 1, + kInt32Type = 3, + kBoolType = 4, + kPositionType = 6, + kQuaternionType = 7, + kScaleType = 8, + kTriggerType = 9, + kLastType + }; + + template<typename TYPE> struct traits; + + template<> struct traits<int32_t> + { + typedef int32_t value_type; + + static value_type zero() { return 0; } + static ValueType type() { return kInt32Type; } + }; + + template<> struct traits<float> + { + typedef float value_type; + + static value_type zero() { return 0.f; } + static ValueType type() { return kFloatType; } + }; + + template<> struct traits<bool> + { + typedef bool value_type; + + static value_type zero() { return false; } + static ValueType type() { return kBoolType; } + }; +} diff --git a/Runtime/mecanim/generic/valuearray.cpp b/Runtime/mecanim/generic/valuearray.cpp new file mode 100644 index 0000000..e7bfb58 --- /dev/null +++ b/Runtime/mecanim/generic/valuearray.cpp @@ -0,0 +1,925 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/generic/valuearray.h" + +#include "Runtime/Math/Simd/float4.h" +#include "Runtime/Math/Simd/bool4.h" +#include "Runtime/Math/Simd/xform.h" + +namespace mecanim +{ + template<typename TYPE> + static void ValueCopy(TYPE const * RESTRICT source, TYPE * RESTRICT destination, uint32_t sourceIndex, uint32_t destinationIndex) + { + destination[destinationIndex] = source[sourceIndex]; + } + + template<typename TYPE> + static void ValueArrayCopy(TYPE const * RESTRICT source, TYPE * RESTRICT destination, uint32_t sourceCount, uint32_t destinationCount) + { + sourceCount = min (destinationCount, sourceCount); + + uint32_t i; + for(i = 0; i < sourceCount; ++i) + { + destination[i] = source[i]; + } + } + + template<typename TYPE> + static void ValueArrayCopyMask(TYPE const * RESTRICT source, TYPE * RESTRICT destination, const bool* RESTRICT mask, uint32_t sourceCount) + { + uint32_t i; + for(i = 0; i < sourceCount; ++i) + { + if (mask[i]) + destination[i] = source[i]; + } + } + + void SetupValueArrayConstant(ValueArrayConstant* cst, ValueType aType, uint32_t aCount, memory::Allocator& alloc) + { + cst->m_Count = aCount; + cst->m_ValueArray = alloc.ConstructArray<ValueConstant>(cst->m_Count); + + uint32_t i; + for(i=0;i<aCount;++i) + { + cst->m_ValueArray[i].m_Type = aType; + cst->m_ValueArray[i].m_Index = i; + } + } + + ValueArrayConstant* CreateValueArrayConstant(uint32_t* apTypeArray, uint32_t aCount, memory::Allocator& alloc) + { + SETPROFILERLABEL(ValueArrayConstant); + + ValueArrayConstant* cst = alloc.Construct<ValueArrayConstant>(); + + cst->m_Count = aCount; + cst->m_ValueArray = alloc.ConstructArray<ValueConstant>(cst->m_Count); + + uint32_t positionCount = 0; + uint32_t quaternionCount = 0; + uint32_t scaleCount = 0; + uint32_t floatCount = 0; + uint32_t intCount = 0; + uint32_t boolCount = 0; + + uint32_t i; + for(i=0;i<aCount;++i) + { + cst->m_ValueArray[i].m_Type = apTypeArray[i]; + + switch(apTypeArray[i]) + { + case kPositionType: cst->m_ValueArray[i].m_Index = positionCount++; break; + case kQuaternionType: cst->m_ValueArray[i].m_Index = quaternionCount++; break; + case kScaleType: cst->m_ValueArray[i].m_Index = scaleCount++; break; + case kFloatType: cst->m_ValueArray[i].m_Index = floatCount++; break; + case kInt32Type: cst->m_ValueArray[i].m_Index = intCount++; break; + case kTriggerType: + case kBoolType: cst->m_ValueArray[i].m_Index = boolCount++; break; + default: assert(false); break; + } + } + return cst; + } + + ValueArrayConstant* CreateValueArrayConstant(ValueType aType, uint32_t aCount, memory::Allocator& alloc) + { + + SETPROFILERLABEL(ValueArrayConstant); + ValueArrayConstant* cst = alloc.Construct<ValueArrayConstant>(); + + SetupValueArrayConstant(cst, aType, aCount, alloc); + return cst; + } + + ValueArrayConstant* CreateValueArrayConstantCopy (const ValueArrayConstant* sourceConstant, uint32_t count, memory::Allocator& alloc) + { + + SETPROFILERLABEL(ValueArrayConstant); + + Assert(count <= sourceConstant->m_Count); + + ValueArrayConstant* cst = alloc.Construct<ValueArrayConstant>(); + cst->m_Count = count; + cst->m_ValueArray = alloc.ConstructArray<ValueConstant>(sourceConstant->m_ValueArray.Get(), count); + + return cst; + } + + + void DestroyValueArrayConstant(ValueArrayConstant * apCst, memory::Allocator& alloc) + { + if(apCst) + { + alloc.Deallocate(apCst->m_ValueArray); + alloc.Deallocate(apCst); + } + } + + void SetValueMask(ValueArrayMask *valueArrayMask, bool value) + { + for(int i = 0; i < valueArrayMask->m_PositionCount; i++) + valueArrayMask->m_PositionValues[i] = value; + + for(int i = 0; i < valueArrayMask->m_QuaternionCount; i++) + valueArrayMask->m_QuaternionValues[i] = value; + + for(int i = 0; i < valueArrayMask->m_ScaleCount; i++) + valueArrayMask->m_ScaleValues[i] = value; + + for(int i = 0; i < valueArrayMask->m_FloatCount; i++) + valueArrayMask->m_FloatValues[i] = value; + + for(int i = 0; i < valueArrayMask->m_IntCount; i++) + valueArrayMask->m_IntValues[i] = value; + } + + void CopyValueMask(ValueArrayMask *valueArrayMask,ValueArrayMask const *srcValueArrayMask) + { + for(int i = 0; i < valueArrayMask->m_PositionCount; i++) + valueArrayMask->m_PositionValues[i] = srcValueArrayMask->m_PositionValues[i]; + + for(int i = 0; i < valueArrayMask->m_QuaternionCount; i++) + valueArrayMask->m_QuaternionValues[i] = srcValueArrayMask->m_QuaternionValues[i]; + + for(int i = 0; i < valueArrayMask->m_ScaleCount; i++) + valueArrayMask->m_ScaleValues[i] = srcValueArrayMask->m_ScaleValues[i]; + + for(int i = 0; i < valueArrayMask->m_FloatCount; i++) + valueArrayMask->m_FloatValues[i] = srcValueArrayMask->m_FloatValues[i]; + + for(int i = 0; i < valueArrayMask->m_IntCount; i++) + valueArrayMask->m_IntValues[i] = srcValueArrayMask->m_IntValues[i]; + } + + void OrValueMask(ValueArrayMask *valueArrayMask,ValueArrayMask const *srcValueArrayMask) + { + for(int i = 0; i < valueArrayMask->m_PositionCount; i++) + valueArrayMask->m_PositionValues[i] = valueArrayMask->m_PositionValues[i] || srcValueArrayMask->m_PositionValues[i]; + + for(int i = 0; i < valueArrayMask->m_QuaternionCount; i++) + valueArrayMask->m_QuaternionValues[i] = valueArrayMask->m_QuaternionValues[i] || srcValueArrayMask->m_QuaternionValues[i]; + + for(int i = 0; i < valueArrayMask->m_ScaleCount; i++) + valueArrayMask->m_ScaleValues[i] = valueArrayMask->m_ScaleValues[i] || srcValueArrayMask->m_ScaleValues[i]; + + for(int i = 0; i < valueArrayMask->m_FloatCount; i++) + valueArrayMask->m_FloatValues[i] = valueArrayMask->m_FloatValues[i] || srcValueArrayMask->m_FloatValues[i]; + + for(int i = 0; i < valueArrayMask->m_IntCount; i++) + valueArrayMask->m_IntValues[i] = valueArrayMask->m_IntValues[i] || srcValueArrayMask->m_IntValues[i]; + } + + void AndValueMask(ValueArrayMask *valueArrayMask,ValueArrayMask const *srcValueArrayMask) + { + for(int i = 0; i < valueArrayMask->m_PositionCount; i++) + valueArrayMask->m_PositionValues[i] = valueArrayMask->m_PositionValues[i] && srcValueArrayMask->m_PositionValues[i]; + + for(int i = 0; i < valueArrayMask->m_QuaternionCount; i++) + valueArrayMask->m_QuaternionValues[i] = valueArrayMask->m_QuaternionValues[i] && srcValueArrayMask->m_QuaternionValues[i]; + + for(int i = 0; i < valueArrayMask->m_ScaleCount; i++) + valueArrayMask->m_ScaleValues[i] = valueArrayMask->m_ScaleValues[i] && srcValueArrayMask->m_ScaleValues[i]; + + for(int i = 0; i < valueArrayMask->m_FloatCount; i++) + valueArrayMask->m_FloatValues[i] = valueArrayMask->m_FloatValues[i] && srcValueArrayMask->m_FloatValues[i]; + + for(int i = 0; i < valueArrayMask->m_IntCount; i++) + valueArrayMask->m_IntValues[i] = valueArrayMask->m_IntValues[i] && srcValueArrayMask->m_IntValues[i]; + } + + void InvertValueMask(ValueArrayMask *valueArrayMask) + { + for(int i = 0; i < valueArrayMask->m_PositionCount; i++) + valueArrayMask->m_PositionValues[i] = !valueArrayMask->m_PositionValues[i]; + + for(int i = 0; i < valueArrayMask->m_QuaternionCount; i++) + valueArrayMask->m_QuaternionValues[i] = !valueArrayMask->m_QuaternionValues[i]; + + for(int i = 0; i < valueArrayMask->m_ScaleCount; i++) + valueArrayMask->m_ScaleValues[i] = !valueArrayMask->m_ScaleValues[i]; + + for(int i = 0; i < valueArrayMask->m_FloatCount; i++) + valueArrayMask->m_FloatValues[i] = !valueArrayMask->m_FloatValues[i]; + + for(int i = 0; i < valueArrayMask->m_IntCount; i++) + valueArrayMask->m_IntValues[i] = !valueArrayMask->m_IntValues[i]; + } + + ValueArrayMask* CreateValueArrayMask(ValueArrayConstant const* constant, memory::Allocator& alloc) + { + SETPROFILERLABEL(ValueArrayMask); + ValueArrayMask* valueArrayMask = alloc.Construct<ValueArrayMask>(); + + uint32_t i; + for(i=0;i<constant->m_Count;++i) + { + switch(constant->m_ValueArray[i].m_Type) + { + case kPositionType: valueArrayMask->m_PositionCount++; break; + case kQuaternionType: valueArrayMask->m_QuaternionCount++; break; + case kScaleType: valueArrayMask->m_ScaleCount++; break; + case kFloatType: valueArrayMask->m_FloatCount++; break; + case kInt32Type: valueArrayMask->m_IntCount++; break; + default: assert(false); break; + } + } + + valueArrayMask->m_IntValues = alloc.ConstructArray<bool>(valueArrayMask->m_IntCount); + valueArrayMask->m_FloatValues = alloc.ConstructArray<bool>(valueArrayMask->m_FloatCount); + valueArrayMask->m_PositionValues = alloc.ConstructArray<bool>(valueArrayMask->m_PositionCount); + valueArrayMask->m_QuaternionValues = alloc.ConstructArray<bool>(valueArrayMask->m_QuaternionCount); + valueArrayMask->m_ScaleValues = alloc.ConstructArray<bool>(valueArrayMask->m_ScaleCount); + + SetValueMask (valueArrayMask, false); + + return valueArrayMask; + } + + void DestroyValueArrayMask(ValueArrayMask *valueArrayMask, memory::Allocator& alloc) + { + if(valueArrayMask) + { + alloc.Deallocate(valueArrayMask->m_IntValues); + alloc.Deallocate(valueArrayMask->m_FloatValues); + alloc.Deallocate(valueArrayMask->m_PositionValues); + alloc.Deallocate(valueArrayMask->m_QuaternionValues); + alloc.Deallocate(valueArrayMask->m_ScaleValues); + alloc.Deallocate(valueArrayMask); + } + } + + ValueArray* CreateValueArray(ValueArrayConstant const* apValueArrayConstant, memory::Allocator& alloc) + { + SETPROFILERLABEL(ValueArray); + ValueArray* valueArray = alloc.Construct<ValueArray>(); + + uint32_t i; + for(i=0;i<apValueArrayConstant->m_Count;++i) + { + switch(apValueArrayConstant->m_ValueArray[i].m_Type) + { + case kPositionType: valueArray->m_PositionCount++; break; + case kQuaternionType: valueArray->m_QuaternionCount++; break; + case kScaleType: valueArray->m_ScaleCount++; break; + case kFloatType: valueArray->m_FloatCount++; break; + case kInt32Type: valueArray->m_IntCount++; break; + case kTriggerType: + case kBoolType: valueArray->m_BoolCount++; break; + default: assert(false); break; + } + } + + valueArray->m_BoolValues = alloc.ConstructArray<bool>(valueArray->m_BoolCount); + valueArray->m_IntValues = alloc.ConstructArray<int32_t>(valueArray->m_IntCount); + valueArray->m_FloatValues = alloc.ConstructArray<float>(valueArray->m_FloatCount); + valueArray->m_PositionValues = alloc.ConstructArray<math::float4>(valueArray->m_PositionCount); + valueArray->m_QuaternionValues = alloc.ConstructArray<math::float4>(valueArray->m_QuaternionCount); + valueArray->m_ScaleValues = alloc.ConstructArray<math::float4>(valueArray->m_ScaleCount); + + for(i=0;i<valueArray->m_BoolCount;++i) + valueArray->m_BoolValues[i] = false; + + for(i=0;i<valueArray->m_IntCount;++i) + valueArray->m_IntValues[i] = 0; + + for(i=0;i<valueArray->m_FloatCount;++i) + valueArray->m_FloatValues[i] = 0.f; + + for(i=0;i<valueArray->m_PositionCount;++i) + valueArray->m_PositionValues[i] = math::float4::zero(); + + for(i=0;i<valueArray->m_QuaternionCount;++i) + valueArray->m_QuaternionValues[i] = math::quatIdentity(); + + for(i=0;i<valueArray->m_ScaleCount;++i) + valueArray->m_ScaleValues[i] = math::float4::one(); + + return valueArray; + } + + void DestroyValueArray(ValueArray * apInput, memory::Allocator& alloc) + { + if(apInput) + { + alloc.Deallocate(apInput->m_BoolValues); + alloc.Deallocate(apInput->m_IntValues); + alloc.Deallocate(apInput->m_FloatValues); + alloc.Deallocate(apInput->m_PositionValues); + alloc.Deallocate(apInput->m_QuaternionValues); + alloc.Deallocate(apInput->m_ScaleValues); + + alloc.Deallocate(apInput); + } + } + + void ValueArrayCopy(ValueArray const* apSourceValueArray, ValueArray * apDestinationValueArray) + { + ValueArrayCopy(apSourceValueArray->m_BoolValues.Get(), apDestinationValueArray->m_BoolValues.Get(), apSourceValueArray->m_BoolCount, apDestinationValueArray->m_BoolCount); + ValueArrayCopy(apSourceValueArray->m_IntValues.Get(), apDestinationValueArray->m_IntValues.Get(), apSourceValueArray->m_IntCount, apDestinationValueArray->m_IntCount); + ValueArrayCopy(apSourceValueArray->m_FloatValues.Get(), apDestinationValueArray->m_FloatValues.Get(), apSourceValueArray->m_FloatCount, apDestinationValueArray->m_FloatCount); + ValueArrayCopy(apSourceValueArray->m_PositionValues.Get(), apDestinationValueArray->m_PositionValues.Get(), apSourceValueArray->m_PositionCount, apDestinationValueArray->m_PositionCount); + ValueArrayCopy(apSourceValueArray->m_QuaternionValues.Get(), apDestinationValueArray->m_QuaternionValues.Get(), apSourceValueArray->m_QuaternionCount, apDestinationValueArray->m_QuaternionCount); + ValueArrayCopy(apSourceValueArray->m_ScaleValues.Get(), apDestinationValueArray->m_ScaleValues.Get(), apSourceValueArray->m_ScaleCount, apDestinationValueArray->m_ScaleCount); + } + + void ValueArrayCopy(ValueArrayConstant const* sourceConstant, ValueArray const* source, ValueArrayConstant const* destinationConstant, ValueArray* destination, int32_t const* destinationInSourceIndexArray) + { + int dstCount = destinationConstant->m_Count; + + for(int dstIter = 0; dstIter < dstCount; dstIter++) + { + int32_t srcIndex = destinationInSourceIndexArray[dstIter]; + int32_t dstIndex = dstIter; + + if(srcIndex != -1 && sourceConstant->m_ValueArray[srcIndex].m_Type == destinationConstant->m_ValueArray[dstIndex].m_Type) + { + switch(sourceConstant->m_ValueArray[srcIndex].m_Type) + { + case kPositionType: + { + math::float4 value = source->ReadPosition(sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WritePosition(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + case kQuaternionType: + { + math::float4 value = source->ReadQuaternion(sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WriteQuaternion(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + case kScaleType: + { + math::float4 value = source->ReadScale(sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WriteScale(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + case kFloatType: + { + float value; + source->ReadData(value, sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WriteData(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + case kInt32Type: + { + int32_t value; + source->ReadData(value, sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WriteData(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + case kTriggerType: + case kBoolType: + { + bool value; + source->ReadData(value, sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WriteData(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + } + } + } + } + + void ValueArrayReverseCopy(ValueArrayConstant const* sourceConstant, ValueArray const* source, ValueArrayConstant const* destinationConstant, ValueArray* destination, int32_t const* sourceInDestinationIndexArray) + { + int srcCount = sourceConstant->m_Count; + + for(int srcIter = 0; srcIter < srcCount; srcIter++) + { + int32_t dstIndex = sourceInDestinationIndexArray[srcIter]; + int32_t srcIndex = srcIter; + + if(dstIndex != -1 && sourceConstant->m_ValueArray[srcIndex].m_Type == destinationConstant->m_ValueArray[dstIndex].m_Type) + { + switch(sourceConstant->m_ValueArray[srcIndex].m_Type) + { + case kPositionType: + { + math::float4 value = source->ReadPosition(sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WritePosition(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + case kQuaternionType: + { + math::float4 value = source->ReadQuaternion(sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WriteQuaternion(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + case kScaleType: + { + math::float4 value = source->ReadScale(sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WriteScale(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + case kFloatType: + { + float value; + source->ReadData(value, sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WriteData(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + case kInt32Type: + { + int32_t value; + source->ReadData(value, sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WriteData(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + case kTriggerType: + case kBoolType: + { + bool value; + source->ReadData(value, sourceConstant->m_ValueArray[srcIndex].m_Index); + destination->WriteData(value, destinationConstant->m_ValueArray[dstIndex].m_Index); + break; + } + } + } + } + } + + void ValueArrayCopy(ValueArray const *aSource, ValueArray* aValues, ValueArrayMask const *mask) + { + ValueArrayCopyMask (aSource->m_PositionValues.Get(), aValues->m_PositionValues.Get(), mask->m_PositionValues.Get(), aValues->m_PositionCount); + ValueArrayCopyMask (aSource->m_QuaternionValues.Get(), aValues->m_QuaternionValues.Get(), mask->m_QuaternionValues.Get(), aValues->m_QuaternionCount); + ValueArrayCopyMask (aSource->m_ScaleValues.Get(), aValues->m_ScaleValues.Get(), mask->m_ScaleValues.Get(), aValues->m_ScaleCount); + ValueArrayCopyMask (aSource->m_FloatValues.Get(), aValues->m_FloatValues.Get(), mask->m_FloatValues.Get(), aValues->m_FloatCount); + ValueArrayCopyMask (aSource->m_IntValues.Get(), aValues->m_IntValues.Get(), mask->m_IntValues.Get(), aValues->m_IntCount); + ValueArrayCopyMask (aSource->m_BoolValues.Get(), aValues->m_BoolValues.Get(), mask->m_BoolValues.Get(), aValues->m_BoolCount); + } + + static uint32_t GetLargestBlendIndex(const float *apWeightArray, uint32_t aCount) + { + float largest = apWeightArray[0]; + uint32_t index = 0; + for (int i=1;i<aCount;i++) + { + if (apWeightArray[i] > largest) + { + index = i; + largest = apWeightArray[i]; + } + } + + return index; + } + + + void ValueArrayBlend(ValueArray const *apValuesDefault,ValueArray* aValues, ValueArray ** aValuesArray, const float *apWeightArray, uint32_t aCount, ValueArrayMask const *mask) + { + // Blend positions + for (int valueIndex=0;valueIndex<aValues->m_PositionCount;valueIndex++) + { + if(!mask->m_PositionValues[valueIndex]) + continue; + + float sumW = 0; + math::float4 value4 = math::float4::zero(); + + for(int blendIter = 0; blendIter < aCount; blendIter++) + { + sumW += apWeightArray[blendIter]; + math::float1 w = math::float1(apWeightArray[blendIter]); + + math::float4 valuei = aValuesArray[blendIter]->ReadPosition(valueIndex); + value4 += valuei*w; + } + + if(sumW < 1.0f) + { + math::float1 wd = math::float1(1.0-sumW); + math::float4 valued = apValuesDefault->ReadPosition(valueIndex); + value4 += valued*math::float1(wd); + } + + aValues->WritePosition(value4,valueIndex); + } + + // Blend Quaternions + for (int valueIndex=0;valueIndex<aValues->m_QuaternionCount;valueIndex++) + { + if(!mask->m_QuaternionValues[valueIndex]) + continue; + + float sumW = 0; + math::float4 value4 = math::float4::zero(); + + for(int blendIter = 0; blendIter < aCount; blendIter++) + { + sumW += apWeightArray[blendIter]; + math::float1 w = math::float1(apWeightArray[blendIter]); + + math::float4 valuei = aValuesArray[blendIter]->ReadQuaternion(valueIndex); + value4 += math::cond(math::dot(value4,valuei) < math::float1::zero(), valuei * -w, valuei * w); + } + + if(sumW < 1.0f) + { + math::float1 wd = math::float1(1.0-sumW); + math::float4 valued = apValuesDefault->ReadQuaternion(valueIndex); + value4 += math::cond(math::dot(value4,valued) < math::float1::zero(), valued * -wd, valued * wd); + } + + value4 = math::normalize(value4); + aValues->WriteQuaternion(value4,valueIndex); + } + + // Blend scale + for (int valueIndex=0;valueIndex<aValues->m_ScaleCount;valueIndex++) + { + if(!mask->m_ScaleValues[valueIndex]) + continue; + + float sumW = 0; + math::float4 value4 = math::float4::one(); + + for(int blendIter = 0; blendIter < aCount; blendIter++) + { + sumW += apWeightArray[blendIter]; + math::float1 w = math::float1(apWeightArray[blendIter]); + + math::float4 valuei = aValuesArray[blendIter]->ReadScale(valueIndex); + math::float4 sng = math::sgn(valuei); + value4 = sng * math::abs( value4 * scaleWeight(valuei, w)); + } + + if(sumW < 1.0f) + { + math::float1 wd = math::float1(1.0-sumW); + + math::float4 valued = apValuesDefault->ReadScale(valueIndex); + math::float4 sng = math::sgn(valued); + value4 = sng * math::abs( value4 * scaleWeight(valued, wd)); + } + + aValues->WriteScale(value4,valueIndex); + } + + // Blend floats + for (int valueIndex=0;valueIndex<aValues->m_FloatCount;valueIndex++) + { + if(!mask->m_FloatValues[valueIndex]) + continue; + + float sumW = 0; + float value = 0; + + for(int blendIter = 0; blendIter < aCount; blendIter++) + { + float w = apWeightArray[blendIter]; + sumW += w; + + float valuei; + aValuesArray[blendIter]->ReadData(valuei,valueIndex); + value += valuei*w; + } + + if(sumW < 1.0f) + { + float wd = 1.0-sumW; + + float valued; + apValuesDefault->ReadData(valued,valueIndex); + value += valued*wd; + } + + aValues->WriteData(value,valueIndex); + } + + // Blend integers (pick by largest weight) + uint32_t largestBlendIndex = GetLargestBlendIndex (apWeightArray, aCount); + for (int valueIndex=0;valueIndex<aValues->m_IntCount;valueIndex++) + { + if(!mask->m_IntValues[valueIndex]) + continue; + + int32_t valueInt; + aValuesArray[largestBlendIndex]->ReadData(valueInt,valueIndex); + aValues->WriteData(valueInt,valueIndex); + } + } + + void ValueArrayAdd(ValueArray const *apValuesDefault, ValueArray const* apValues, ValueArrayMask const *readMask, float aWeight, bool aAdditive, ValueArray* apValuesOut, ValueArrayMask *defaultMask) + { + math::float1 w(aWeight); + float base1; + float value1; + math::float4 base4; + math::float4 value4; + int32_t valueInt; + + // Positions + for (int valueIndex=0;valueIndex<apValues->m_PositionCount;valueIndex++) + { + if(!readMask->m_PositionValues[valueIndex]) + continue; + + value4 = apValues->ReadPosition(valueIndex); + if(aAdditive) + { + if(defaultMask->m_PositionValues[valueIndex]) + base4 = apValuesDefault->ReadPosition(valueIndex); + else + base4 = apValuesOut->ReadPosition(valueIndex); + value4 = base4 + value4 * w; + } + else + { + if(aWeight < 1) + { + if(defaultMask->m_PositionValues[valueIndex]) + base4 = apValuesDefault->ReadPosition(valueIndex); + else + base4 = apValuesOut->ReadPosition(valueIndex); + + value4 = math::lerp(base4,value4,w); + } + } + apValuesOut->WritePosition(value4,valueIndex); + defaultMask->m_PositionValues[valueIndex] = false; + } + + // Quaternions + for (int valueIndex=0;valueIndex<apValues->m_QuaternionCount;valueIndex++) + { + if(!readMask->m_QuaternionValues[valueIndex]) + continue; + + value4 = apValues->ReadQuaternion(valueIndex); + if(aAdditive) + { + if(defaultMask->m_QuaternionValues[valueIndex]) + base4 = apValuesDefault->ReadQuaternion(valueIndex); + else + base4 = apValuesOut->ReadQuaternion(valueIndex); + + value4 = math::quatMul(base4,math::quatWeight(value4,w)); + } + else + { + if(aWeight < 1) + { + if(defaultMask->m_QuaternionValues[valueIndex]) + base4 = apValuesDefault->ReadQuaternion(valueIndex); + else + base4 = apValuesOut->ReadQuaternion(valueIndex); + + value4 = math::quatLerp(base4,value4,w); + } + } + apValuesOut->WriteQuaternion(value4,valueIndex); + defaultMask->m_QuaternionValues[valueIndex] = false; + } + + // Scale + for (int valueIndex=0;valueIndex<apValues->m_ScaleCount;valueIndex++) + { + if(!readMask->m_ScaleValues[valueIndex]) + continue; + + value4 = apValues->ReadScale(valueIndex); + if(aAdditive) + { + if(defaultMask->m_ScaleValues[valueIndex]) + base4 = apValuesDefault->ReadScale(valueIndex); + else + base4 = apValuesOut->ReadScale(valueIndex); + + value4 = base4 * math::scaleWeight(value4,w); + } + else + { + if(aWeight < 1) + { + if(defaultMask->m_ScaleValues[valueIndex]) + base4 = apValuesDefault->ReadScale(valueIndex); + else + base4 = apValuesOut->ReadScale(valueIndex); + + value4 = math::scaleBlend(base4,value4,w); + } + } + apValuesOut->WriteScale(value4,valueIndex); + defaultMask->m_ScaleValues[valueIndex] = false; + } + + // Floats + for (int valueIndex=0;valueIndex<apValues->m_FloatCount;valueIndex++) + { + if(!readMask->m_FloatValues[valueIndex]) + continue; + + apValues->ReadData(value1,valueIndex); + if(aAdditive) + { + if(defaultMask->m_FloatValues[valueIndex]) + apValuesDefault->ReadData(base1,valueIndex); + else + apValuesOut->ReadData(base1,valueIndex); + + value1 = base1 + value1 * aWeight; + } + else + { + if(aWeight < 1) + { + if(defaultMask->m_FloatValues[valueIndex]) + apValuesDefault->ReadData(base1,valueIndex); + else + apValuesOut->ReadData(base1,valueIndex); + + value1 = (1-aWeight) * base1 + value1 * aWeight; + } + } + apValuesOut->WriteData(value1,valueIndex); + defaultMask->m_FloatValues[valueIndex] = false; + } + + // Ints + if (aWeight > 0.5F) + { + for (int valueIndex=0;valueIndex<apValues->m_IntCount;valueIndex++) + { + if(!readMask->m_IntValues[valueIndex]) + continue; + + apValues->ReadData(valueInt,valueIndex); + apValuesOut->WriteData(valueInt,valueIndex); + defaultMask->m_IntValues[valueIndex] = false; + } + } + else + { + for (int valueIndex=0;valueIndex<apValues->m_IntCount;valueIndex++) + { + if(!readMask->m_IntValues[valueIndex]) + continue; + + if(defaultMask->m_IntValues[valueIndex]) + apValuesDefault->ReadData(valueInt,valueIndex); + else + apValuesOut->ReadData(valueInt,valueIndex); + + apValuesOut->WriteData(valueInt,valueIndex); + defaultMask->m_IntValues[valueIndex] = false; + } + } + } + + void ValueArraySub(ValueArray const &starts, ValueArray &values, ValueArrayMask const *mask) + { + float value,start; + math::float4 value4,start4; + + // Positions + for (int valueIndex=0;valueIndex<values.m_PositionCount;valueIndex++) + { + if(!mask->m_PositionValues[valueIndex]) + continue; + + value4 = values.ReadPosition(valueIndex); + start4 = starts.ReadPosition(valueIndex); + + value4 -= start4; + + values.WritePosition(value4,valueIndex); + } + + // Quaternions + for (int valueIndex=0;valueIndex<values.m_QuaternionCount;valueIndex++) + { + if(!mask->m_QuaternionValues[valueIndex]) + continue; + + value4 = values.ReadQuaternion(valueIndex); + start4 = starts.ReadQuaternion(valueIndex); + + value4 = math::normalize(math::quatMul(math::quatConj(start4),value4)); + + values.WriteQuaternion(value4,valueIndex); + } + + // Scale + for (int valueIndex=0;valueIndex<values.m_ScaleCount;valueIndex++) + { + if(!mask->m_ScaleValues[valueIndex]) + continue; + + value4 = values.ReadScale(valueIndex); + start4 = starts.ReadScale(valueIndex); + + value4 /= start4; + + values.WriteScale(value4,valueIndex); + } + + // Floats + for (int valueIndex=0;valueIndex<values.m_FloatCount;valueIndex++) + { + if(!mask->m_FloatValues[valueIndex]) + continue; + + values.ReadData(value,valueIndex); + starts.ReadData(start,valueIndex); + + value -= start; + + values.WriteData(value,valueIndex); + } + + // Integer substraction does not make sense + } + + void ValueArrayLoop( ValueArray const &starts, + ValueArray const &stops, + ValueArray &values, + float loopWeight, + const ValueArrayMask& mask) + { + math::float1 loopWeight1(loopWeight); + + float value,start,stop; + math::float4 value4,start4,stop4; + + // Positions + for (int valueIndex=0;valueIndex<values.m_PositionCount;valueIndex++) + { + if(!mask.m_PositionValues[valueIndex]) + continue; + + value4 = values.ReadPosition(valueIndex); + start4 = starts.ReadPosition(valueIndex); + stop4 = stops.ReadPosition(valueIndex); + + value4 += (start4 - stop4) * loopWeight1; + + values.WritePosition(value4,valueIndex); + } + + // Quaternions + for (int valueIndex=0;valueIndex<values.m_QuaternionCount;valueIndex++) + { + if(!mask.m_QuaternionValues[valueIndex]) + continue; + + value4 = values.ReadQuaternion(valueIndex); + start4 = starts.ReadQuaternion(valueIndex); + stop4 = stops.ReadQuaternion(valueIndex); + + value4 = math::normalize(math::quatMul(value4,math::quatWeight(math::quatMul(math::quatConj(stop4),start4),loopWeight1))); + + values.WriteQuaternion(value4,valueIndex); + } + + // Scales + for (int valueIndex=0;valueIndex<values.m_ScaleCount;valueIndex++) + { + if(!mask.m_ScaleValues[valueIndex]) + continue; + + value4 = values.ReadScale(valueIndex); + start4 = starts.ReadScale(valueIndex); + stop4 = stops.ReadScale(valueIndex); + + value4 *= math::scaleWeight(start4/stop4,loopWeight1); + + values.WriteScale(value4,valueIndex); + } + + // Floats + for (int valueIndex=0;valueIndex<values.m_FloatCount;valueIndex++) + { + if(!mask.m_FloatValues[valueIndex]) + continue; + + values.ReadData(value,valueIndex); + starts.ReadData(start,valueIndex); + stops.ReadData(stop,valueIndex); + + value += (start - stop) * loopWeight; + + values.WriteData(value,valueIndex); + } + } + + int32_t FindValueIndex(const ValueArrayConstant *aValueArrayConstant, uint32_t id) + { + int32_t ret = -1; + if(aValueArrayConstant) + { + uint32_t i; + for( i = 0 ; i < aValueArrayConstant->m_Count ; i++) + { + if(aValueArrayConstant->m_ValueArray[i].m_ID == id) + { + return i; + } + } + } + return ret; + } +} diff --git a/Runtime/mecanim/generic/valuearray.h b/Runtime/mecanim/generic/valuearray.h new file mode 100644 index 0000000..25dcacd --- /dev/null +++ b/Runtime/mecanim/generic/valuearray.h @@ -0,0 +1,256 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" + +#include "Runtime/mecanim/generic/crc32.h" +#include "Runtime/mecanim/generic/typetraits.h" + +#include "Runtime/Serialize/Blobification/offsetptr.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Animation/MecanimArraySerialization.h" + +namespace mecanim +{ + struct ValueConstant + { + DEFINE_GET_TYPESTRING(ValueConstant) + + ValueConstant():m_ID(0),m_Type(kLastType),m_TypeID(0),m_Index(0){} + + uint32_t m_ID; + uint32_t m_TypeID; //@TODO: This is deprecated. We should probably make this webplayer only? + uint32_t m_Type; + uint32_t m_Index; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_ID); + TRANSFER(m_TypeID); + TRANSFER(m_Type); + TRANSFER(m_Index); + } + }; + + struct ValueArrayConstant + { + DEFINE_GET_TYPESTRING(ValueArrayConstant) + + ValueArrayConstant():m_Count(0){} + + uint32_t m_Count; + OffsetPtr<ValueConstant> m_ValueArray; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_Count); + MANUAL_ARRAY_TRANSFER2(ValueConstant, m_ValueArray, m_Count); + } + }; + + struct ValueArrayMask + { + DEFINE_GET_TYPESTRING(ValueArrayConstant) + + ValueArrayMask():m_BoolCount(0),m_IntCount(0),m_FloatCount(0),m_PositionCount(0),m_QuaternionCount(0),m_ScaleCount(0) {} + + uint32_t m_BoolCount; + OffsetPtr<bool> m_BoolValues; + uint32_t m_IntCount; + OffsetPtr<bool> m_IntValues; + uint32_t m_FloatCount; + OffsetPtr<bool> m_FloatValues; + uint32_t m_PositionCount; + OffsetPtr<bool> m_PositionValues; + uint32_t m_QuaternionCount; + OffsetPtr<bool> m_QuaternionValues; + uint32_t m_ScaleCount; + OffsetPtr<bool> m_ScaleValues; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_BoolCount); + MANUAL_ARRAY_TRANSFER2(bool, m_BoolValues, m_BoolCount); + + TRANSFER_BLOB_ONLY(m_IntCount); + MANUAL_ARRAY_TRANSFER2(bool, m_IntValues, m_IntCount); + + TRANSFER_BLOB_ONLY(m_FloatCount); + MANUAL_ARRAY_TRANSFER2(bool, m_FloatValues, m_FloatCount); + + TRANSFER_BLOB_ONLY(m_PositionCount); + MANUAL_ARRAY_TRANSFER2(bool, m_PositionValues, m_PositionCount); + + TRANSFER_BLOB_ONLY(m_QuaternionCount); + MANUAL_ARRAY_TRANSFER2(bool, m_QuaternionValues, m_QuaternionCount); + + TRANSFER_BLOB_ONLY(m_ScaleCount); + MANUAL_ARRAY_TRANSFER2(bool, m_ScaleValues, m_ScaleCount); + } + }; + + struct ValueArray + { + DEFINE_GET_TYPESTRING(ValueArray) + + ValueArray():m_BoolCount(0),m_IntCount(0),m_FloatCount(0),m_PositionCount(0),m_QuaternionCount(0),m_ScaleCount(0) {} + + uint32_t m_BoolCount; + OffsetPtr<bool> m_BoolValues; + uint32_t m_IntCount; + OffsetPtr<int32_t> m_IntValues; + uint32_t m_FloatCount; + OffsetPtr<float> m_FloatValues; + uint32_t m_PositionCount; + OffsetPtr<math::float4> m_PositionValues; + uint32_t m_QuaternionCount; + OffsetPtr<math::float4> m_QuaternionValues; + uint32_t m_ScaleCount; + OffsetPtr<math::float4> m_ScaleValues; + + MECANIM_FORCE_INLINE void ReadData(bool& data, uint32_t index)const + { + Assert(index < m_BoolCount); + data = m_BoolValues[index]; + } + + MECANIM_FORCE_INLINE void WriteData(bool const& data, uint32_t index) + { + Assert(index < m_BoolCount); + m_BoolValues[index] = data; + } + + MECANIM_FORCE_INLINE void ReadData(int32_t& data, uint32_t index)const + { + Assert(index < m_IntCount); + data = m_IntValues[index]; + } + + MECANIM_FORCE_INLINE void WriteData(int32_t const& data, uint32_t index) + { + Assert(index < m_IntCount); + m_IntValues[index] = data; + } + + MECANIM_FORCE_INLINE void ReadData(float& data, uint32_t index)const + { + Assert(index < m_FloatCount); + data = m_FloatValues[index]; + } + + MECANIM_FORCE_INLINE void WriteData(float const& data, uint32_t index) + { + Assert(index < m_FloatCount); + m_FloatValues[index] = data; + } + + MECANIM_FORCE_INLINE math::float4 ReadPosition(uint32_t index)const + { + Assert(index < m_PositionCount); + return m_PositionValues[index]; + } + + MECANIM_FORCE_INLINE void WritePosition(math::float4 const& data, uint32_t index) + { + Assert(index < m_PositionCount); + m_PositionValues[index] = data; + } + + MECANIM_FORCE_INLINE math::float4 ReadQuaternion(uint32_t index)const + { + Assert(index < m_QuaternionCount); + return m_QuaternionValues[index]; + } + + MECANIM_FORCE_INLINE void WriteQuaternion(math::float4 const& data, uint32_t index) + { + Assert(index < m_QuaternionCount); + m_QuaternionValues[index] = data; + } + + MECANIM_FORCE_INLINE math::float4 ReadScale(uint32_t index)const + { + Assert(index < m_ScaleCount); + return m_ScaleValues[index]; + } + + MECANIM_FORCE_INLINE void WriteScale(math::float4 const& data, uint32_t index) + { + Assert(index < m_ScaleCount); + m_ScaleValues[index] = data; + } + + const float* GetFloatValues () const + { + return m_FloatValues.Get(); + } + + float* GetFloatValues () + { + return m_FloatValues.Get(); + } + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_BoolCount); + MANUAL_ARRAY_TRANSFER2(bool, m_BoolValues, m_BoolCount); + transfer.Align(); + + TRANSFER_BLOB_ONLY(m_IntCount); + MANUAL_ARRAY_TRANSFER2(int32_t, m_IntValues, m_IntCount); + + TRANSFER_BLOB_ONLY(m_FloatCount); + MANUAL_ARRAY_TRANSFER2(float, m_FloatValues, m_FloatCount); + + TRANSFER_BLOB_ONLY(m_PositionCount); + MANUAL_ARRAY_TRANSFER2(math::float4, m_PositionValues, m_PositionCount); + + TRANSFER_BLOB_ONLY(m_QuaternionCount); + MANUAL_ARRAY_TRANSFER2(math::float4, m_QuaternionValues, m_QuaternionCount); + + TRANSFER_BLOB_ONLY(m_ScaleCount); + MANUAL_ARRAY_TRANSFER2(math::float4, m_ScaleValues, m_ScaleCount); + } + }; + + void SetupValueArrayConstant(ValueArrayConstant* constant, ValueType aType, uint32_t aCount, memory::Allocator& alloc); + + ValueArrayConstant* CreateValueArrayConstant(uint32_t* typeArray, uint32_t count, memory::Allocator& alloc); + ValueArrayConstant* CreateValueArrayConstantCopy(const ValueArrayConstant* constant, uint32_t count, memory::Allocator& alloc); + ValueArrayConstant* CreateValueArrayConstant(ValueType type, uint32_t count, memory::Allocator& alloc); + void DestroyValueArrayConstant(ValueArrayConstant * constant, memory::Allocator& alloc); + + ValueArrayMask* CreateValueArrayMask(ValueArrayConstant const* constant, memory::Allocator& alloc); + void DestroyValueArrayMask(ValueArrayMask *valueArrayMask, memory::Allocator& alloc); + void SetValueMask(ValueArrayMask *valueArrayMask, bool value); + void CopyValueMask(ValueArrayMask *valueArrayMask,ValueArrayMask const *srcValueArrayMask); + void OrValueMask(ValueArrayMask *valueArrayMask,ValueArrayMask const *srcValueArrayMask); + void AndValueMask(ValueArrayMask *valueArrayMask,ValueArrayMask const *srcValueArrayMask); + void InvertValueMask(ValueArrayMask *valueArrayMask); + + ValueArray* CreateValueArray(ValueArrayConstant const* constant, memory::Allocator& alloc); + void DestroyValueArray(ValueArray * valueArray, memory::Allocator& alloc); + + void ValueArrayCopy(ValueArray const* source, ValueArray* destination); + void ValueArrayCopy(ValueArrayConstant const* sourceConstant, ValueArray const* source, ValueArrayConstant const* destinationConstant, ValueArray* destination, int32_t const* destinationInSourceIndexArray); + void ValueArrayReverseCopy(ValueArrayConstant const* sourceConstant, ValueArray const* source, ValueArrayConstant const* destinationConstant, ValueArray* destination, int32_t const* sourceInDestinationIndexArray); + void ValueArrayCopy(ValueArray const *aSource, ValueArray* aValues, ValueArrayMask const *mask); + + void ValueArrayBlend(ValueArray const *apValuesDefault,ValueArray* aValues, ValueArray ** aValuesArray, const float *apWeightArray, uint32_t aCount, const ValueArrayMask *mask); + + void ValueArrayAdd(ValueArray const *apValuesDefault, ValueArray const* apValues, ValueArrayMask const *readMask, float aWeight, bool aAdditive, ValueArray* apValuesOut, ValueArrayMask *defaultMask); + void ValueArraySub(ValueArray const &starts, ValueArray &values, ValueArrayMask const *mask); + void ValueArrayLoop(ValueArray const &starts, ValueArray const &stops, ValueArray &values, float loopWeight, const ValueArrayMask& mask); + + int32_t FindValueIndex(const ValueArrayConstant *constant, uint32_t id); + + STATIC_INLINE int32_t FindValueIndex(const ValueArrayConstant *constant, char const* binding) + { + return FindValueIndex(constant, processCRC32(binding)); + } +} diff --git a/Runtime/mecanim/graph/binarynode.h b/Runtime/mecanim/graph/binarynode.h new file mode 100644 index 0000000..82ff596 --- /dev/null +++ b/Runtime/mecanim/graph/binarynode.h @@ -0,0 +1,291 @@ +/* + Copyright (c) 7244339 Canada Inc. (Mecanim) + All Rights Reserved. +*/ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/object.h" + +#include "Runtime/Math/Simd/float4.h" + +#include "Runtime/mecanim/graph/plug.h" +#include "Runtime/mecanim/graph/node.h" + +namespace mecanim +{ + +namespace graph +{ + template <typename TYPE1, typename TYPE2, typename RESULT, typename BinaryPolicies> class BinaryNode : public Node + { + public: + TypePlug<TYPE1> mA; + TypePlug<TYPE2> mB; + + TypePlug<RESULT> mResult; + + BinaryNode() + :mA(true, CRCKey(eA) ), + mB(true,CRCKey(eB)), + mResult(false,CRCKey(eResult)) + { + mA.m_Owner = this; + mB.m_Owner = this; + mResult.m_Owner = this; + } + + virtual ~BinaryNode(){} + + virtual uint32_t GetPlugCount()const + { + return 3; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mA; + case 1: return mB; + case 2: + default: return mResult; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mA; + case 1: return mB; + case 2: + default: return mResult; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + TYPE1 a; + TYPE2 b; + RESULT result; + + mA.ReadData(&a, arEvaluationInfo); + mB.ReadData(&b, arEvaluationInfo); + + result = BinaryPolicies::template Operation<TYPE1, TYPE2, RESULT>(a, b); + + mResult.WriteData(&result, arEvaluationInfo); + } + }; + + class AdditionOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return l+r; } + }; + + class SubstractionOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return l-r; } + }; + + class MultiplicationOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return l*r; } + }; + + class DivisionOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return l/r; } + }; + + class GreaterThanOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return l > r; } + }; + class LesserThanOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return l < r; } + }; + class GreaterThanOrEqualOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return l >= r; } + }; + class LesserThanOrEqualOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return l <= r; } + }; + class AndOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return l && r; } + }; + class OrOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return l || r; } + }; + class XorOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return ((!l)&&r)||(l&&(!r)); } + }; + + class AdditionFloat : public BinaryNode<float, float, float, AdditionOp> + { + public: + static const eNodeType mId = AdditionFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class AdditionUInt : public BinaryNode<uint32_t, uint32_t, uint32_t,AdditionOp> + { + public: + static const eNodeType mId = AdditionUIntId; + virtual eNodeType NodeType(){return mId;} + }; + class AdditionInt : public BinaryNode<int32_t, int32_t, int32_t, AdditionOp> + { + public: + static const eNodeType mId = AdditionIntId; + virtual eNodeType NodeType(){return mId;} + }; + class AdditionFloat4 : public BinaryNode<math::float4, math::float4, math::float4, AdditionOp> + { + public: + static const eNodeType mId = AdditionFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + class SubstractionFloat : public BinaryNode<float, float, float, SubstractionOp> + { + public: + static const eNodeType mId = SubstractionFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class SubstractionUInt : public BinaryNode<uint32_t, uint32_t, uint32_t, SubstractionOp> + { + public: + static const eNodeType mId = SubstractionUIntId; + virtual eNodeType NodeType(){return mId;} + }; + class SubstractionInt : public BinaryNode<int32_t, int32_t, int32_t, SubstractionOp> + { + public: + static const eNodeType mId = SubstractionIntId; + virtual eNodeType NodeType(){return mId;} + }; + class SubstractionFloat4 : public BinaryNode<math::float4, math::float4, math::float4, SubstractionOp> + { + public: + static const eNodeType mId = SubstractionFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + class MultiplicationFloat : public BinaryNode<float, float, float, MultiplicationOp> + { + public: + static const eNodeType mId = MultiplicationFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class MultiplicationUInt : public BinaryNode<uint32_t, uint32_t, uint32_t, MultiplicationOp> + { + public: + static const eNodeType mId = MultiplicationUIntId; + virtual eNodeType NodeType(){return mId;} + }; + class MultiplicationInt : public BinaryNode<int32_t, int32_t, int32_t, MultiplicationOp> + { + public: + static const eNodeType mId = MultiplicationIntId; + virtual eNodeType NodeType(){return mId;} + }; + class MultiplicationFloat4 : public BinaryNode<math::float4, math::float4, math::float4, MultiplicationOp> + { + public: + static const eNodeType mId = MultiplicationFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + class DivisionFloat : public BinaryNode<float, float, float, DivisionOp> + { + public: + static const eNodeType mId = DivisionFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class DivisionUInt : public BinaryNode<uint32_t, uint32_t, uint32_t, DivisionOp> + { + public: + static const eNodeType mId = DivisionUIntId; + virtual eNodeType NodeType(){return mId;} + }; + class DivisionInt : public BinaryNode<int32_t, int32_t, int32_t, DivisionOp> + { + public: + static const eNodeType mId = DivisionIntId; + virtual eNodeType NodeType(){return mId;} + }; + class DivisionFloat4 : public BinaryNode<math::float4, math::float4, math::float4, DivisionOp> + { + public: + static const eNodeType mId = DivisionFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + class GreaterThan : public BinaryNode<float, float, bool, GreaterThanOp> + { + public: + static const eNodeType mId = GreaterThanId; + virtual eNodeType NodeType(){return mId;} + }; + + class LesserThan : public BinaryNode<float, float, bool, LesserThanOp> + { + public: + static const eNodeType mId = LesserThanId; + virtual eNodeType NodeType(){return mId;} + }; + + class GreaterThanOrEqual : public BinaryNode<float, float, bool, GreaterThanOrEqualOp> + { + public: + static const eNodeType mId = GreaterThanOrEqualId; + virtual eNodeType NodeType(){return mId;} + }; + + class LesserThanOrEqual : public BinaryNode<float, float, bool, LesserThanOrEqualOp> + { + public: + static const eNodeType mId = LesserThanOrEqualId; + virtual eNodeType NodeType(){return mId;} + }; + + class And : public BinaryNode<bool, bool, bool, AndOp> + { + public: + static const eNodeType mId = AndId; + virtual eNodeType NodeType(){return mId;} + }; + + class Or : public BinaryNode<bool, bool, bool, OrOp> + { + public: + static const eNodeType mId = OrId; + virtual eNodeType NodeType(){return mId;} + }; + class Xor : public BinaryNode<bool, bool, bool, XorOp> + { + public: + static const eNodeType mId = XorId; + virtual eNodeType NodeType(){return mId;} + }; +} + +}
\ No newline at end of file diff --git a/Runtime/mecanim/graph/factory.cpp b/Runtime/mecanim/graph/factory.cpp new file mode 100644 index 0000000..3efe757 --- /dev/null +++ b/Runtime/mecanim/graph/factory.cpp @@ -0,0 +1,172 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/graph/factory.h" + +#include "Runtime/mecanim/graph/unarynode.h" +#include "Runtime/mecanim/graph/binarynode.h" +#include "Runtime/mecanim/graph/genericnode.h" +#include "Runtime/mecanim/graph/xformnode.h" +#include "Runtime/mecanim/graph/quaternionnode.h" + +#define REGISTERNODE(classnode) \ + case classnode::mId: \ + { \ + node = arAlloc.Construct<classnode>(); \ + break; \ + } + +namespace mecanim +{ + +namespace graph +{ + Node* GraphFactory::Create(eNodeType aNodeId, memory::Allocator& arAlloc)const + { + Node* node = 0; + switch(aNodeId) + { + REGISTERNODE(NegationFloat); + REGISTERNODE(NegationInt); + REGISTERNODE(NegationFloat4); + REGISTERNODE(NegationBool); + REGISTERNODE(AdditionFloat); + REGISTERNODE(AdditionUInt); + REGISTERNODE(AdditionInt); + REGISTERNODE(AdditionFloat4); + REGISTERNODE(SubstractionFloat); + REGISTERNODE(SubstractionUInt); + REGISTERNODE(SubstractionInt); + REGISTERNODE(SubstractionFloat4); + REGISTERNODE(MultiplicationFloat); + REGISTERNODE(MultiplicationUInt); + REGISTERNODE(MultiplicationInt); + REGISTERNODE(MultiplicationFloat4); + REGISTERNODE(DivisionFloat); + REGISTERNODE(DivisionUInt); + REGISTERNODE(DivisionInt); + REGISTERNODE(DivisionFloat4); + REGISTERNODE(CondFloat); + REGISTERNODE(CondUInt); + REGISTERNODE(CondInt); + REGISTERNODE(CondFloat4); + REGISTERNODE(AbsFloat); + REGISTERNODE(AbsFloat4); + REGISTERNODE(CrossFloat4); + REGISTERNODE(DegreesFloat); + REGISTERNODE(DegreesFloat4); + REGISTERNODE(DotFloat4); + REGISTERNODE(LengthFloat4); + REGISTERNODE(MaximumFloat); + REGISTERNODE(MaximumUInt); + REGISTERNODE(MaximumInt); + REGISTERNODE(MaximumFloat4); + REGISTERNODE(MinimumFloat); + REGISTERNODE(MinimumUInt); + REGISTERNODE(MinimumInt); + REGISTERNODE(MinimumFloat4); + REGISTERNODE(NormalizeFloat4); + REGISTERNODE(RadiansFloat); + REGISTERNODE(RadiansFloat4); + REGISTERNODE(FloatToFloat4); + REGISTERNODE(Float4ToFloat); + REGISTERNODE(GreaterThan); + REGISTERNODE(LesserThan); + REGISTERNODE(GreaterThanOrEqual); + REGISTERNODE(LesserThanOrEqual); + REGISTERNODE(SmoothStepFloat); + REGISTERNODE(Mux5Float); + REGISTERNODE(Mul5Float); + REGISTERNODE(Sin); + REGISTERNODE(Fmod); + REGISTERNODE(And); + REGISTERNODE(Or); + REGISTERNODE(xformMulInv); + REGISTERNODE(xformIdentity); + REGISTERNODE(xformMulVec); + REGISTERNODE(xformInvMulVec); + REGISTERNODE(xformMul); + REGISTERNODE(xformInvMul); + REGISTERNODE(xformEqual); + REGISTERNODE(xformWeight); + REGISTERNODE(xformAdd); + REGISTERNODE(xformSub); + REGISTERNODE(xformBlend); + REGISTERNODE(quatIdentity); + REGISTERNODE(quatConj); + REGISTERNODE(quatMul); + REGISTERNODE(quatMulVec); + REGISTERNODE(quatLerp); + REGISTERNODE(quatArcRotate); + REGISTERNODE(quatArcRotateX); + REGISTERNODE(quatXcos); + REGISTERNODE(quatYcos); + REGISTERNODE(quatZcos); + REGISTERNODE(quatEulerToQuat); + REGISTERNODE(quatQuatToEuler); + REGISTERNODE(quatProjOnYPlane); + REGISTERNODE(quat2Qtan); + REGISTERNODE(qtan2Quat); + REGISTERNODE(ZYRoll2Quat); + REGISTERNODE(quat2ZYRoll); + REGISTERNODE(RollZY2Quat); + REGISTERNODE(quat2RollZY); + REGISTERNODE(quatWeight); + REGISTERNODE(xformCompose); + REGISTERNODE(xformDecompose); + REGISTERNODE(CondXform); + REGISTERNODE(Rand); + REGISTERNODE(Damp); + REGISTERNODE(xformRefChange); + REGISTERNODE(Xor); + REGISTERNODE(SmoothPulseFloat); + } + return node; + } + + GraphPlug* GraphFactory::Create(ePlugType aPlugType, memory::Allocator& arAlloc)const + { + GraphPlug* plug = 0; + switch(aPlugType) + { + case Float4Id: + { + plug = arAlloc.Construct< TypePlug<math::float4> >(); + break; + } + case Float1Id: + case FloatId: + { + plug = arAlloc.Construct< TypePlug<float> >(); + break; + } + case UInt32Id: + { + plug = arAlloc.Construct< TypePlug<uint32_t> >(); + break; + } + case Int32Id: + { + plug = arAlloc.Construct< TypePlug<int32_t> >(); + break; + } + case BoolId: + { + plug = arAlloc.Construct< TypePlug<bool> >(); + break; + } + case Bool4Id: + { + plug = arAlloc.Construct< TypePlug<math::bool4> >(); + break; + } + case XformId: + { + plug = arAlloc.Construct< TypePlug<math::xform> >(); + break; + } + + } + return plug; + } +} + +} diff --git a/Runtime/mecanim/graph/factory.h b/Runtime/mecanim/graph/factory.h new file mode 100644 index 0000000..d76fcbf --- /dev/null +++ b/Runtime/mecanim/graph/factory.h @@ -0,0 +1,35 @@ +/* + Copyright (c) 7244339 Canada Inc. (Mecanim) + All Rights Reserved. +*/ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/object.h" + +#include "Runtime/mecanim/graph/plug.h" +#include "Runtime/mecanim/graph/node.h" + +namespace mecanim +{ + +namespace graph +{ + // This class can be subclassed if you need to create custom node and want to add them to the list of + // instanciable node + class GraphFactory + { + public: + virtual Node* Create(eNodeType aNodeId, memory::Allocator& arAlloc)const; + virtual GraphPlug* Create(ePlugType aPlugType, memory::Allocator& arAlloc)const; + }; + + template <typename TYPE> TYPE* Create(GraphFactory const& arFactory, memory::Allocator& arAlloc) + { + return static_cast<TYPE*>(arFactory.Create(TYPE::mId, arAlloc)); + } +} + +}
\ No newline at end of file diff --git a/Runtime/mecanim/graph/genericnode.h b/Runtime/mecanim/graph/genericnode.h new file mode 100644 index 0000000..2bc82e7 --- /dev/null +++ b/Runtime/mecanim/graph/genericnode.h @@ -0,0 +1,1093 @@ +/* + Copyright (c) 7244339 Canada Inc. (Mecanim) + All Rights Reserved. +*/ +#pragma once + +#include <cstdlib> + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/object.h" + +#include "Runtime/Math/Simd/math.h" + +#include "Runtime/mecanim/graph/plug.h" +#include "Runtime/mecanim/graph/node.h" + +#include "Runtime/mecanim/graph/unarynode.h" + +#include "Runtime/mecanim/generic/stringtable.h" + +namespace mecanim +{ + +namespace graph +{ + template <typename COND, typename TYPE, typename POLICIES> class CondNode : public Node + { + public: + TypePlug<COND> mCondition; + TypePlug<TYPE> mA; + TypePlug<TYPE> mB; + + TypePlug<TYPE> mResult; + + CondNode() + :mCondition(true,CRCKey(eCondition)), + mA(true, CRCKey(eA)), + mB(true,CRCKey(eB)), + mResult(false,CRCKey(eResult)) + { + mCondition.m_Owner = this; + mA.m_Owner = this; + mB.m_Owner = this; + mResult.m_Owner = this; + } + + virtual ~CondNode(){} + + virtual uint32_t GetPlugCount()const + { + return 4; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mCondition; + case 1: return mA; + case 2: return mB; + case 3: + default: return mResult; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mCondition; + case 1: return mA; + case 2: return mB; + case 3: + default: return mResult; + } + } + + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + TYPE a, b, result; + COND cond; + + mCondition.ReadData(&cond, arEvaluationInfo); + mA.ReadData(&a, arEvaluationInfo); + mB.ReadData(&b, arEvaluationInfo); + + result = POLICIES::template Operation<COND, TYPE>(cond, a, b); + + mResult.WriteData(&result, arEvaluationInfo); + } + }; + + class CondFloatOp + { + public: + template< typename COND, typename TYPE > static TYPE Operation( COND const& p, TYPE const& l, TYPE const& r){ return math::cond(p, l, r); } + }; + class CondUIntOp + { + public: + template< typename COND, typename TYPE > static TYPE Operation( COND const& p, TYPE const& l, TYPE const& r){ return math::cond(p, l, r); } + }; + class CondIntOp + { + public: + template< typename COND, typename TYPE > static TYPE Operation( COND const& p, TYPE const& l, TYPE const& r){ return math::cond(p, l, r); } + }; + class CondFloat4Op + { + public: + template< typename COND, typename TYPE > static math::float4 Operation( bool const& p, math::float4 const& l, math::float4 const& r){ return math::cond(math::bool1(p), l, r); } + }; + class CondXformOp + { + public: + template< typename COND, typename TYPE > static TYPE Operation( COND const& p, TYPE const& l, TYPE const& r){ return math::cond(p, l, r); } + }; + + class CondFloat : public CondNode<bool, float, CondFloatOp> + { + public: + static const eNodeType mId = CondFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class CondUInt : public CondNode<bool, uint32_t, CondUIntOp> + { + public: + static const eNodeType mId = CondUIntId; + virtual eNodeType NodeType(){return mId;} + }; + class CondInt : public CondNode<bool, int32_t, CondIntOp> + { + public: + static const eNodeType mId = CondIntId; + virtual eNodeType NodeType(){return mId;} + }; + class CondFloat4 : public CondNode<bool, math::float4, CondFloat4Op> + { + public: + static const eNodeType mId = CondFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + class CondXform : public CondNode<bool, math::xform, CondXformOp> + { + public: + static const eNodeType mId = CondXformId; + virtual eNodeType NodeType(){return mId;} + }; + + template< typename TYPE, typename RESULT > class AbsOp + { + public: + static RESULT Operation( TYPE const& l){ return math::abs(l); } + }; + + class AbsFloat : public UnaryNode<float, float, AbsOp<float, float> > + { + public: + static const eNodeType mId = AbsFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class AbsFloat4 : public UnaryNode<math::float4, math::float4, AbsOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = AbsFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + class CrossOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return math::cross(l, r); } + }; + + class CrossFloat4 : public BinaryNode<math::float4, math::float4, math::float4, CrossOp> + { + public: + static const eNodeType mId = CrossFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + template< typename TYPE, typename RESULT > class DegreesOp + { + public: + static RESULT Operation( TYPE const& l){ return math::degrees(l); } + }; + + class DegreesFloat : public UnaryNode<float, float, DegreesOp<float, float> > + { + public: + static const eNodeType mId = DegreesFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class DegreesFloat4 : public UnaryNode<math::float4, math::float4, DegreesOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = DegreesFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + class DotOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return float(math::dot(l, r)); } + }; + + class DotFloat4 : public BinaryNode<math::float4, math::float4, float, DotOp> + { + public: + static const eNodeType mId = DotFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + template< typename TYPE, typename RESULT > class LengthOp + { + public: + static RESULT Operation( TYPE const& l){ return float(math::length(l)); } + }; + + class LengthFloat4 : public UnaryNode<math::float4, float, LengthOp<math::float4, float> > + { + public: + static const eNodeType mId = LengthFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + class MaximumOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return math::maximum(l, r); } + }; + + class MaximumFloat : public BinaryNode<float, float, float, MaximumOp> + { + public: + static const eNodeType mId = MaximumFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class MaximumUInt : public BinaryNode<uint32_t, uint32_t, uint32_t, MaximumOp> + { + public: + static const eNodeType mId = MaximumUIntId; + virtual eNodeType NodeType(){return mId;} + }; + class MaximumInt : public BinaryNode<int32_t, int32_t, int32_t, MaximumOp> + { + public: + static const eNodeType mId = MaximumIntId; + virtual eNodeType NodeType(){return mId;} + }; + class MaximumFloat4 : public BinaryNode<math::float4, math::float4, math::float4, MaximumOp> + { + public: + static const eNodeType mId = MaximumFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + class MinimumOp + { + public: + template< typename TYPE1, typename TYPE2, typename RESULT > static RESULT Operation( TYPE1 const& l, TYPE2 const& r){ return math::minimum(l, r); } + }; + + class MinimumFloat : public BinaryNode<float, float, float, MinimumOp> + { + public: + static const eNodeType mId = MinimumFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class MinimumUInt : public BinaryNode<uint32_t, uint32_t, uint32_t, MinimumOp> + { + public: + static const eNodeType mId = MinimumUIntId; + virtual eNodeType NodeType(){return mId;} + }; + class MinimumInt : public BinaryNode<int32_t, int32_t, int32_t, MinimumOp> + { + public: + static const eNodeType mId = MinimumIntId; + virtual eNodeType NodeType(){return mId;} + }; + class MinimumFloat4 : public BinaryNode<math::float4, math::float4, math::float4, MinimumOp> + { + public: + static const eNodeType mId = MinimumFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + template< typename TYPE, typename RESULT > class NormalizeOp + { + public: + static RESULT Operation( TYPE const& l){ return math::normalize(l); } + }; + + class NormalizeFloat4 : public UnaryNode<math::float4, math::float4, NormalizeOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = NormalizeFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + template< typename TYPE, typename RESULT > class RadiansOp + { + public: + static RESULT Operation( TYPE const& l){ return math::radians(l); } + }; + + class RadiansFloat : public UnaryNode<float, float, RadiansOp<float, float> > + { + public: + static const eNodeType mId = RadiansFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class RadiansFloat4 : public UnaryNode<math::float4, math::float4, RadiansOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = RadiansFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + + class Float4ToFloat : public Node + { + public: + static const eNodeType mId = Float4ToFloatId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<math::float4> mA; + TypePlug<float> mX; + TypePlug<float> mY; + TypePlug<float> mZ; + TypePlug<float> mW; + + Float4ToFloat() + :mA(true, CRCKey(eA)), + mX(false, CRCKey(eX)), + mY(false, CRCKey(eY)), + mZ(false, CRCKey(eZ)), + mW(false, CRCKey(eW)) + { + mA.m_Owner = this; + mX.m_Owner = this; + mY.m_Owner = this; + mZ.m_Owner = this; + mW.m_Owner = this; + } + + virtual ~Float4ToFloat(){} + + virtual uint32_t GetPlugCount()const + { + return 5; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mA; + case 1: return mX; + case 2: return mY; + case 3: return mZ; + case 4: + default: return mW; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mA; + case 1: return mX; + case 2: return mY; + case 3: return mZ; + case 4: + default: return mW; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + math::float4 a; + + mA.ReadData(&a, arEvaluationInfo); + + float ATTRIBUTE_ALIGN(ALIGN4F) b[4]; + + math::store(a, b); + + mX.WriteData(&b[0], arEvaluationInfo); + mY.WriteData(&b[1], arEvaluationInfo); + mZ.WriteData(&b[2], arEvaluationInfo); + mW.WriteData(&b[3], arEvaluationInfo); + } + }; + + class FloatToFloat4 : public Node + { + public: + static const eNodeType mId = FloatToFloat4Id; + virtual eNodeType NodeType(){return mId;} + + TypePlug<float> mX; + TypePlug<float> mY; + TypePlug<float> mZ; + TypePlug<float> mW; + + TypePlug<math::float4> mResult; + + + FloatToFloat4() + :mX(true,CRCKey(eX)), + mY(true,CRCKey(eY)), + mZ(true,CRCKey(eZ)), + mW(true,CRCKey(eW)), + mResult(false,CRCKey(eResult)) + { + mX.m_Owner = this; + mY.m_Owner = this; + mZ.m_Owner = this; + mW.m_Owner = this; + mResult.m_Owner = this; + } + + virtual ~FloatToFloat4(){} + + virtual uint32_t GetPlugCount()const + { + return 5; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mResult; + case 1: return mX; + case 2: return mY; + case 3: return mZ; + case 4: + default: return mW; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mResult; + case 1: return mX; + case 2: return mY; + case 3: return mZ; + case 4: + default: return mW; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + float ATTRIBUTE_ALIGN(ALIGN4F) a[4]; + + mX.ReadData(&a[0], arEvaluationInfo); + mY.ReadData(&a[1], arEvaluationInfo); + mZ.ReadData(&a[2], arEvaluationInfo); + mW.ReadData(&a[3], arEvaluationInfo); + + math::float4 b = math::load(a); + mResult.WriteData(&b, arEvaluationInfo); + } + }; + + + class SmoothStepFloat : public Node + { + public: + static const eNodeType mId = SmoothstepFloatId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<float> mMin; + TypePlug<float> mMax; + TypePlug<float> mValue; + TypePlug<float> mResult; + + SmoothStepFloat() + :mMin(true, CRCKey(eMin)), + mMax(true,CRCKey(eMax)), + mValue(true,CRCKey(eValue)), + mResult(false,CRCKey(eResult)) + { + mMin.m_Owner = this; + mMax.m_Owner = this; + mValue.m_Owner = this; + mResult.m_Owner = this; + } + + virtual ~SmoothStepFloat(){} + + virtual uint32_t GetPlugCount()const + { + return 4; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mMin; + case 1: return mMax; + case 2: return mValue; + case 3: + default: return mResult; + + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mMin; + case 1: return mMax; + case 2: return mValue; + case 3: + default: return mResult; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + float min, max, value; + + mMin.ReadData(&min, arEvaluationInfo); + mMax.ReadData(&max, arEvaluationInfo); + mValue.ReadData(&value, arEvaluationInfo); + + float ret = math::smoothstep( min, max, value); + + mResult.WriteData(&ret, arEvaluationInfo); + } + }; + + class SmoothPulseFloat : public Node + { + public: + static const eNodeType mId = SmoothPulseFloatId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<float> mMinMin; + TypePlug<float> mMinMax; + TypePlug<float> mMaxMin; + TypePlug<float> mMaxMax; + TypePlug<float> mValue; + TypePlug<float> mResult; + + SmoothPulseFloat(): + mMinMin(true, CRCKey(eMinMin)), + mMinMax(true, CRCKey(eMinMax)), + mMaxMin(true, CRCKey(eMaxMin)), + mMaxMax(true, CRCKey(eMaxMax)), + mValue(true, CRCKey(eValue)), + mResult(false, CRCKey(eResult)) + { + mMinMin.m_Owner = this; + mMinMax.m_Owner = this; + mMaxMin.m_Owner = this; + mMaxMax.m_Owner = this; + mValue.m_Owner = this; + mResult.m_Owner = this; + } + + virtual ~SmoothPulseFloat(){} + + virtual uint32_t GetPlugCount()const + { + return 6; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mMinMin; + case 1: return mMinMax; + case 2: return mMaxMin; + case 3: return mMaxMax; + case 4: return mValue; + case 5: + default: return mResult; + + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mMinMin; + case 1: return mMinMax; + case 2: return mMaxMin; + case 3: return mMaxMax; + case 4: return mValue; + case 5: + default: return mResult; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + float minmin, minmax, maxmin, maxmax, value; + + mMinMin.ReadData(&minmin, arEvaluationInfo); + mMinMax.ReadData(&minmax, arEvaluationInfo); + mMaxMin.ReadData(&maxmin, arEvaluationInfo); + mMaxMax.ReadData(&maxmax, arEvaluationInfo); + mValue.ReadData(&value, arEvaluationInfo); + + float ret = math::smoothpulse( minmin, minmax, maxmin, maxmax, value); + + mResult.WriteData(&ret, arEvaluationInfo); + } + }; + + class Mux5Float : public Node + { + public: + static const eNodeType mId = Mux5FloatId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<float> mIn; + TypePlug<float> mRangeA; + TypePlug<float> mRangeB; + TypePlug<float> mRangeC; + TypePlug<float> mRangeD; + TypePlug<float> mRangeE; + TypePlug<float> mWeightA; + TypePlug<float> mWeightB; + TypePlug<float> mWeightC; + TypePlug<float> mWeightD; + TypePlug<float> mWeightE; + + Mux5Float() : mIn(true,CRCKey(eIn)), + mRangeA(true,CRCKey(eRangeA)), + mRangeB(true,CRCKey(eRangeB)), + mRangeC(true,CRCKey(eRangeC)), + mRangeD(true,CRCKey(eRangeD)), + mRangeE(true,CRCKey(eRangeE)), + mWeightA(false,CRCKey(eWeightA)), + mWeightB(false,CRCKey(eWeightB)), + mWeightC(false,CRCKey(eWeightC)), + mWeightD(false,CRCKey(eWeightD)), + mWeightE(false,CRCKey(eWeightE)) + { + mIn.m_Owner = this; + mRangeA.m_Owner = this; + mRangeB.m_Owner = this; + mRangeC.m_Owner = this; + mRangeD.m_Owner = this; + mRangeE.m_Owner = this; + mWeightA.m_Owner = this; + mWeightB.m_Owner = this; + mWeightC.m_Owner = this; + mWeightD.m_Owner = this; + mWeightE.m_Owner = this; + } + + virtual ~Mux5Float(){} + + virtual uint32_t GetPlugCount()const + { + return 11; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mIn; + case 1: return mRangeA; + case 2: return mRangeB; + case 3: return mRangeC; + case 4: return mRangeD; + case 5: return mRangeE; + case 6: return mWeightA; + case 7: return mWeightB; + case 8: return mWeightC; + case 9: return mWeightD; + case 10: + default: return mWeightE; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mIn; + case 1: return mRangeA; + case 2: return mRangeB; + case 3: return mRangeC; + case 4: return mRangeD; + case 5: return mRangeE; + case 6: return mWeightA; + case 7: return mWeightB; + case 8: return mWeightC; + case 9: return mWeightD; + case 10: + default: return mWeightE; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + float in; + float r[5]; + float w[5]; + + mIn.ReadData(&in,arEvaluationInfo); + mRangeA.ReadData(&r[0],arEvaluationInfo); + mRangeB.ReadData(&r[1],arEvaluationInfo); + mRangeC.ReadData(&r[2],arEvaluationInfo); + mRangeD.ReadData(&r[3],arEvaluationInfo); + mRangeE.ReadData(&r[4],arEvaluationInfo); + + w[0] = 1.0f; + w[1] = 0.0f; + w[2] = 0.0f; + w[3] = 0.0f; + w[4] = 0.0f; + + if(in > r[0]) + { + w[0] = 0.f; + for(uint32_t i = 0; i < 4; i++) + { + float d = r[i+1] - r[i]; + + if(d > 0.0f) + { + if(in >= r[i] && in <= r[i+1]) + { + w[i] = (d-(in-r[i]))/(r[i+1]-r[i]); + w[i+1] = 1.f-w[i]; + } + } + else + { + break; + } + } + } + + mWeightA.WriteData(&w[0],arEvaluationInfo); + mWeightB.WriteData(&w[1],arEvaluationInfo); + mWeightC.WriteData(&w[2],arEvaluationInfo); + mWeightD.WriteData(&w[3],arEvaluationInfo); + mWeightE.WriteData(&w[4],arEvaluationInfo); + } + }; + + class Mul5Float : public Node + { + public: + static const eNodeType mId = Mul5FloatId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<float> mIn; + TypePlug<float> mA; + TypePlug<float> mB; + TypePlug<float> mC; + TypePlug<float> mD; + TypePlug<float> mE; + TypePlug<float> mOutA; + TypePlug<float> mOutB; + TypePlug<float> mOutC; + TypePlug<float> mOutD; + TypePlug<float> mOutE; + + Mul5Float() : mIn(true,CRCKey(eIn)), + mA(true,CRCKey(eA)), + mB(true,CRCKey(eB)), + mC(true,CRCKey(eC)), + mD(true,CRCKey(eD)), + mE(true,CRCKey(eE)), + mOutA(false,CRCKey(eOutA)), + mOutB(false,CRCKey(eOutB)), + mOutC(false,CRCKey(eOutC)), + mOutD(false,CRCKey(eOutD)), + mOutE(false,CRCKey(eOutE)) + { + mIn.m_Owner = this; + mOutA.m_Owner = this; + mOutB.m_Owner = this; + mOutC.m_Owner = this; + mD.m_Owner = this; + mE.m_Owner = this; + mOutA.m_Owner = this; + mOutB.m_Owner = this; + mOutC.m_Owner = this; + mOutD.m_Owner = this; + mOutE.m_Owner = this; + } + + virtual ~Mul5Float(){} + + virtual uint32_t GetPlugCount()const + { + return 11; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mIn; + case 1: return mA; + case 2: return mB; + case 3: return mC; + case 4: return mD; + case 5: return mE; + case 6: return mOutA; + case 7: return mOutB; + case 8: return mOutC; + case 9: return mOutD; + case 10: + default: return mOutE; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mIn; + case 1: return mA; + case 2: return mB; + case 3: return mC; + case 4: return mD; + case 5: return mE; + case 6: return mOutA; + case 7: return mOutB; + case 8: return mOutC; + case 9: return mOutD; + case 10: + default: return mOutE; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + float in; + float i[5]; + float o[5]; + + mIn.ReadData(&in,arEvaluationInfo); + mA.ReadData(&i[0],arEvaluationInfo); + mB.ReadData(&i[1],arEvaluationInfo); + mC.ReadData(&i[2],arEvaluationInfo); + mD.ReadData(&i[3],arEvaluationInfo); + mE.ReadData(&i[4],arEvaluationInfo); + + for(uint32_t iter = 0; iter < 5; iter++) + { + o[iter] = i[iter] * in; + } + + mOutA.WriteData(&o[0],arEvaluationInfo); + mOutB.WriteData(&o[1],arEvaluationInfo); + mOutC.WriteData(&o[2],arEvaluationInfo); + mOutD.WriteData(&o[3],arEvaluationInfo); + mOutE.WriteData(&o[4],arEvaluationInfo); + } + }; + + class Fmod : public Node + { + public: + static const eNodeType mId = FmodId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<float> mNum; + TypePlug<float> mDen; + TypePlug<float> mRem; + + Fmod() : mNum(true,CRCKey(eNum)), mDen(true,CRCKey(eDen)), mRem(false, CRCKey(eRem)) + { + mNum.m_Owner = this; + mDen.m_Owner = this; + mRem.m_Owner = this; + } + + virtual ~Fmod(){} + + virtual uint32_t GetPlugCount()const + { + return 3; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mNum; + case 1: return mDen; + case 2: + default: return mRem; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mNum; + case 1: return mDen; + case 2: + default: return mRem; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + float n,d,r; + + mNum.ReadData(&n, arEvaluationInfo); + mDen.ReadData(&d, arEvaluationInfo); + + r = math::fmod(n,d); + + mRem.WriteData(&r,arEvaluationInfo); + } + }; + + class Sin : public Node + { + public: + static const eNodeType mId = SinId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<float> mIn; + TypePlug<float> mOut; + + Sin() : mIn(true,CRCKey(eIn)), mOut(false,CRCKey(eOut)) + { + mIn.m_Owner = this; + mOut.m_Owner = this; + } + + virtual ~Sin(){} + + virtual uint32_t GetPlugCount()const + { + return 2; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mIn; + case 1: + default: return mOut; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mIn; + case 1: + default: return mOut; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + float i,o; + + mIn.ReadData(&i, arEvaluationInfo); + + o = math::sin(i); + + mOut.WriteData(&o,arEvaluationInfo); + } + }; + + class Rand : public Node + { + public: + static const eNodeType mId = RandId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<float> mResult; + + Rand() : mResult(false,CRCKey(eResult)) + { + mResult.m_Owner = this; + } + + virtual ~Rand(){} + + virtual uint32_t GetPlugCount()const + { + return 1; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: + default: return mResult; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: + default: return mResult; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + float intpart = static_cast<float>(std::rand()); + float fractpart = (std::rand() % 100) / 100.f; + + float result = intpart+fractpart; + + mResult.WriteData(&result,arEvaluationInfo); + } + }; + + class Damp : public Node + { + public: + static const eNodeType mId = DampId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<float> mDampTime; + TypePlug<float> mValue; + TypePlug<float> mDeltaTime; + TypePlug<float> mPreviousValue; + TypePlug<float> mResult; + + Damp() + :mDampTime(true,CRCKey(eDampTime)), + mValue(true,CRCKey(eValue)), + mDeltaTime(true,CRCKey(eDeltaTime)), + mPreviousValue(true,CRCKey(ePreviousValue)), + mResult(false,CRCKey(eResult)) + { + mDampTime.m_Owner = this; + mValue.m_Owner = this; + mDeltaTime.m_Owner = this; + mPreviousValue.m_Owner = this; + mResult.m_Owner = this; + } + + virtual ~Damp(){} + + virtual uint32_t GetPlugCount()const + { + return 5; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mDampTime; + case 1: return mValue; + case 2: return mDeltaTime; + case 3: return mPreviousValue; + case 4: + default: return mResult; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mDampTime; + case 1: return mValue; + case 2: return mDeltaTime; + case 3: return mPreviousValue; + case 4: + default: return mResult; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + float dampTime,value,deltaTime,previousValue, result; + + mDampTime.ReadData(&dampTime, arEvaluationInfo); + mValue.ReadData(&value, arEvaluationInfo); + mDeltaTime.ReadData(&deltaTime, arEvaluationInfo); + mPreviousValue.ReadData(&previousValue, arEvaluationInfo); + + result = math::cond(dampTime > 0, previousValue + (value - previousValue) * deltaTime / (dampTime + deltaTime), value); + + mResult.WriteData(&result,arEvaluationInfo); + } + }; + +} + +} diff --git a/Runtime/mecanim/graph/graph.cpp b/Runtime/mecanim/graph/graph.cpp new file mode 100644 index 0000000..4edbec7 --- /dev/null +++ b/Runtime/mecanim/graph/graph.cpp @@ -0,0 +1,476 @@ +#include "UnityPrefix.h" +#include "Runtime/Math/Simd/math.h" +#include "Runtime/mecanim/graph/graph.h" +#include "Runtime/mecanim/graph/factory.h" + +namespace mecanim +{ + +namespace graph +{ + static void AssignPlugId(EvaluationGraph* apGraph) + { + // Assigning id to each graph plug + uint32_t i, id = 0; + for(i=0;i<apGraph->m_NodeCount;i++) + { + Node* node = apGraph->m_NodeArray[i]; + + apGraph->m_Graph.m_Vertices[i].m_Id = node->NodeType(); + apGraph->m_Graph.m_Vertices[i].m_Binding = node->mID; + uint32_t j; + for(j=0;j<node->GetPlugCount();j++) + { + node->GetPlug(j).m_PlugId = id++; + } + } + + for(i=0;i<apGraph->m_Input->GetPlugCount();i++) + { + apGraph->m_Input->GetPlug(i).m_PlugId = id++; + } + + for(i=0;i<apGraph->m_Output->GetPlugCount();i++) + { + apGraph->m_Output->GetPlug(i).m_PlugId = id++; + } + } + + static void AssignPlugOffset(EvaluationGraph* apGraph, memory::Allocator& arAlloc) + { + // assigning value offset for each plug + uint32_t i, offset = 0; + for(i=0;i<apGraph->m_NodeCount;i++) + { + Node* node = apGraph->m_NodeArray[i]; + uint32_t j; + for(j=0;j<node->GetPlugCount();j++) + { + GraphPlug& plug = node->GetPlug(j); + offset = plug.m_Offset = arAlloc.AlignAddress(offset, plug.ValueAlign()); + offset += plug.ValueSize(); + } + } + + for(i=0;i<apGraph->m_Input->GetPlugCount();i++) + { + GraphPlug& plug = apGraph->m_Input->GetPlug(i); + offset = plug.m_Offset = arAlloc.AlignAddress(offset, plug.ValueAlign()); + offset += plug.ValueSize(); + } + + for(i=0;i<apGraph->m_Output->GetPlugCount();i++) + { + GraphPlug& plug = apGraph->m_Output->GetPlug(i); + offset = plug.m_Offset = arAlloc.AlignAddress(offset, plug.ValueAlign()); + offset += plug.ValueSize(); + } + } + + static void ConnectEdges(EvaluationGraph* apGraph) + { + uint32_t i, edgesIt = 0; + for(i=0;i<apGraph->m_NodeCount;i++) + { + Node* node = apGraph->m_NodeArray[i]; + uint32_t j; + for(j=0;j<node->GetPlugCount();j++) + { + GraphPlug* plug = node->GetPlug(j).GetSource(); + if(plug) + { + apGraph->m_Graph.m_Edges[edgesIt].m_SourceId = plug->m_PlugId; + apGraph->m_Graph.m_Edges[edgesIt].m_DestinationId = node->GetPlug(j).m_PlugId; + edgesIt++; + } + } + } + + for(i=0;i<apGraph->m_Output->GetPlugCount();i++) + { + GraphPlug* plug = apGraph->m_Output->mPlugArray[i]->GetSource(); + if(plug) + { + apGraph->m_Graph.m_Edges[edgesIt].m_SourceId = plug->m_PlugId; + apGraph->m_Graph.m_Edges[edgesIt].m_DestinationId = apGraph->m_Output->mPlugArray[i]->m_PlugId; + edgesIt++; + } + } + } + + static EvaluationGraph* CreateGraph(Node** apNodeArray, uint32_t aNodeCount, + GraphPlug** apInputPlugArray, uint32_t aInputPlugCount, + GraphPlug** apOutputPlugArray, uint32_t aOutputPlugCount, + uint32_t aConnectionCount, + Constant** apConstantArray, uint32_t aConstantCount, + memory::Allocator& arAlloc) + { + EvaluationGraph* cst = arAlloc.Construct<EvaluationGraph>(); + + cst->m_NodeCount = aNodeCount; + cst->m_NodeArray = arAlloc.ConstructArray<Node*>(aNodeCount); + + cst->m_Graph.m_VerticesCount = aNodeCount; + cst->m_Graph.m_Vertices = arAlloc.ConstructArray<Vertex>(aNodeCount); + + cst->m_Graph.m_EdgesCount = aConnectionCount; + cst->m_Graph.m_Edges = arAlloc.ConstructArray<Edge>(aConnectionCount); + + cst->m_Graph.m_InEdgesCount = aInputPlugCount; + cst->m_Graph.m_InEdges = arAlloc.ConstructArray<ExternalEdge>(aInputPlugCount); + + cst->m_Graph.m_OutEdgesCount = aOutputPlugCount; + cst->m_Graph.m_OutEdges = arAlloc.ConstructArray<ExternalEdge>(aOutputPlugCount); + + cst->m_Graph.m_ConstantCount = aConstantCount; + cst->m_Graph.m_ConstantEdges = arAlloc.ConstructArray<ConstantEdge>(cst->m_Graph.m_ConstantCount); + + cst->m_Input = arAlloc.Construct<GraphInput>(); + + cst->m_Input->mPlugArray = arAlloc.ConstructArray<GraphPlug*>(aInputPlugCount); + + cst->m_Output = arAlloc.Construct<GraphOutput>(); + + cst->m_Output->mPlugArray = arAlloc.ConstructArray<GraphPlug*>(aOutputPlugCount); + + cst->m_ConstantCount = aConstantCount; + cst->m_ConstantArray = arAlloc.ConstructArray<Constant*>(aConstantCount); + + memcpy(&cst->m_NodeArray[0], &apNodeArray[0], sizeof(Node*)*aNodeCount); + + memcpy(&cst->m_ConstantArray[0], &apConstantArray[0], sizeof(Constant*)*aConstantCount); + + cst->m_Input->mPlugCount = aInputPlugCount; + memcpy(&cst->m_Input->mPlugArray[0], &apInputPlugArray[0], sizeof(GraphPlug*)*aInputPlugCount); + uint32_t i; + for(i=0;i<aInputPlugCount;i++) + { + cst->m_Input->mPlugArray[i]->m_Owner = cst->m_Input; + cst->m_Graph.m_InEdges[i].m_Id = GetPlugType(*apInputPlugArray[i]); + cst->m_Graph.m_InEdges[i].m_Binding = apInputPlugArray[i]->m_ID; + } + + cst->m_Output->mPlugCount = aOutputPlugCount; + memcpy(&cst->m_Output->mPlugArray[0], &apOutputPlugArray[0], sizeof(GraphPlug*)*aOutputPlugCount); + for(i=0;i<aOutputPlugCount;i++) + { + cst->m_Output->mPlugArray[i]->m_Owner = cst->m_Output; + cst->m_Graph.m_OutEdges[i].m_Id = GetPlugType(*apOutputPlugArray[i]); + cst->m_Graph.m_OutEdges[i].m_Binding = apOutputPlugArray[i]->m_ID; + } + + AssignPlugId(cst); + AssignPlugOffset(cst, arAlloc); + + for(i=0;i<aConstantCount;i++) + { + // copy the largest data element + cst->m_Graph.m_ConstantEdges[i].m_FloatValue = cst->m_ConstantArray[i]->m_FloatValue; + cst->m_Graph.m_ConstantEdges[i].m_VectorValue = cst->m_ConstantArray[i]->m_VectorValue; + if(cst->m_ConstantArray[i]->m_Plug) + { + cst->m_Graph.m_ConstantEdges[i].m_Id = cst->m_ConstantArray[i]->m_Plug->m_PlugId; + } + } + + return cst; + } + + EvaluationGraph* CreateEvaluationGraph(Node** apNodeArray, uint32_t aNodeCount, + GraphPlug** apInputPlugArray, uint32_t aInputPlugCount, + GraphPlug** apOutputPlugArray, uint32_t aOutputPlugCount, + Constant** apConstantArray, uint32_t aConstantCount, + memory::Allocator& arAlloc) + { + uint32_t connectionCount = 0; + uint32_t i; + for(i=0;i<aNodeCount;i++) + { + Node* node = apNodeArray[i]; + uint32_t j; + for(j=0;j<node->GetPlugCount();j++) + { + GraphPlug* plug = node->GetPlug(j).GetSource(); + if(plug) + { + connectionCount++; + } + } + } + + for(i=0;i<aOutputPlugCount;i++) + { + GraphPlug* plug = apOutputPlugArray[i]->GetSource(); + if(plug) + { + connectionCount++; + } + } + + EvaluationGraph* cst = CreateGraph(apNodeArray, aNodeCount, apInputPlugArray, aInputPlugCount, apOutputPlugArray, aOutputPlugCount, connectionCount, apConstantArray, aConstantCount, arAlloc); + + ConnectEdges(cst); + + return cst; + } + + static void Connect(Edge const& arEdges, EvaluationGraph& arGraph) + { + GraphPlug *source = 0, *destination = 0; + + uint32_t i, j; + for(i=0;i < arGraph.m_NodeCount && (source == 0 || destination == 0);i++) + { + Node* node = arGraph.m_NodeArray[i]; + for(j=0;j<node->GetPlugCount() && (source == 0 || destination == 0);j++) + { + if(node->GetPlug(j).m_PlugId == arEdges.m_SourceId) + source = &node->GetPlug(j); + if(node->GetPlug(j).m_PlugId == arEdges.m_DestinationId) + destination = &node->GetPlug(j); + } + } + + for(j=0;j<arGraph.m_Input->GetPlugCount() && (source == 0 || destination == 0);j++) + { + if(arGraph.m_Input->GetPlug(j).m_PlugId == arEdges.m_SourceId) + source = &arGraph.m_Input->GetPlug(j); + if(arGraph.m_Input->GetPlug(j).m_PlugId == arEdges.m_DestinationId) + destination = &arGraph.m_Input->GetPlug(j); + } + + + for(j=0;j<arGraph.m_Output->GetPlugCount() && (source == 0 || destination == 0);j++) + { + if(arGraph.m_Output->GetPlug(j).m_PlugId == arEdges.m_SourceId) + source = &arGraph.m_Output->GetPlug(j); + if(arGraph.m_Output->GetPlug(j).m_PlugId == arEdges.m_DestinationId) + destination = &arGraph.m_Output->GetPlug(j); + } + + if(source && destination) + { + destination->m_Source = source; + } + } + + static void Connect(ConstantEdge const& arConstantEdge, Constant& arConstant, EvaluationGraph& arGraph) + { + GraphPlug *plug = 0; + + uint32_t i, j; + for(i=0;i < arGraph.m_NodeCount && plug == 0; i++) + { + Node* node = arGraph.m_NodeArray[i]; + for(j=0;j<node->GetPlugCount() && plug == 0; j++) + { + if(node->GetPlug(j).m_PlugId == arConstantEdge.m_Id) + plug = &node->GetPlug(j); + } + } + + for(j=0;j<arGraph.m_Input->GetPlugCount() && plug == 0; j++) + { + if(arGraph.m_Input->GetPlug(j).m_PlugId == arConstantEdge.m_Id) + plug = &arGraph.m_Input->GetPlug(j); + } + + + for(j=0;j<arGraph.m_Output->GetPlugCount() && plug == 0; j++) + { + if(arGraph.m_Output->GetPlug(j).m_PlugId == arConstantEdge.m_Id) + plug = &arGraph.m_Output->GetPlug(j); + } + + if(plug) + { + arConstant.m_Plug = plug; + } + } + + EvaluationGraph* CreateEvaluationGraph(Graph* apGraph, GraphFactory const& arFactory, memory::Allocator& arAlloc) + { + EvaluationGraph* cst = 0; + uint32_t i; + Node** nodes = arAlloc.ConstructArray<Node*>(apGraph->m_VerticesCount); + GraphPlug** inputPlug = arAlloc.ConstructArray<GraphPlug*>(apGraph->m_InEdgesCount); + GraphPlug** outputPlug = arAlloc.ConstructArray<GraphPlug*>(apGraph->m_OutEdgesCount); + Constant** constants = arAlloc.ConstructArray<Constant*>(apGraph->m_ConstantCount); + + for(i=0;i<apGraph->m_VerticesCount;i++) + { + nodes[i] = arFactory.Create(static_cast<eNodeType>(apGraph->m_Vertices[i].m_Id), arAlloc); + nodes[i]->mID = apGraph->m_Vertices[i].m_Binding; + } + + for(i=0;i<apGraph->m_InEdgesCount;i++) + { + inputPlug[i] = arFactory.Create(static_cast<ePlugType>(apGraph->m_InEdges[i].m_Id), arAlloc); + inputPlug[i]->m_ID = apGraph->m_InEdges[i].m_Binding; + inputPlug[i]->m_Input = false; + } + + for(i=0;i<apGraph->m_OutEdgesCount;i++) + { + outputPlug[i] = arFactory.Create(static_cast<ePlugType>(apGraph->m_OutEdges[i].m_Id), arAlloc); + outputPlug[i]->m_ID = apGraph->m_OutEdges[i].m_Binding; + outputPlug[i]->m_Input = true; + } + + for(i=0;i<apGraph->m_ConstantCount;i++) + { + constants[i] = arAlloc.Construct<Constant>(); + + constants[i]->m_FloatValue = apGraph->m_ConstantEdges[i].m_FloatValue; + constants[i]->m_VectorValue = apGraph->m_ConstantEdges[i].m_VectorValue; + } + + cst = CreateGraph(nodes, apGraph->m_VerticesCount, inputPlug, apGraph->m_InEdgesCount, outputPlug, apGraph->m_OutEdgesCount, apGraph->m_EdgesCount, constants, apGraph->m_ConstantCount, arAlloc); + + for(i=0;i<apGraph->m_EdgesCount;i++) + { + cst->m_Graph.m_Edges[i] = apGraph->m_Edges[i]; + Connect(apGraph->m_Edges[i], *cst); + } + + for(i=0;i<apGraph->m_ConstantCount;i++) + { + cst->m_Graph.m_ConstantEdges[i].m_Id = apGraph->m_ConstantEdges[i].m_Id; + Connect(apGraph->m_ConstantEdges[i], *cst->m_ConstantArray[i], *cst); + } + + arAlloc.Deallocate(nodes); + arAlloc.Deallocate(inputPlug); + arAlloc.Deallocate(outputPlug); + arAlloc.Deallocate(constants); + + return cst; + } + + void DestroyEvaluationGraph(EvaluationGraph* apGraph, memory::Allocator& arAlloc) + { + if(apGraph) + { + uint32_t i; + for(i=0;i<apGraph->m_NodeCount;i++) + { + arAlloc.Deallocate(apGraph->m_NodeArray[i]); + } + arAlloc.Deallocate(apGraph->m_NodeArray); + + for(i=0;i<apGraph->m_Graph.m_InEdgesCount;i++) + { + arAlloc.Deallocate(apGraph->m_Input->mPlugArray[i]); + } + arAlloc.Deallocate(apGraph->m_Input->mPlugArray); + arAlloc.Deallocate(apGraph->m_Input); + + for(i=0;i<apGraph->m_Graph.m_OutEdgesCount;i++) + { + arAlloc.Deallocate(apGraph->m_Output->mPlugArray[i]); + } + arAlloc.Deallocate(apGraph->m_Output->mPlugArray); + arAlloc.Deallocate(apGraph->m_Output); + + for(i=0;i<apGraph->m_ConstantCount;i++) + { + arAlloc.Deallocate(apGraph->m_ConstantArray[i]); + } + arAlloc.Deallocate(apGraph->m_ConstantArray); + + arAlloc.Deallocate(apGraph->m_Graph.m_Vertices); + arAlloc.Deallocate(apGraph->m_Graph.m_Edges); + arAlloc.Deallocate(apGraph->m_Graph.m_InEdges); + arAlloc.Deallocate(apGraph->m_Graph.m_OutEdges); + arAlloc.Deallocate(apGraph->m_Graph.m_ConstantEdges); + + arAlloc.Deallocate(apGraph); + } + } + + EvaluationGraphWorkspace* CreateEvaluationGraphWorkspace(EvaluationGraph* apGraph, memory::Allocator& arAlloc) + { + std::size_t sizePlugDataBlock = 0; + uint32_t i, plugCount = 0; + + for(i=0;i<apGraph->m_NodeCount;i++) + { + Node* node = apGraph->m_NodeArray[i]; + uint32_t j; + for(j=0;j<node->GetPlugCount();j++) + { + plugCount++; + sizePlugDataBlock = arAlloc.AlignAddress(sizePlugDataBlock, node->GetPlug(j).ValueAlign()); + sizePlugDataBlock += node->GetPlug(j).ValueSize(); + } + } + + for(i=0;i<apGraph->m_Input->GetPlugCount();i++) + { + plugCount++; + sizePlugDataBlock = arAlloc.AlignAddress(sizePlugDataBlock, apGraph->m_Input->GetPlug(i).ValueAlign()); + sizePlugDataBlock += apGraph->m_Input->GetPlug(i).ValueSize(); + } + + for(i=0;i<apGraph->m_Output->GetPlugCount();i++) + { + plugCount++; + sizePlugDataBlock = arAlloc.AlignAddress(sizePlugDataBlock, apGraph->m_Output->GetPlug(i).ValueAlign()); + sizePlugDataBlock += apGraph->m_Output->GetPlug(i).ValueSize(); + } + + EvaluationGraphWorkspace* ws = arAlloc.Construct<EvaluationGraphWorkspace>(); + + ws->m_EvaluationInfo.m_DataBlock.m_PlugCount = plugCount; + ws->m_EvaluationInfo.m_DataBlock.m_EvaluationId = arAlloc.ConstructArray<uint32_t>(plugCount); + + ws->m_EvaluationInfo.m_DataBlock.m_Buffer = reinterpret_cast<char*>(arAlloc.Allocate( sizePlugDataBlock, ALIGN4F) ); + ws->m_EvaluationInfo.m_EvaluationId = 0; + + memset(ws->m_EvaluationInfo.m_DataBlock.m_EvaluationId, numeric_limits<uint32_t>::max_value, sizeof(mecanim::uint32_t)*plugCount); + memset(ws->m_EvaluationInfo.m_DataBlock.m_Buffer, 0, sizePlugDataBlock); + + for(i=0;i<apGraph->m_ConstantCount;i++) + { + if(apGraph->m_ConstantArray[i]->m_Plug) + { + ePlugType plugType = GetPlugType(*apGraph->m_ConstantArray[i]->m_Plug); + switch(plugType) + { + case Float4Id: + ws->m_EvaluationInfo.m_DataBlock.SetData(apGraph->m_ConstantArray[i]->m_VectorValue, apGraph->m_ConstantArray[i]->m_Plug->m_PlugId, apGraph->m_ConstantArray[i]->m_Plug->m_Offset, 0); + break; + case Float1Id: + ws->m_EvaluationInfo.m_DataBlock.SetData(apGraph->m_ConstantArray[i]->m_VectorValue, apGraph->m_ConstantArray[i]->m_Plug->m_PlugId, apGraph->m_ConstantArray[i]->m_Plug->m_Offset, 0); + break; + case FloatId: + ws->m_EvaluationInfo.m_DataBlock.SetData(apGraph->m_ConstantArray[i]->m_FloatValue, apGraph->m_ConstantArray[i]->m_Plug->m_PlugId, apGraph->m_ConstantArray[i]->m_Plug->m_Offset, 0); + break; + case UInt32Id: + ws->m_EvaluationInfo.m_DataBlock.SetData(apGraph->m_ConstantArray[i]->m_UIntValue, apGraph->m_ConstantArray[i]->m_Plug->m_PlugId, apGraph->m_ConstantArray[i]->m_Plug->m_Offset, 0); + break; + case Int32Id: + ws->m_EvaluationInfo.m_DataBlock.SetData(apGraph->m_ConstantArray[i]->m_IntValue, apGraph->m_ConstantArray[i]->m_Plug->m_PlugId, apGraph->m_ConstantArray[i]->m_Plug->m_Offset, 0); + break; + case BoolId: + ws->m_EvaluationInfo.m_DataBlock.SetData(apGraph->m_ConstantArray[i]->m_BoolValue, apGraph->m_ConstantArray[i]->m_Plug->m_PlugId, apGraph->m_ConstantArray[i]->m_Plug->m_Offset, 0); + break; + } + } + } + + return ws; + } + + void DestroyEvaluationGraphWorkspace(EvaluationGraphWorkspace* apGraphWorkspace, memory::Allocator& arAlloc) + { + if(apGraphWorkspace) + { + arAlloc.Deallocate(apGraphWorkspace->m_EvaluationInfo.m_DataBlock.m_Buffer); + arAlloc.Deallocate(apGraphWorkspace->m_EvaluationInfo.m_DataBlock.m_EvaluationId); + arAlloc.Deallocate(apGraphWorkspace); + } + } +} + +}
\ No newline at end of file diff --git a/Runtime/mecanim/graph/graph.h b/Runtime/mecanim/graph/graph.h new file mode 100644 index 0000000..2c12b4f --- /dev/null +++ b/Runtime/mecanim/graph/graph.h @@ -0,0 +1,188 @@ +/* + Copyright (c) 7244339 Canada Inc. (Mecanim) + All Rights Reserved. +*/ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/object.h" +#include "Runtime/mecanim/bitset.h" + +#include "Runtime/mecanim/graph/plug.h" +#include "Runtime/mecanim/graph/node.h" +#include "Runtime/mecanim/graph/factory.h" + +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Animation/MecanimArraySerialization.h" + +namespace mecanim +{ + +namespace graph +{ + // If somebody change a node interface, like adding a new plug, he should create a new class instead for his new node + // because some user's file may still use the old node and won't have the data to handle new plug, unexpected behavior + // can result if this rule is not respected + + struct Vertex : public Object + { + DEFINE_GET_TYPESTRING(Vertex) + + DeclareObject(Vertex) + + Vertex():m_Id(numeric_limits<uint32_t>::max_value){} + uint32_t m_Id; + uint32_t m_Binding; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_Id); + TRANSFER(m_Binding); + } + }; + + struct Edge : public Object + { + DEFINE_GET_TYPESTRING(Edge) + + DeclareObject(Edge) + + Edge():m_SourceId(numeric_limits<uint32_t>::max_value),m_DestinationId(numeric_limits<uint32_t>::max_value){} + uint32_t m_SourceId; + uint32_t m_DestinationId; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_SourceId); + TRANSFER(m_DestinationId); + } + }; + + struct ConstantEdge : public Object + { + DEFINE_GET_TYPESTRING(ConstantEdge) + + DeclareObject(ConstantEdge) + + ConstantEdge():m_Id(numeric_limits<uint32_t>::max_value){} + union { + float m_FloatValue; + uint32_t m_UIntValue; + int32_t m_IntValue; + bool m_BoolValue; + }; + math::float4 m_VectorValue; + + uint32_t m_Id; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_Id); + TRANSFER(m_VectorValue); + TRANSFER(m_UIntValue); + } + }; + + struct ExternalEdge : public Object + { + DEFINE_GET_TYPESTRING(ExternalEdge) + + DeclareObject(ExternalEdge) + + ExternalEdge():m_Id(numeric_limits<uint32_t>::max_value), m_Binding(numeric_limits<uint32_t>::max_value){} + uint32_t m_Id; + uint32_t m_Binding; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_Id); + TRANSFER(m_Binding); + } + }; + + struct Graph : public Object + { + DEFINE_GET_TYPESTRING(Graph) + + DeclareObject(Graph) + + Graph() + :m_VerticesCount(0),m_Vertices(0), + m_EdgesCount(0),m_Edges(0), + m_InEdgesCount(0),m_InEdges(0), + m_OutEdgesCount(0),m_OutEdges(0), + m_ConstantCount(0),m_ConstantEdges(0) + {} + + uint32_t m_VerticesCount; + Vertex* m_Vertices; + + uint32_t m_EdgesCount; + Edge* m_Edges; + + uint32_t m_InEdgesCount; + ExternalEdge* m_InEdges; + + uint32_t m_OutEdgesCount; + ExternalEdge* m_OutEdges; + + uint32_t m_ConstantCount; + ConstantEdge* m_ConstantEdges; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + MANUAL_ARRAY_TRANSFER(mecanim::graph::Vertex, m_Vertices, m_VerticesCount); + MANUAL_ARRAY_TRANSFER(mecanim::graph::Edge, m_Edges, m_EdgesCount); + MANUAL_ARRAY_TRANSFER(mecanim::graph::ExternalEdge, m_InEdges, m_InEdgesCount); + MANUAL_ARRAY_TRANSFER(mecanim::graph::ExternalEdge, m_OutEdges, m_OutEdgesCount); + MANUAL_ARRAY_TRANSFER(mecanim::graph::ConstantEdge, m_ConstantEdges, m_ConstantCount); + } + }; + + struct EvaluationGraph + { + public: + EvaluationGraph():m_NodeCount(0),m_NodeArray(0),m_Input(0),m_Output(0),m_ConstantCount(0),m_ConstantArray(0){} + + Graph m_Graph; + + uint32_t m_NodeCount; + Node** m_NodeArray; + + GraphInput* m_Input; + GraphOutput* m_Output; + + uint32_t m_ConstantCount; + Constant** m_ConstantArray; + }; + + struct EvaluationGraphWorkspace + { + public: + EvaluationGraphWorkspace(){} + + EvaluationInfo m_EvaluationInfo; + }; + + EvaluationGraph* CreateEvaluationGraph(Node** apNodeArray, uint32_t aNodeCount, + GraphPlug** apInputPlugArray, uint32_t aInputPlugCount, + GraphPlug** apOutputPlugArray, uint32_t aOutputPlugCount, + Constant** apConstantArray, uint32_t aConstantCount, + memory::Allocator& arAlloc); + + EvaluationGraph* CreateEvaluationGraph(Graph* apGraph, GraphFactory const& arFactory, memory::Allocator& arAlloc); + + void DestroyEvaluationGraph(EvaluationGraph* apGraph, memory::Allocator& arAlloc); + + EvaluationGraphWorkspace* CreateEvaluationGraphWorkspace(EvaluationGraph* apGraph, memory::Allocator& arAlloc); + void DestroyEvaluationGraphWorkspace(EvaluationGraphWorkspace* apGraphWorkspace, memory::Allocator& arAlloc); +} + +} diff --git a/Runtime/mecanim/graph/node.cpp b/Runtime/mecanim/graph/node.cpp new file mode 100644 index 0000000..8e1ead8 --- /dev/null +++ b/Runtime/mecanim/graph/node.cpp @@ -0,0 +1,11 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/graph/node.h" + +namespace mecanim +{ + +namespace graph +{ +} + +} diff --git a/Runtime/mecanim/graph/node.h b/Runtime/mecanim/graph/node.h new file mode 100644 index 0000000..7dcea89 --- /dev/null +++ b/Runtime/mecanim/graph/node.h @@ -0,0 +1,346 @@ +/* + Copyright (c) 7244339 Canada Inc. (Mecanim) + All Rights Reserved. +*/ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/object.h" +#include "Runtime/mecanim/generic/stringtable.h" + +#include "Runtime/mecanim/graph/plug.h" + +namespace mecanim +{ + +namespace graph +{ + // If you add a new node id, add it at the end of this list, otherwise you will break + // saved graph asset. + enum eNodeType { + InvalidNodeId, + NegationFloatId, + NegationIntId, + NegationFloat4Id, + AdditionFloatId, + AdditionUIntId, + AdditionIntId, + AdditionFloat4Id, + SubstractionFloatId, + SubstractionUIntId, + SubstractionIntId, + SubstractionFloat4Id, + MultiplicationFloatId, + MultiplicationUIntId, + MultiplicationIntId, + MultiplicationFloat4Id, + DivisionFloatId, + DivisionUIntId, + DivisionIntId, + DivisionFloat4Id, + CondFloatId, + CondUIntId, + CondIntId, + CondFloat4Id, + AbsFloatId, + AbsFloat4Id, + CrossFloat4Id, + DegreesFloatId, + DegreesFloat4Id, + DotFloat4Id, + LengthFloat4Id, + MaximumFloatId, + MaximumUIntId, + MaximumIntId, + MaximumFloat4Id, + MinimumFloatId, + MinimumUIntId, + MinimumIntId, + MinimumFloat4Id, + NormalizeFloat4Id, + RadiansFloatId, + RadiansFloat4Id, + FloatToFloat4Id, + Float4ToFloatId, + GreaterThanId, + LesserThanId, + SmoothstepFloatId, + Mux5FloatId, + Mul5FloatId, + FmodId, + SinId, + AndId, + OrId, + NegationBoolId, + GreaterThanOrEqualId, + LesserThanOrEqualId, + xformMulInvId, + xformIdentityId, + xformMulVecId, + xformInvMulVecId, + xformMulId, + xformInvMulId, + xformEqualId, + xformWeightId, + xformAddId, + xformSubId, + quatIdentityId, + quatConjId, + quatMulId, + quatMulVecId, + quatLerpId, + quatArcRotateId, + quatArcRotateXId, + quatXcosId, + quatYcosId, + quatZcosId, + quatEulerToQuatId, + quatQuatToEulerId, + quatProjOnYPlaneId, + quat2QtanId, + qtan2QuatId, + ZYRoll2QuatId, + quat2ZYRollId, + RollZY2QuatId, + quat2RollZYId, + quatWeightId, + xformComposeId, + xformDecomposeId, + CondXformId, + xformBlendId, + FloatToFloat1Id, // Not used anymore + Float1ToFloatId, // Not used anymore + RandId, + DampId, + xformRefChangeId, + XorId, + SmoothPulseFloatId, + InputId, + OutputId, + LastNodeId + }; + + class Node : public Object + { + public: + uint32_t mID; + + Node(){} + virtual ~Node(){} + + virtual eNodeType NodeType()=0; + virtual void Evaluate(EvaluationInfo& arEvaluationInfo)=0; + + virtual uint32_t GetPlugCount()const=0; + virtual GraphPlug& GetPlug(uint32_t aIndex)=0; + virtual GraphPlug const& GetPlug(uint32_t aIndex)const=0; + }; + + template<typename TYPE> class TypePlug : public GraphPlug + { + public: + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef TYPE* pointer; + typedef TYPE const* const_pointer; + + TypePlug(){} + TypePlug(bool input, uint32_t id, Node* apOwner=0):GraphPlug(input, id, apOwner){} + + virtual std::size_t ValueSize()const + { + return sizeof(value_type); + } + + virtual std::size_t ValueAlign()const + { + return ALIGN_OF(value_type); + } + + virtual ePlugType PlugType()const + { + return Trait<value_type>::PlugType(); + } + + void ConnectTo(TypePlug& arPlug) + { + m_Source = &arPlug; + } + + bool ReadData(void* apData, EvaluationInfo& arEvaluationInfo)const + { + uint32_t offset = m_Offset; + if(m_Source) + { + offset = m_Source->m_Offset; + if(arEvaluationInfo.m_DataBlock.IsDirty(m_Source->m_PlugId, arEvaluationInfo.m_EvaluationId)) + { + m_Source->m_Owner->Evaluate(arEvaluationInfo); + } + } + + *reinterpret_cast<pointer>(apData) = arEvaluationInfo.m_DataBlock.GetData<value_type>(offset); + + return true; + } + + bool WriteData(void const* apData, EvaluationInfo& arEvaluationInfo)const + { + arEvaluationInfo.m_DataBlock.SetData(*reinterpret_cast<const_pointer>(apData), m_PlugId, m_Offset, arEvaluationInfo.m_EvaluationId); + return true; + } + }; + + + class GraphOutput : public Node + { + public: + uint32_t mPlugCount; + GraphPlug** mPlugArray; + + GraphOutput():mPlugCount(0), mPlugArray(0){} + virtual ~GraphOutput(){} + + virtual eNodeType NodeType(){return OutputId;} + + virtual uint32_t GetPlugCount()const{return mPlugCount;} + virtual GraphPlug& GetPlug(uint32_t aIndex){return *(mPlugArray[aIndex]);} + virtual GraphPlug const& GetPlug(uint32_t aIndex)const{return *(mPlugArray[aIndex]);} + + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo){}; + }; + + class GraphInput : public Node + { + public: + uint32_t mPlugCount; + GraphPlug** mPlugArray; + + GraphInput():mPlugCount(0), mPlugArray(0){} + virtual ~GraphInput(){} + + virtual eNodeType NodeType(){return InputId;} + + virtual uint32_t GetPlugCount()const{return mPlugCount;} + virtual GraphPlug& GetPlug(uint32_t aIndex){return *(mPlugArray[aIndex]);} + virtual GraphPlug const& GetPlug(uint32_t aIndex)const{return *(mPlugArray[aIndex]);} + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo){}; + }; + + template <typename RESULT, typename ResultPolicies> class ResultNode : public Node + { + public: + TypePlug<RESULT> mResult; + + ResultNode() + :mResult(false,CRCKey(eResult)) + { + mResult.m_Owner = this; + } + virtual ~ResultNode(){} + + virtual uint32_t GetPlugCount()const + { + return 1; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: + default: return mResult; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: + default: return mResult; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + RESULT result = ResultPolicies::template Operation<RESULT>(); + mResult.WriteData(&result, arEvaluationInfo); + } + }; + + template <typename TYPE1, typename TYPE2, typename TYPE3, typename RESULT, typename TernaryPolicies> class TernaryNode : public Node + { + public: + TypePlug<TYPE1> mA; + TypePlug<TYPE2> mB; + TypePlug<TYPE3> mC; + + TypePlug<RESULT> mResult; + + TernaryNode() + :mA(true, CRCKey(eA)), + mB(true,CRCKey(eB)), + mC(true,CRCKey(eC)), + mResult(false,CRCKey(eResult)) + { + mA.m_Owner = this; + mB.m_Owner = this; + mC.m_Owner = this; + mResult.m_Owner = this; + } + + virtual ~TernaryNode(){} + + virtual uint32_t GetPlugCount()const + { + return 4; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mA; + case 1: return mB; + case 2: return mC; + case 3: + default: return mResult; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mA; + case 1: return mB; + case 2: return mC; + case 3: + default: return mResult; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + TYPE1 a; + TYPE2 b; + TYPE3 c; + RESULT result; + + mA.ReadData(&a, arEvaluationInfo); + mB.ReadData(&b, arEvaluationInfo); + mC.ReadData(&c, arEvaluationInfo); + + result = TernaryPolicies::template Operation<TYPE1, TYPE2, TYPE3, RESULT>(a, b, c); + + mResult.WriteData(&result, arEvaluationInfo); + } + }; + + + +} + +} diff --git a/Runtime/mecanim/graph/plug.h b/Runtime/mecanim/graph/plug.h new file mode 100644 index 0000000..ce90020 --- /dev/null +++ b/Runtime/mecanim/graph/plug.h @@ -0,0 +1,123 @@ +/* + Copyright (c) 7244339 Canada Inc. (Mecanim) + All Rights Reserved. +*/ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/object.h" + +#include "Runtime/Math/Simd/float4.h" +#include "Runtime/Math/Simd/xform.h" + +namespace mecanim +{ + +namespace graph +{ + class Node; + + enum ePlugType + { + InvalidPlugId, + Float4Id, + FloatId, + UInt32Id, // Not used anymore + Int32Id, + BoolId, + Float1Id, // Not used anymore + Bool4Id, // Not used anymore + XformId, + LastPlugId + }; + + template<typename T> struct Trait; + + template<> struct Trait<math::float4> { static ePlugType PlugType(){return Float4Id;} }; + template<> struct Trait<float> { static ePlugType PlugType(){return FloatId;} }; + template<> struct Trait<math::float1> { static ePlugType PlugType(){return FloatId;} }; + template<> struct Trait<uint32_t> { static ePlugType PlugType(){return UInt32Id;} }; + template<> struct Trait<int32_t> { static ePlugType PlugType(){return Int32Id;} }; + template<> struct Trait<bool> { static ePlugType PlugType(){return BoolId;} }; + template<> struct Trait<math::bool4> { static ePlugType PlugType(){return Bool4Id;} }; + template<> struct Trait<math::xform> { static ePlugType PlugType(){return XformId;} }; + + class DataBlock + { + public: + uint32_t m_PlugCount; + uint32_t* m_EvaluationId; + char* m_Buffer; + + bool IsDirty(uint32_t aPlugId, uint32_t aEvaluationId)const + { + return m_EvaluationId[aPlugId] != aEvaluationId; + } + template<typename TYPE> TYPE GetData(uint32_t offset)const + { + return *reinterpret_cast<TYPE*>(&m_Buffer[offset]); + } + template<typename TYPE> void SetData(TYPE const& arValue, uint32_t aPlugId, uint32_t offset, uint32_t aEvaluationId) + { + *reinterpret_cast<TYPE*>(&m_Buffer[offset]) = arValue; + m_EvaluationId[aPlugId] = aEvaluationId; + } + }; + + class EvaluationInfo + { + public: + DataBlock m_DataBlock; + uint32_t m_EvaluationId; + }; + + class GraphPlug + { + public: + GraphPlug():m_PlugId(numeric_limits<uint32_t>::max_value),m_Input(false),m_Owner(0),m_Source(0),m_Offset(0){} + GraphPlug(bool input, uint32_t id, Node* owner=0):m_PlugId(numeric_limits<uint32_t>::max_value),m_Input(input),m_Owner(owner),m_Source(0),m_ID(id){} + + virtual ~GraphPlug(){} + + virtual std::size_t ValueSize()const=0; + virtual std::size_t ValueAlign()const=0; + virtual ePlugType PlugType()const=0; + + GraphPlug* GetSource(){return m_Source;} + + virtual bool ReadData(void* apData, EvaluationInfo& arEvaluationInfo)const=0; + virtual bool WriteData(void const* apData, EvaluationInfo& arEvaluationInfo)const=0; + + uint32_t m_PlugId; + bool m_Input; + Node* m_Owner; + GraphPlug* m_Source; + uint32_t m_ID; + uint32_t m_Offset; + }; + + struct Constant + { + Constant():m_Plug(0),m_VectorValue(0),m_FloatValue(0){} + + union { + float m_FloatValue; + uint32_t m_UIntValue; + int32_t m_IntValue; + bool m_BoolValue; + }; + // Cannot put float4 in union because of operator= + math::float4 m_VectorValue; + graph::GraphPlug* m_Plug; + }; + + + STATIC_INLINE ePlugType GetPlugType(GraphPlug const& arPlug) + { + return arPlug.PlugType(); + } +} + +} diff --git a/Runtime/mecanim/graph/plugbinder.cpp b/Runtime/mecanim/graph/plugbinder.cpp new file mode 100644 index 0000000..03ffb30 --- /dev/null +++ b/Runtime/mecanim/graph/plugbinder.cpp @@ -0,0 +1,180 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/graph/plugbinder.h" + +namespace mecanim +{ + void InitializeSetPlugBinder3(uint32_t& arCount, SetPlugBinder3 *& arSetPlugBinder3, ValuesList const& arValuesList, graph::EvaluationGraph const* apGraph, memory::Allocator& arAlloc) + { + arCount = 0; + arSetPlugBinder3 = 0; + + uint32_t i; + for(i = 0 ; i < apGraph->m_Input->mPlugCount; i++) + { + graph::GraphPlug& plug = *apGraph->m_Input->mPlugArray[i]; + uint32_t j; + for(j=0;j<arValuesList.m_InValuesCount;++j) + { + int32_t index = FindValueIndex(arValuesList.m_InValuesConstant[j], plug.m_ID); + if(index > -1) + { + arCount++; + } + } + } + + if(arCount) + { + arSetPlugBinder3 = arAlloc.ConstructArray<SetPlugBinder3>(arCount); + uint32_t binderCount; + for(i = 0, binderCount = 0; i < apGraph->m_Input->mPlugCount; i++) + { + graph::GraphPlug const* plug = apGraph->m_Input->mPlugArray[i]; + uint32_t j; + for(j=0;j<arValuesList.m_InValuesCount;++j) + { + int32_t index = FindValueIndex(arValuesList.m_InValuesConstant[j], plug->m_ID); + if(index > -1) + { + uint32_t valueIndex = static_cast<uint32_t>(index); + arSetPlugBinder3[binderCount++] = bind( SetPlugValueList, plug, j, valueIndex); + } + } + } + } + } + + void InitializeGetPlugBinder3(uint32_t& arCount, + GetPlugBinder3 *& arGetPlugBinder3, + ValuesList const& arValuesList, + graph::EvaluationGraph const* apGraph, + memory::Allocator& arAlloc) + { + arCount = 0; + arGetPlugBinder3 = 0; + + uint32_t i; + for(i = 0 ; i < apGraph->m_Output->mPlugCount; i++) + { + graph::GraphPlug& plug = *apGraph->m_Output->mPlugArray[i]; + uint32_t j; + for(j=0;j<arValuesList.m_OutValuesCount;++j) + { + int32_t index = FindValueIndex(arValuesList.m_OutValuesConstant[j], plug.m_ID); + if(index > -1) + { + arCount++; + } + } + } + + if(arCount) + { + arGetPlugBinder3 = arAlloc.ConstructArray<GetPlugBinder3>(arCount); + uint32_t binderCount; + for(i = 0, binderCount = 0; i < apGraph->m_Output->mPlugCount; i++) + { + graph::GraphPlug const* plug = apGraph->m_Output->mPlugArray[i]; + uint32_t j; + for(j=0;j<arValuesList.m_OutValuesCount;++j) + { + int32_t index = FindValueIndex(arValuesList.m_OutValuesConstant[j], plug->m_ID); + if(index > -1) + { + uint32_t valueIndex = static_cast<uint32_t>(index); + arGetPlugBinder3[binderCount++] = bind( GetPlugValueList, plug, j, valueIndex); + } + } + } + } + } + + + + void InitializeSetPlugBinder2(uint32_t& arCount, SetPlugBinder2 *& arSetPlugBinder2, ValuesList const& arValuesList, graph::EvaluationGraph const* apGraph, memory::Allocator& arAlloc) + { + arCount = 0; + arSetPlugBinder2 = 0; + + uint32_t i; + for(i = 0 ; i < apGraph->m_Input->mPlugCount; i++) + { + graph::GraphPlug& plug = *apGraph->m_Input->mPlugArray[i]; + uint32_t j; + for(j=0;j<arValuesList.m_InValuesCount;++j) + { + int32_t index = FindValueIndex(arValuesList.m_InValuesConstant[j], plug.m_ID); + if(index > -1) + { + arCount++; + } + } + } + + if(arCount) + { + arSetPlugBinder2 = arAlloc.ConstructArray<SetPlugBinder2>(arCount); + uint32_t binderCount; + for(i = 0, binderCount = 0; i < apGraph->m_Input->mPlugCount; i++) + { + graph::GraphPlug const* plug = apGraph->m_Input->mPlugArray[i]; + uint32_t j; + for(j=0;j<arValuesList.m_InValuesCount;++j) + { + int32_t index = FindValueIndex(arValuesList.m_InValuesConstant[j], plug->m_ID); + if(index > -1) + { + uint32_t valueIndex = static_cast<uint32_t>(index); + arSetPlugBinder2[binderCount++] = bind( SetPlugValue, plug, valueIndex); + } + } + } + } + } + + void InitializeGetPlugBinder5(uint32_t& arCount, + GetPlugBinder2 *& arGetPlugBinder2, + ValuesList const& arValuesList, + graph::EvaluationGraph const* apGraph, + memory::Allocator& arAlloc) + { + arCount = 0; + arGetPlugBinder2 = 0; + + uint32_t i; + for(i = 0 ; i < apGraph->m_Output->mPlugCount; i++) + { + graph::GraphPlug& plug = *apGraph->m_Output->mPlugArray[i]; + uint32_t j; + for(j=0;j<arValuesList.m_OutValuesCount;++j) + { + int32_t index = FindValueIndex(arValuesList.m_OutValuesConstant[j], plug.m_ID); + if(index > -1) + { + arCount++; + } + } + } + + if(arCount) + { + arGetPlugBinder2 = arAlloc.ConstructArray<GetPlugBinder2>(arCount); + uint32_t binderCount; + for(i = 0, binderCount = 0; i < apGraph->m_Output->mPlugCount; i++) + { + graph::GraphPlug const* plug = apGraph->m_Output->mPlugArray[i]; + uint32_t j; + for(j=0;j<arValuesList.m_OutValuesCount;++j) + { + int32_t index = FindValueIndex(arValuesList.m_OutValuesConstant[j], plug->m_ID); + if(index > -1) + { + uint32_t valueIndex = static_cast<uint32_t>(index); + arGetPlugBinder2[binderCount++] = bind( GetPlugValue, plug, valueIndex); + } + } + } + } + } + +} diff --git a/Runtime/mecanim/graph/plugbinder.h b/Runtime/mecanim/graph/plugbinder.h new file mode 100644 index 0000000..f224440 --- /dev/null +++ b/Runtime/mecanim/graph/plugbinder.h @@ -0,0 +1,85 @@ +/* + Copyright (c) 7244339 Canada Inc. (Mecanim) + All Rights Reserved. +*/ +#pragma once + +#include "Runtime/mecanim/bind.h" +#include "Runtime/mecanim/graph/graph.h" +#include "Runtime/mecanim/generic/valuearray.h" + +namespace mecanim +{ + typedef binder2<function<void (graph::GraphPlug const*, uint32_t, ValueArray const*, graph::EvaluationGraphWorkspace*)>::ptr, + graph::GraphPlug const*, + uint32_t> + SetPlugBinder2; + + typedef binder2<function<void (graph::GraphPlug const*, uint32_t, ValueArray*, graph::EvaluationGraphWorkspace*)>::ptr, + graph::GraphPlug const*, + uint32_t > + GetPlugBinder2; + + typedef binder3<function<void (graph::GraphPlug const*, uint32_t, uint32_t, ValuesList const&, graph::EvaluationGraphWorkspace*)>::ptr, + graph::GraphPlug const*, + uint32_t, + uint32_t> + SetPlugBinder3; + + + typedef binder3<function<void (graph::GraphPlug const*, uint32_t, uint32_t, ValuesList const&, graph::EvaluationGraphWorkspace*)>::ptr, + graph::GraphPlug const*, + uint32_t, + uint32_t> + GetPlugBinder3; + + STATIC_INLINE void SetPlugValue(mecanim::graph::GraphPlug const* plug, mecanim::uint32_t valueIndex, mecanim::ValueArray const* valueArray, mecanim::graph::EvaluationGraphWorkspace* graphWS) + { + char ATTRIBUTE_ALIGN(ALIGN4F) value[48]; + valueArray->m_ValueArray[valueIndex]->ReadData(value); + plug->WriteData( value, graphWS->m_EvaluationInfo); + } + STATIC_INLINE void GetPlugValue(mecanim::graph::GraphPlug const* plug, mecanim::uint32_t valueIndex, mecanim::ValueArray* valueArray, mecanim::graph::EvaluationGraphWorkspace* graphWS) + { + char ATTRIBUTE_ALIGN(ALIGN4F) value[48]; + plug->ReadData( value, graphWS->m_EvaluationInfo); + valueArray->m_ValueArray[valueIndex]->WriteData(value); + } + + STATIC_INLINE void SetPlugValueList(mecanim::graph::GraphPlug const* plug, mecanim::uint32_t listIndex, mecanim::uint32_t valueIndex, ValuesList const& valuesList, mecanim::graph::EvaluationGraphWorkspace* graphWS) + { + char ATTRIBUTE_ALIGN(ALIGN4F) value[48]; + valuesList.m_InValues[listIndex]->m_ValueArray[valueIndex]->ReadData(value); + plug->WriteData( value, graphWS->m_EvaluationInfo); + } + STATIC_INLINE void GetPlugValueList(mecanim::graph::GraphPlug const* plug, mecanim::uint32_t listIndex, mecanim::uint32_t valueIndex, ValuesList const& valuesList, mecanim::graph::EvaluationGraphWorkspace* graphWS) + { + char ATTRIBUTE_ALIGN(ALIGN4F) value[48]; + plug->ReadData( value, graphWS->m_EvaluationInfo); + valuesList.m_OutValues[listIndex]->m_ValueArray[valueIndex]->WriteData(value); + } + + void InitializeSetPlugBinder3(uint32_t& arCount, + SetPlugBinder3 *& arSetPlugBinder3, + ValuesList const& arValuesList, + graph::EvaluationGraph const* apGraph, + memory::Allocator& arAlloc); + + void InitializeGetPlugBinder3(uint32_t& arCount, + GetPlugBinder3 *& arGetPlugBinder3, + ValuesList const& arValuesList, + graph::EvaluationGraph const* apGraph, + memory::Allocator& arAlloc); + + void InitializeSetPlugBinder2(uint32_t& arCount, + SetPlugBinder2 *& arSetPlugBinder2, + ValuesList const& arValuesList, + graph::EvaluationGraph const* apGraph, + memory::Allocator& arAlloc); + + void InitializeGetPlugBinder2(uint32_t& arCount, + GetPlugBinder2 *& arGetPlugBinder2, + ValuesList const& arValuesList, + graph::EvaluationGraph const* apGraph, + memory::Allocator& arAlloc); +}
\ No newline at end of file diff --git a/Runtime/mecanim/graph/quaternionnode.h b/Runtime/mecanim/graph/quaternionnode.h new file mode 100644 index 0000000..19f1f86 --- /dev/null +++ b/Runtime/mecanim/graph/quaternionnode.h @@ -0,0 +1,283 @@ +/* + Copyright (c) 7244339 Canada Inc. (Mecanim) + All Rights Reserved. +*/ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/object.h" + +#include "Runtime/Math/Simd/math.h" + +#include "Runtime/mecanim/graph/plug.h" +#include "Runtime/mecanim/graph/node.h" + +#include "Runtime/mecanim/graph/unarynode.h" + +namespace mecanim +{ + +namespace graph +{ + class quatIdentityOp + { + public: + template<typename RESULT> static math::float4 Operation(){ return math::quatIdentity(); } + }; + + class quatIdentity : public ResultNode<math::float4, quatIdentityOp> + { + public: + static const eNodeType mId = quatIdentityId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE, typename RESULT> class quatConjOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quatConj(l); } + }; + + class quatConj : public UnaryNode<math::float4, math::float4, quatConjOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quatConjId; + virtual eNodeType NodeType(){return mId;} + }; + + class quatMulOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::float4 Operation( math::float4 const& l, math::float4 const& r){ return math::quatMul(l, r); } + }; + + class quatMul : public BinaryNode<math::float4, math::float4, math::float4, quatMulOp> + { + public: + static const eNodeType mId = quatMulId; + virtual eNodeType NodeType(){return mId;} + }; + + class quatMulVecOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::float4 Operation( math::float4 const& l, math::float4 const& r){ return math::quatMulVec(l, r); } + }; + + class quatMulVec : public BinaryNode<math::float4, math::float4, math::float4, quatMulVecOp> + { + public: + static const eNodeType mId = quatMulVecId; + virtual eNodeType NodeType(){return mId;} + }; + + class quatLerpOp + { + public: + template<typename TYPE1, typename TYPE2, typename TYPE3, typename RESULT> static math::float4 Operation( math::float4 const& a, math::float4 const& b, float c){ return math::quatLerp(a, b, math::float1(c)); } + }; + + class quatLerp : public TernaryNode<math::float4, math::float4, float, math::float4, quatLerpOp> + { + public: + static const eNodeType mId = quatLerpId; + virtual eNodeType NodeType(){return mId;} + }; + + class quatArcRotateOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::float4 Operation( math::float4 const& l, math::float4 const& r){ return math::quatArcRotate(l, r); } + }; + + class quatArcRotate : public BinaryNode<math::float4, math::float4, math::float4, quatArcRotateOp> + { + public: + static const eNodeType mId = quatArcRotateId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class quatArcRotateXOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quatArcRotateX(l); } + }; + + class quatArcRotateX : public UnaryNode<math::float4, math::float4, quatArcRotateXOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quatArcRotateXId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class quatXcosOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quatXcos(l); } + }; + + class quatXcos : public UnaryNode<math::float4, math::float4, quatXcosOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quatXcosId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class quatYcosOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quatYcos(l); } + }; + + class quatYcos : public UnaryNode<math::float4, math::float4, quatYcosOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quatYcosId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class quatZcosOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quatZcos(l); } + }; + + class quatZcos : public UnaryNode<math::float4, math::float4, quatZcosOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quatZcosId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class quatEulerToQuatOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quatEulerToQuat(l); } + }; + + class quatEulerToQuat : public UnaryNode<math::float4, math::float4, quatEulerToQuatOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quatEulerToQuatId; + virtual eNodeType NodeType(){return mId;} + }; + template<typename TYPE1, typename RESULT> class quatQuatToEulerOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quatQuatToEuler(l); } + }; + + class quatQuatToEuler : public UnaryNode<math::float4, math::float4, quatQuatToEulerOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quatQuatToEulerId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class quatProjOnYPlaneOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quatProjOnYPlane(l); } + }; + + class quatProjOnYPlane : public UnaryNode<math::float4, math::float4, quatProjOnYPlaneOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quatProjOnYPlaneId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class quat2QtanOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quat2Qtan(l); } + }; + + class quat2Qtan : public UnaryNode<math::float4, math::float4, quat2QtanOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quat2QtanId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class qtan2QuatOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::qtan2Quat(l); } + }; + + class qtan2Quat : public UnaryNode<math::float4, math::float4, qtan2QuatOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = qtan2QuatId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class ZYRoll2QuatOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::ZYRoll2Quat(l); } + }; + + class ZYRoll2Quat : public UnaryNode<math::float4, math::float4, ZYRoll2QuatOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = ZYRoll2QuatId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class quat2ZYRollOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quat2ZYRoll(l); } + }; + + class quat2ZYRoll : public UnaryNode<math::float4, math::float4, quat2ZYRollOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quat2ZYRollId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class RollZY2QuatOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::RollZY2Quat(l); } + }; + + class RollZY2Quat : public UnaryNode<math::float4, math::float4, RollZY2QuatOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = RollZY2QuatId; + virtual eNodeType NodeType(){return mId;} + }; + + template<typename TYPE1, typename RESULT> class quat2RollZYOp + { + public: + static math::float4 Operation( math::float4 const& l){ return math::quat2RollZY(l); } + }; + + class quat2RollZY : public UnaryNode<math::float4, math::float4, quat2RollZYOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = quat2RollZYId; + virtual eNodeType NodeType(){return mId;} + }; + + class quatWeightOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::float4 Operation( math::float4 const& l, float r){ return math::quatWeight(l, math::float1(r)); } + }; + + class quatWeight : public BinaryNode<math::float4, float, math::float4, quatWeightOp> + { + public: + static const eNodeType mId = quatWeightId; + virtual eNodeType NodeType(){return mId;} + }; +} +}
\ No newline at end of file diff --git a/Runtime/mecanim/graph/unarynode.h b/Runtime/mecanim/graph/unarynode.h new file mode 100644 index 0000000..8ed5b82 --- /dev/null +++ b/Runtime/mecanim/graph/unarynode.h @@ -0,0 +1,112 @@ +/* + Copyright (c) 7244339 Canada Inc. (Mecanim) + All Rights Reserved. +*/ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/object.h" + +#include "Runtime/Math/Simd/float4.h" + +#include "Runtime/mecanim/graph/plug.h" + +namespace mecanim +{ + +namespace graph +{ + template <typename TYPE, typename RESULT, typename UnaryPolicies> class UnaryNode : public Node + { + public: + TypePlug<TYPE> mA; + + TypePlug<RESULT> mResult; + + UnaryNode() + :mA(true,CRCKey(eA)), + mResult(false,CRCKey(eResult)) + { + mA.m_Owner = this; + mResult.m_Owner = this; + } + virtual ~UnaryNode(){} + + virtual uint32_t GetPlugCount()const + { + return 2; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mA; + case 1: + default: return mResult; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mA; + case 1: + default: return mResult; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + TYPE a; + RESULT result; + + mA.ReadData(&a, arEvaluationInfo); + + result = UnaryPolicies::Operation(a); + + mResult.WriteData(&result, arEvaluationInfo); + } + }; + + + template< typename TYPE, typename RESULT > class NegationOp + { + public: + static RESULT Operation( TYPE const& l){ return -l; } + }; + + template<> class NegationOp<bool, bool> + { + public: + static bool Operation(bool const &l) { return !l; } + }; + + class NegationFloat : public UnaryNode<float, float, NegationOp<float, float> > + { + public: + static const eNodeType mId = NegationFloatId; + virtual eNodeType NodeType(){return mId;} + }; + class NegationInt : public UnaryNode<int32_t, int32_t, NegationOp<int32_t, int32_t> > + { + public: + static const eNodeType mId = NegationIntId; + virtual eNodeType NodeType(){return mId;} + }; + class NegationFloat4 : public UnaryNode<math::float4, math::float4, NegationOp<math::float4, math::float4> > + { + public: + static const eNodeType mId = NegationFloat4Id; + virtual eNodeType NodeType(){return mId;} + }; + class NegationBool : public UnaryNode<bool, bool, NegationOp<bool, bool> > + { + public: + static const eNodeType mId = NegationBoolId; + virtual eNodeType NodeType(){return mId;} + }; +} + +} diff --git a/Runtime/mecanim/graph/xformnode.h b/Runtime/mecanim/graph/xformnode.h new file mode 100644 index 0000000..d0abf24 --- /dev/null +++ b/Runtime/mecanim/graph/xformnode.h @@ -0,0 +1,348 @@ +/* + Copyright (c) 7244339 Canada Inc. (Mecanim) + All Rights Reserved. +*/ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/object.h" + +#include "Runtime/Math/Simd/math.h" + +#include "Runtime/mecanim/graph/plug.h" +#include "Runtime/mecanim/graph/node.h" + +#include "Runtime/mecanim/graph/unarynode.h" + +#include "Runtime/mecanim/generic/stringtable.h" + +namespace mecanim +{ + +namespace graph +{ + class IdentityOp + { + public: + template<typename RESULT> static math::xform Operation(){ return math::xformIdentity(); } + }; + + class xformIdentity : public ResultNode<math::xform, IdentityOp> + { + public: + static const eNodeType mId = xformIdentityId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformMulVecOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::float4 Operation( math::xform const& l, math::float4 const& r){ return math::xformMulVec(l, r); } + }; + + class xformMulVec : public BinaryNode<math::xform, math::float4, math::float4, xformMulVecOp> + { + public: + static const eNodeType mId = xformMulVecId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformInvMulVecOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::float4 Operation( math::xform const& l, math::float4 const& r){ return math::xformInvMulVec(l, r); } + }; + + class xformInvMulVec : public BinaryNode<math::xform, math::float4, math::float4, xformInvMulVecOp> + { + public: + static const eNodeType mId = xformInvMulVecId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformMulOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::xform Operation( math::xform const& l, math::xform const& r){ return math::xformMul(l, r); } + }; + + class xformMul : public BinaryNode<math::xform, math::xform, math::xform, xformMulOp> + { + public: + static const eNodeType mId = xformMulId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformInvMulOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::xform Operation( math::xform const& l, math::xform const& r){ return math::xformInvMul(l, r); } + }; + + class xformInvMul : public BinaryNode<math::xform, math::xform, math::xform, xformInvMulOp> + { + public: + static const eNodeType mId = xformInvMulId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformMulInvOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::xform Operation( math::xform const& l, math::xform const& r){ return math::xformMulInv(l, r); } + }; + + class xformMulInv : public BinaryNode<math::xform, math::xform, math::xform, xformMulInvOp> + { + public: + static const eNodeType mId = xformMulInvId; + virtual eNodeType NodeType(){ return mId;} + }; + + class xformEqualOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static bool Operation( math::xform const& l, math::xform const& r){ return l == r; } + }; + + class xformEqual : public BinaryNode<math::xform, math::xform, bool, xformEqualOp> + { + public: + static const eNodeType mId = xformEqualId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformWeightOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::xform Operation( math::xform const& l, float r){ return math::xformWeight(l, math::float1(r)); } + }; + + class xformWeight : public BinaryNode<math::xform, float, math::xform, xformWeightOp> + { + public: + static const eNodeType mId = xformWeightId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformBlendOp + { + public: + template<typename TYPE1, typename TYPE2, typename TYPE3, typename RESULT> static math::xform Operation( math::xform const& a,math::xform const& b, float const& c){ return math::xformBlend(a,b,math::float1(c)); } + }; + + class xformBlend : public TernaryNode<math::xform, math::xform, float, math::xform, xformBlendOp> + { + public: + static const eNodeType mId = xformBlendId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformAddOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::xform Operation( math::xform const& l, math::xform const& r){ return math::xformAdd(l, r); } + }; + + class xformAdd : public BinaryNode<math::xform, math::xform, math::xform, xformAddOp> + { + public: + static const eNodeType mId = xformAddId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformSubOp + { + public: + template<typename TYPE1, typename TYPE2, typename RESULT> static math::xform Operation( math::xform const& l, math::xform const& r){ return math::xformSub(l, r); } + }; + + class xformSub : public BinaryNode<math::xform, math::xform, math::xform, xformSubOp> + { + public: + static const eNodeType mId = xformSubId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformComposeOp + { + public: + template<typename TYPE1, typename TYPE2, typename TYPE3, typename RESULT> static math::xform Operation( math::float4 const& t, math::float4 const& q, math::float4 const& s){ return math::xform(t, q, s); } + }; + + class xformCompose : public TernaryNode<math::float4, math::float4, math::float4, math::xform, xformComposeOp> + { + public: + xformCompose() + { + mA.m_ID = CRCKey(eT); + mB.m_ID = CRCKey(eQ); + mC.m_ID = CRCKey(eS); + } + static const eNodeType mId = xformComposeId; + virtual eNodeType NodeType(){return mId;} + }; + + class xformDecompose : public Node + { + public: + static const eNodeType mId = xformDecomposeId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<math::xform> mX; + TypePlug<math::float4> mT; + TypePlug<math::float4> mQ; + TypePlug<math::float4> mS; + + xformDecompose() + :mX(true,CRCKey(eX)), + mT(false,CRCKey(eT)), + mQ(false,CRCKey(eQ)), + mS(false,CRCKey(eS)) + { + mX.m_Owner = this; + mT.m_Owner = this; + mQ.m_Owner = this; + mS.m_Owner = this; + } + + virtual ~xformDecompose(){} + + virtual uint32_t GetPlugCount()const + { + return 4; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mX; + case 1: return mT; + case 2: return mQ; + case 3: + default:return mS; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mX; + case 1: return mT; + case 2: return mQ; + case 3: + default:return mS; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + math::xform x; + + mX.ReadData(&x, arEvaluationInfo); + + mT.WriteData(&x.t, arEvaluationInfo); + mQ.WriteData(&x.q, arEvaluationInfo); + mS.WriteData(&x.s, arEvaluationInfo); + } + }; + + class xformRefChange : public Node + { + public: + static const eNodeType mId = xformRefChangeId; + virtual eNodeType NodeType(){return mId;} + + TypePlug<math::xform> mSrcRefX; + TypePlug<math::xform> mDstRefX; + TypePlug<math::xform> mSrcPivotX; + TypePlug<math::xform> mDstPivotX; + TypePlug<float> mRefWeight; + TypePlug<float> mPivotWeight; + TypePlug<math::xform> mXI; + TypePlug<math::xform> mXO; + + xformRefChange() : mSrcRefX(true,CRCKey(eSrcRefX)), + mDstRefX(true,CRCKey(eDstRefX)), + mSrcPivotX(true,CRCKey(eSrcPivotX)), + mDstPivotX(true,CRCKey(eDstPivotX)), + mRefWeight(true,CRCKey(eRefWeight)), + mPivotWeight(true,CRCKey(ePivotWeight)), + mXI(true,CRCKey(eXI)), + mXO(false,CRCKey(eXO)) + { + mSrcRefX.m_Owner = this; + mDstRefX.m_Owner = this; + mSrcPivotX.m_Owner = this; + mDstPivotX.m_Owner = this; + mRefWeight.m_Owner = this; + mPivotWeight.m_Owner = this; + mXI.m_Owner = this; + mXO.m_Owner = this; + } + + virtual ~xformRefChange(){} + + virtual uint32_t GetPlugCount()const + { + return 8; + } + virtual GraphPlug& GetPlug(uint32_t aIndex) + { + switch(aIndex) + { + case 0: return mSrcRefX; + case 1: return mDstRefX; + case 2: return mSrcPivotX; + case 3: return mDstPivotX; + case 4: return mRefWeight; + case 5: return mPivotWeight; + case 6: return mXI; + case 7: + default: return mXO; + } + } + virtual GraphPlug const& GetPlug(uint32_t aIndex)const + { + switch(aIndex) + { + case 0: return mSrcRefX; + case 1: return mDstRefX; + case 2: return mSrcPivotX; + case 3: return mDstPivotX; + case 4: return mRefWeight; + case 5: return mPivotWeight; + case 6: return mXI; + case 7: + default: return mXO; + } + } + + virtual void Evaluate(EvaluationInfo& arEvaluationInfo) + { + math::xform srcRefX, dstRefX, srcPivotX, dstPivotX, xi, xo; + float rw,pw; + + mSrcRefX.ReadData(&srcRefX, arEvaluationInfo); + mDstRefX.ReadData(&dstRefX, arEvaluationInfo); + mSrcPivotX.ReadData(&srcPivotX, arEvaluationInfo); + mDstPivotX.ReadData(&dstPivotX, arEvaluationInfo); + mRefWeight.ReadData(&rw, arEvaluationInfo); + mPivotWeight.ReadData(&pw, arEvaluationInfo); + mXI.ReadData(&xi,arEvaluationInfo); + + xo = math::xformMul(xi,srcPivotX); + xo = math::xformInvMul(srcRefX,xo); + xo = math::xformWeight(xo,math::float1(1-pw)); + xo = math::xformMul(dstRefX,xo); + xo = math::xformMulInv(xo,dstPivotX); + xo = math::xformBlend(xi,xo,math::float1(rw)); + + mXO.WriteData(&xo, arEvaluationInfo); + } + }; +} + +}
\ No newline at end of file diff --git a/Runtime/mecanim/human/hand.cpp b/Runtime/mecanim/human/hand.cpp new file mode 100644 index 0000000..dd5e062 --- /dev/null +++ b/Runtime/mecanim/human/hand.cpp @@ -0,0 +1,351 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/skeleton/skeleton.h" +#include "Runtime/mecanim/human/hand.h" + +namespace mecanim +{ + +namespace hand +{ + const static int32_t Phalange2DoF[hand::kLastPhalange][3] = + { + { kProximalDownUp, kProximalInOut, -1 }, // kProximal + { kIntermediateCloseOpen, -1, -1 }, // kIntermediate + { kDistalCloseOpen, -1, -1 } // kDistal + }; + + const static int32_t DoF2Bone[s_DoFCount] = + { + kThumb * kLastPhalange + kProximal, + kThumb * kLastPhalange + kProximal, + kThumb * kLastPhalange + kIntermediate, + kThumb * kLastPhalange + kDistal, + + kIndex * kLastPhalange + kProximal, + kIndex * kLastPhalange + kProximal, + kIndex * kLastPhalange + kIntermediate, + kIndex * kLastPhalange + kDistal, + + kMiddle * kLastPhalange + kProximal, + kMiddle * kLastPhalange + kProximal, + kMiddle * kLastPhalange + kIntermediate, + kMiddle * kLastPhalange + kDistal, + + kRing * kLastPhalange + kProximal, + kRing * kLastPhalange + kProximal, + kRing * kLastPhalange + kIntermediate, + kRing * kLastPhalange + kDistal, + + kLittle * kLastPhalange + kProximal, + kLittle * kLastPhalange + kProximal, + kLittle * kLastPhalange + kIntermediate, + kLittle * kLastPhalange + kDistal + }; + + + + skeleton::SetupAxesInfo const& GetAxeInfo(uint32_t index) + { + const static skeleton::SetupAxesInfo setupAxesInfoArray[s_BoneCount] = + { + {{0,0.125,0.125f,1},{0,1,0,0},{0,-25,-20},{0,25,20},{-1,-1,1,-1},math::kZYRoll,0}, // kThumb.kProximal + {{0,-0.2f,0,1},{0,1,0,0},{0,0,-40},{0,0,35},{-1,1,1,-1},math::kZYRoll,0}, // kThumb.kIntermediate + {{0,-0.2f,0,1},{0,1,0,0},{0,0,-40},{0,0,35},{-1,1,1,-1},math::kZYRoll,0}, // kThumb.kDistal + {{0,0.08f,0.3f,1},{0,0,1,0},{0,-20,-50},{0,20,50},{-1,-1,-1,-1},math::kZYRoll,0}, // kIndex.kProximal + {{0,0,0.33f,1},{0,0,1,0},{0,0,-45},{0,0,45},{-1,1,-1,-1},math::kZYRoll,0}, // kIndex.kIntermediate + {{0,0,0.33f,1},{0,0,1,0},{0,0,-45},{0,0,45},{-1,1,-1,-1},math::kZYRoll,0}, // kIndex.kDistal + {{0,0.04f,0.3f,1},{0,0,1,0},{0,-7.5f,-50},{0,7.5f,50},{-1,-1,-1,-1},math::kZYRoll,0}, // kMiddle.kProximal + {{0,0,0.33f,1},{0,0,1,0},{0,0,-45},{0,0,45},{-1,1,-1,-1},math::kZYRoll,0}, // kMiddle.kIntermediate + {{0,0,0.33f,1},{0,0,1,0},{0,0,-45},{0,0,45},{-1,1,-1,-1},math::kZYRoll,0}, // kMiddle.kDistal + {{0,-0.04f,0.3f,1},{0,0,1,0},{0,-7.5f,-50},{0,7.5f,50},{-1,1,-1,-1},math::kZYRoll,0}, // kRing.kProximal + {{0,0,0.33f,1},{0,0,1,0},{0,0,-45},{0,0,45},{-1,1,-1,-1},math::kZYRoll,0}, // kRing.kIntermediate + {{0,0,0.33f,1},{0,0,1,0},{0,0,-45},{0,0,45},{-1,1,-1,-1},math::kZYRoll,0}, // kRing.kDistal + {{0,-0.08f,0.3f,1},{0,0,1,0},{0,-20,-50},{0,20,50},{-1,1,-1,-1},math::kZYRoll,0}, // kLittle.kProximal + {{0,0,0.33f,1},{0,0,1,0},{0,0,-45},{0,0,45},{-1,1,-1,-1},math::kZYRoll,0}, // kLittle.kIntermediate + {{0,0,0.33f,1},{0,0,1,0},{0,0,-45},{0,0,45},{-1,1,-1,-1},math::kZYRoll,0} // kLittle.kDistal + }; + + return setupAxesInfoArray[index]; + } + + + const char* FingerName(uint32_t aFinger) + { + const static char* fingerName[kLastFinger] = { + "Thumb", + "Index", + "Middle", + "Ring", + "Little" + }; + + return fingerName[aFinger]; + } + + const char* PhalangeName(uint32_t aFinger) + { + const static char* phalangeName[kLastPhalange] = { + "Proximal", + "Intermediate", + "Distal" + }; + return phalangeName[aFinger]; + } + + const char* FingerDoFName(uint32_t aFinger) + { + const static char* fingerDoFName[kLastFingerDoF] = { + "1 Stretched", + "Spread", + "2 Stretched", + "3 Stretched" + }; + return fingerDoFName[aFinger]; + } + + Hand::Hand() + { + memset(m_HandBoneIndex, -1, sizeof(int32_t)*s_BoneCount); + } + + int32_t MuscleFromBone(int32_t aBoneIndex, int32_t aDoFIndex) + { + int32_t ret = -1; + int32_t findex = GetFingerIndex(aBoneIndex); + int32_t pindex = GetPhalangeIndex(aBoneIndex); + + if(Phalange2DoF[pindex][2-aDoFIndex] != -1) + { + ret = findex * kLastFingerDoF + Phalange2DoF[pindex][2-aDoFIndex]; + } + + return ret; + } + + int32_t BoneFromMuscle(int32_t aDoFIndex) + { + return DoF2Bone[aDoFIndex]; + } + + Hand* CreateHand(memory::Allocator& arAlloc) + { + Hand* hand = arAlloc.Construct<Hand>(); + memset(hand->m_HandBoneIndex,-1,sizeof(int32_t)*kLastFinger*kLastPhalange); + + return hand; + } + + void DestroyHand(Hand *apHand, memory::Allocator& arAlloc) + { + if(apHand) + { + arAlloc.Deallocate(apHand); + } + } + + HandPose::HandPose() + { + int32_t i; + + for(i = 0; i < s_DoFCount; i++) + { + m_DoFArray[i]= 0; + } + + m_Override = 0; + m_CloseOpen = 0; + m_InOut = 0; + m_Grab = 0; + m_GrabX = math::xformIdentity(); + } + + void HandPoseCopy(HandPose const *apHandPoseSrc,HandPose *apHandPoseDst) + { + int32_t i; + + for(i = 0; i < s_DoFCount; i++) + { + apHandPoseDst->m_DoFArray[i] = apHandPoseSrc->m_DoFArray[i]; + } + + apHandPoseDst->m_Override = apHandPoseDst->m_Override; + apHandPoseDst->m_CloseOpen = apHandPoseDst->m_CloseOpen; + apHandPoseDst->m_InOut = apHandPoseDst->m_InOut; + apHandPoseDst->m_Grab = apHandPoseDst->m_Grab; + apHandPoseDst->m_GrabX = apHandPoseDst->m_GrabX; + } + + void HandSetupAxes(Hand const *apHand, skeleton::SkeletonPose const *apSkeletonPose, skeleton::Skeleton *apSkeleton, bool aLeft) + { + int32_t f,p,b; + + for(f = 0; f < kLastFinger; f++) + { + for(p = 0; p < kLastPhalange; p++) + { + float len = 1.0f; + int32_t skAxisBoneId = -1; + + b = GetBoneIndex(f,p); + int32_t skBoneIndex = apHand->m_HandBoneIndex[b]; + + if(p < kLastPhalange-1 && apHand->m_HandBoneIndex[GetBoneIndex(f,p+1)] >= 0) + { + skAxisBoneId = apHand->m_HandBoneIndex[GetBoneIndex(f,p+1)]; + } + else if(p > 0 && apHand->m_HandBoneIndex[GetBoneIndex(f,p-1)] >= 0) + { + skAxisBoneId = apHand->m_HandBoneIndex[GetBoneIndex(f,p-1)]; + len = -0.75f; + } + + if(skBoneIndex >= 0) + { + skeleton::SetupAxes(apSkeleton,apSkeletonPose, GetAxeInfo(b), skBoneIndex,skAxisBoneId,aLeft,len); + } + } + } + } + + void HandCopyAxes(Hand const *apSrcHand, skeleton::Skeleton const *apSrcSkeleton, Hand const *apHand, skeleton::Skeleton *apSkeleton) + { + int32_t i; + + for(i = 0; i < s_BoneCount; i++) + { + skeleton::Node const *srcNode = apSrcHand->m_HandBoneIndex[i] >= 0 ? &apSrcSkeleton->m_Node[apSrcHand->m_HandBoneIndex[i]] : 0; + skeleton::Node const *node = apHand->m_HandBoneIndex[i] >= 0 ? &apSkeleton->m_Node[apHand->m_HandBoneIndex[i]] : 0; + + if(srcNode != 0 && node != 0 && srcNode->m_AxesId != -1 && node->m_AxesId != -1) + { + apSkeleton->m_AxesArray[node->m_AxesId] = apSrcSkeleton->m_AxesArray[srcNode->m_AxesId]; + } + } + } + + void HandPoseSolve(HandPose const* apHandPoseIn,HandPose* apHandPoseOut) + { + int32_t f; + + for(f = 0; f < kLastFinger; f++) + { + int32_t i = f*kLastFingerDoF; + apHandPoseOut->m_DoFArray[i+kProximalDownUp] = (1-apHandPoseIn->m_Override) * apHandPoseIn->m_DoFArray[i+kProximalDownUp] + apHandPoseIn->m_CloseOpen; + apHandPoseOut->m_DoFArray[i+kProximalInOut] = (1-apHandPoseIn->m_Override) * apHandPoseIn->m_DoFArray[i+kProximalInOut] + apHandPoseIn->m_InOut; + apHandPoseOut->m_DoFArray[i+kIntermediateCloseOpen] = (1-apHandPoseIn->m_Override) * apHandPoseIn->m_DoFArray[i+kIntermediateCloseOpen] + apHandPoseIn->m_CloseOpen; + apHandPoseOut->m_DoFArray[i+kDistalCloseOpen] = (1-apHandPoseIn->m_Override) * apHandPoseIn->m_DoFArray[i+kDistalCloseOpen] + apHandPoseIn->m_CloseOpen; + } + } + + void Hand2SkeletonPose(Hand const *apHand, skeleton::Skeleton const *apSkeleton, HandPose const *apHandPose, skeleton::SkeletonPose *apSkeletonPose) + { + int32_t f,p; + + for(f = 0; f < kLastFinger; f++) + { + for(p = 0; p < kLastPhalange; p++) + { + int32_t i = GetBoneIndex(f,p); + + if(apHand->m_HandBoneIndex[i] >= 0) + { + math::float4 xyz = math::cond( math::bool4(Phalange2DoF[p][2] != -1,Phalange2DoF[p][1] != -1,Phalange2DoF[p][0] != -1,false), + math::float4(apHandPose->m_DoFArray[GetDoFIndex(f,Phalange2DoF[p][2])],apHandPose->m_DoFArray[GetDoFIndex(f,Phalange2DoF[p][1])],apHandPose->m_DoFArray[GetDoFIndex(f,Phalange2DoF[p][0])],0), + math::float4::zero()); + + skeleton::SkeletonSetDoF(apSkeleton,apSkeletonPose,xyz,apHand->m_HandBoneIndex[i]); + } + } + } + } + + void Skeleton2HandPose(Hand const *apHand, skeleton::Skeleton const *apSkeleton,skeleton::SkeletonPose const *apSkeletonPose, HandPose *apHandPose, float aOffset) + { + int32_t f,p; + + for(f = 0; f < kLastFinger; f++) + { + for(p = 0; p < kLastPhalange; p++) + { + int32_t i = GetBoneIndex(f,p); + + if(apHand->m_HandBoneIndex[i] >= 0) + { + const math::float4 xyz = skeleton::SkeletonGetDoF(apSkeleton,apSkeletonPose,apHand->m_HandBoneIndex[i]); + + if(Phalange2DoF[p][2] != -1) apHandPose->m_DoFArray[GetDoFIndex(f,Phalange2DoF[p][2])] = xyz.x().tofloat() + aOffset; + if(Phalange2DoF[p][1] != -1) apHandPose->m_DoFArray[GetDoFIndex(f,Phalange2DoF[p][1])] = xyz.y().tofloat() + aOffset; + if(Phalange2DoF[p][0] != -1) apHandPose->m_DoFArray[GetDoFIndex(f,Phalange2DoF[p][0])] = xyz.z().tofloat() + aOffset; + } + } + } + } + + void FingerLengths(Hand const *apHand, skeleton::Skeleton const *apSkeleton, float *apLengthArray) + { + int32_t f,p; + + for(f = 0; f < kLastFinger; f++) + { + apLengthArray[f] = 0.0f; + + for(p = 0; p < kLastPhalange; p++) + { + int32_t i = GetBoneIndex(f,p); + + if(apHand->m_HandBoneIndex[i] >= 0) + { + apLengthArray[f] += apSkeleton->m_AxesArray[apHand->m_HandBoneIndex[i]].m_Length; + } + } + } + } + + void FingerBaseFromPose(Hand const *apHand,skeleton::SkeletonPose const *apSkeletonPose,math::float4 *apPositionArray) + { + int32_t f; + + for(f = 0; f < kLastFinger; f++) + { + int32_t i = GetBoneIndex(f,kProximal); + + if(apHand->m_HandBoneIndex[i] >= 0) + { + apPositionArray[f] = apSkeletonPose->m_X[apHand->m_HandBoneIndex[i]].t; + } + } + } + + void FingerTipsFromPose(Hand const *apHand,skeleton::Skeleton const *apSkeleton, skeleton::SkeletonPose const *apSkeletonPose,math::float4 *apPositionArray) + { + int32_t f; + + for(f = 0; f < kLastFinger; f++) + { + int32_t i = GetBoneIndex(f,kDistal); + + if(apHand->m_HandBoneIndex[i] >= 0) + { + apPositionArray[f] = skeleton::SkeletonNodeEndPoint(apSkeleton,apHand->m_HandBoneIndex[i],apSkeletonPose); + } + } + } + + void FingersIKSolve(Hand const *apHand, skeleton::Skeleton const *apSkeleton,math::float4 const *apPositionArray, float *apWeightArray, skeleton::SkeletonPose *apSkeletonPose, skeleton::SkeletonPose *apSkeletonPoseWorkspace) + { + int32_t f; + + for(f = 0; f < kLastFinger; f++) + { + int32_t topIndex = apHand->m_HandBoneIndex[hand::GetBoneIndex(f,hand::kProximal)]; + int32_t midIndex = apHand->m_HandBoneIndex[hand::GetBoneIndex(f,hand::kIntermediate)]; + int32_t endIndex = apHand->m_HandBoneIndex[hand::GetBoneIndex(f,hand::kDistal)]; + + if(topIndex >= 0 && midIndex >= 0 && endIndex >= 0) + { + skeleton::Skeleton3BoneIK(apSkeleton,topIndex,midIndex,endIndex,apPositionArray[f],apWeightArray[f],apSkeletonPose,apSkeletonPoseWorkspace); + } + } + } +} +} diff --git a/Runtime/mecanim/human/hand.h b/Runtime/mecanim/human/hand.h new file mode 100644 index 0000000..1050340 --- /dev/null +++ b/Runtime/mecanim/human/hand.h @@ -0,0 +1,124 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/Math/Simd/xform.h" + + +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Animation/MecanimArraySerialization.h" + + +namespace mecanim +{ + +namespace skeleton { struct Skeleton; struct SkeletonPose; struct SetupAxesInfo; } + +namespace hand +{ + enum Fingers + { + kThumb = 0 , + kIndex, + kMiddle, + kRing, + kLittle, + kLastFinger + }; + + enum Phalanges + { + kProximal = 0, + kIntermediate, + kDistal, + kLastPhalange + }; + + enum FingerDoF + { + kProximalDownUp = 0, + kProximalInOut, + kIntermediateCloseOpen, + kDistalCloseOpen, + kLastFingerDoF + }; + + const int32_t s_BoneCount = kLastFinger*kLastPhalange; + const int32_t s_DoFCount = kLastFinger*kLastFingerDoF; + + inline int32_t GetBoneIndex(int32_t fingerIndex, int32_t phalangeIndex) { return fingerIndex * kLastPhalange + phalangeIndex; }; + inline int32_t GetFingerIndex(int32_t boneIndex) { return boneIndex / kLastPhalange; }; + inline int32_t GetPhalangeIndex(int32_t boneIndex) { return boneIndex % kLastPhalange; }; + inline int32_t GetDoFIndex(int32_t fingerIndex, int32_t phalangeDoFIndex) { return fingerIndex * kLastFingerDoF + phalangeDoFIndex; }; + + const char* FingerName(uint32_t finger); + const char* FingerDoFName(uint32_t finger); + const char* PhalangeName(uint32_t finger); + + struct Hand + { + DEFINE_GET_TYPESTRING(Hand) + + Hand(); + int32_t m_HandBoneIndex[s_BoneCount]; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + STATIC_ARRAY_TRANSFER(int32_t, m_HandBoneIndex, s_BoneCount); + } + }; + + struct HandPose + { + DEFINE_GET_TYPESTRING(HandPose) + + HandPose(); + + math::xform m_GrabX; + float m_DoFArray[s_DoFCount]; + float m_Override; + float m_CloseOpen; + float m_InOut; + float m_Grab; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_GrabX); + STATIC_ARRAY_TRANSFER(float, m_DoFArray, s_DoFCount); + + TRANSFER(m_Override); + TRANSFER(m_CloseOpen); + TRANSFER(m_InOut); + TRANSFER(m_Grab); + } + }; + + int32_t MuscleFromBone(int32_t aBoneIndex, int32_t aDoFIndex); + int32_t BoneFromMuscle(int32_t aDoFIndex); + + Hand* CreateHand(memory::Allocator& alloc); + void DestroyHand(Hand *hand, memory::Allocator& alloc); + + void HandSetupAxes(Hand const *hand, skeleton::SkeletonPose const *skeletonPose, skeleton::Skeleton *skeleton, bool aLeft); + void HandCopyAxes(Hand const *srcHand, skeleton::Skeleton const *srcSkeleton, Hand const *hand, skeleton::Skeleton *skeleton); + void HandPoseCopy(HandPose const *handPoseSrc, HandPose *handPoseDst); + + // Retargeting function set + void HandPoseSolve(HandPose const* handPose,HandPose* handPoseOut); + void Hand2SkeletonPose(Hand const *hand, skeleton::Skeleton const *skeleton, HandPose const *handPose, skeleton::SkeletonPose *skeletonPose); + void Skeleton2HandPose(Hand const *hand, skeleton::Skeleton const *skeleton,skeleton::SkeletonPose const *skeletonPose, HandPose *handPose, float offset = 0.0f); + // IK + void FingerLengths(Hand const *hand, float *lengthArray); + void FingerBaseFromPose(Hand const *hand,skeleton::SkeletonPose const *skeletonPose,math::float4 *positionArray); + void FingerTipsFromPose(Hand const *hand,skeleton::Skeleton const *skeleton, skeleton::SkeletonPose const *skeletonPose,math::float4 *positionArray); + + void FingersIKSolve(Hand const *hand, skeleton::Skeleton const *skeleton,math::float4 const *positionArray, float *apWeightArray, skeleton::SkeletonPose *skeletonPose, skeleton::SkeletonPose *skeletonPoseWorkspace); + + mecanim::skeleton::SetupAxesInfo const& GetAxeInfo(uint32_t index); + +}// namespace hand + +} diff --git a/Runtime/mecanim/human/handle.h b/Runtime/mecanim/human/handle.h new file mode 100644 index 0000000..af1df50 --- /dev/null +++ b/Runtime/mecanim/human/handle.h @@ -0,0 +1,40 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/Math/Simd/xform.h" + +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" + +namespace mecanim +{ + +namespace human +{ + struct Handle + { + DEFINE_GET_TYPESTRING(Handle) + + Handle(): + m_X(math::xformIdentity()), + m_ParentHumanIndex(numeric_limits<uint32_t>::max_value), + m_ID(numeric_limits<uint32_t>::max_value) + { + } + + math::xform m_X; // Local tranform + uint32_t m_ParentHumanIndex; // Related parent's human bone index + uint32_t m_ID; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_X); + TRANSFER(m_ParentHumanIndex); + TRANSFER(m_ID); + } + }; +} + +} diff --git a/Runtime/mecanim/human/human.cpp b/Runtime/mecanim/human/human.cpp new file mode 100644 index 0000000..4e2f8bf --- /dev/null +++ b/Runtime/mecanim/human/human.cpp @@ -0,0 +1,2405 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/skeleton/skeleton.h" +#include "Runtime/mecanim/human/human.h" + +namespace mecanim +{ + +// anonymous namespace to hide data in local file scope +namespace +{ + using namespace human; + + static const int32_t Bone2DoF[kLastBone][3] = + { + { -1, -1, -1 }, // kHips + { kLeftLegDoFStart+kUpperLegFrontBack, kLeftLegDoFStart+kUpperLegInOut, kLeftLegDoFStart+kUpperLegRollInOut }, // kLeftUpperLeg + { kRightLegDoFStart+kUpperLegFrontBack, kRightLegDoFStart+kUpperLegInOut, kRightLegDoFStart+kUpperLegRollInOut }, // kRightUpperLeg + { kLeftLegDoFStart+kLegCloseOpen, -1, kLeftLegDoFStart+kLegRollInOut }, // kLeftLeg + { kRightLegDoFStart+kLegCloseOpen, -1, kRightLegDoFStart+kLegRollInOut }, // kRightLeg + { kLeftLegDoFStart+kFootCloseOpen, kLeftLegDoFStart+kFootInOut, -1 }, // kLeftFoot + { kRightLegDoFStart+kFootCloseOpen, kRightLegDoFStart+kFootInOut, -1 }, // kRightFoot + { kBodyDoFStart+kSpineFrontBack, kBodyDoFStart+kSpineLeftRight, kBodyDoFStart+kSpineRollLeftRight }, // kSpine + { kBodyDoFStart+kChestFrontBack, kBodyDoFStart+kChestLeftRight, kBodyDoFStart+kChestRollLeftRight }, // kChest + { kHeadDoFStart+kNeckFrontBack, kHeadDoFStart+kNeckLeftRight, kHeadDoFStart+kNeckRollLeftRight }, // kNeck + { kHeadDoFStart+kHeadFrontBack, kHeadDoFStart+kHeadLeftRight, kHeadDoFStart+kHeadRollLeftRight }, // kHead + { kLeftArmDoFStart+kShoulderDownUp, kLeftArmDoFStart+kShoulderFrontBack, -1 }, // kLeftShoulder + { kRightArmDoFStart+kShoulderDownUp, kRightArmDoFStart+kShoulderFrontBack, -1 }, // kRightShoulder + { kLeftArmDoFStart+kArmDownUp, kLeftArmDoFStart+kArmFrontBack, kLeftArmDoFStart+kArmRollInOut }, // kLeftArm + { kRightArmDoFStart+kArmDownUp, kRightArmDoFStart+kArmFrontBack, kRightArmDoFStart+kArmRollInOut }, // kRightArm + { kLeftArmDoFStart+kForeArmCloseOpen, -1, kLeftArmDoFStart+kForeArmRollInOut }, // kLeftForeArm + { kRightArmDoFStart+kForeArmCloseOpen, -1, kRightArmDoFStart+kForeArmRollInOut }, // kRightForeArm + { kLeftArmDoFStart+kHandDownUp,kLeftArmDoFStart+kHandInOut, -1 }, // kLeftHand + { kRightArmDoFStart+kHandDownUp,kRightArmDoFStart+kHandInOut, -1 }, // kRightHand + { kLeftLegDoFStart+kToesUpDown, -1, -1}, // kLeftToes + { kRightLegDoFStart+kToesUpDown, -1, -1 }, // kRightToes + { kHeadDoFStart+kLeftEyeDownUp, kHeadDoFStart+kLeftEyeLeftRight,-1 }, // LeftEye + { kHeadDoFStart+kRightEyeDownUp, kHeadDoFStart+kRightEyeLeftRight,-1 }, // RightEye + { kHeadDoFStart+kJawDownUp, kHeadDoFStart+kJawLeftRight,-1 } // Jaw + }; + + static const float HumanBoneDefaultMass[kLastBone] = + { + 12.0f, // kHips + 10.0f, // kLeftUpperLeg + 10.0f, // kRightUpperLeg + 4.0f, // kLeftLowerLeg + 4.0f, // kRightLowerLeg + 0.8f, // kLeftFoot + 0.8f, // kRightFoot + 2.5f, // kSpine + 24.0f, // kChest + 1.0f, // kNeck + 4.0f, // kHead + 0.5f, // kLeftShoulder + 0.5f, // kRightShoulder + 2.0f, // kLeftUpperArm + 2.0f, // kRightUpperArm + 1.5f, // kLeftLowerArm + 1.5f, // kRightLowerArm + 0.5f, // kLeftHand + 0.5f, // kRightHand + 0.2f, // kLeftToes + 0.2f, // kRightToes + 0.0f, // LeftEye + 0.0f, // RightEye + 0.0f // Jaw + }; + + static const int32_t BoneMirror[kLastBone] = + { + kHips, // kHips + kRightUpperLeg, // kLeftUpperLeg + -kLeftUpperLeg, // kRightUpperLeg + kRightLowerLeg, // kLeftLowerLeg + -kLeftLowerLeg, // kRightLowerLeg + kRightFoot, // kLeftFoot + -kLeftFoot, // kRightFoot + kSpine, // kSpine + kChest, // kChest + kNeck, // kNeck + kHead, // kHead + kRightShoulder, // kLeftShoulder + -kLeftShoulder, // kRightShoulder + kRightUpperArm, // kLeftUpperArm + -kLeftUpperArm, // kRightUpperArm + kRightLowerArm, // kLeftLowerArm + -kLeftLowerArm, // kRightLowerArm + kRightHand, // kLeftHand + -kLeftHand, // kRightHand + kRightToes, // kLeftToes + -kLeftToes, // kRightToes + kRightEye, // kLeftEye + -kLeftEye, // kRightEye + kJaw, // kJaw + }; + + static const float BodyDoFMirror[kLastBodyDoF] = + { + +1.0f, // kSpineFrontBack = 0, + -1.0f, // kSpineLeftRight, + -1.0f, // kSpineRollLeftRight, + +1.0f, // kChestFrontBack, + -1.0f, // kChestLeftRight, + -1.0f // kChestRollLeftRight, + }; + + static const float HeadDoFMirror[kLastHeadDoF] = + { + +1.0f, // kNeckFrontBack = 0, + -1.0f, // kNeckLeftRight, + -1.0f, // kNeckRollLeftRight, + +1.0f, // kHeadFrontBack, + -1.0f, // kHeadLeftRight, + -1.0f, // kHeadRollLeftRight, + +1.0f, // kLeftEyeDownUp, + -1.0f, // kLeftEyeLeftRight, + +1.0f, // kRightEyeDownUp, + -1.0f, // kRightEyeLeftRight, + +1.0f, // kJawDownUp, + -1.0f // kJawLeftRight, + }; + + static const int32_t BoneChildren[kLastBone][4] = + { + { 3,kLeftUpperLeg, kRightUpperLeg, kSpine },// kHips + { 1,kLeftLowerLeg }, // kLeftUpperLeg + { 1,kRightLowerLeg }, // kRightUpperLeg + { 1,kLeftFoot }, // kLeftLowerLeg + { 1,kRightFoot }, // kRightLowerLeg + { 1,kLeftToes }, // kLeftFoot + { 1,kRightToes }, // kRightFoot + { 1,kChest }, // kSpine + { 3,kNeck, kLeftShoulder, kRightShoulder }, // kChest + { 1,kHead }, // kNeck + { 3,kLeftEye,kRightEye,kJaw }, // kHead + { 1,kLeftUpperArm }, // kLeftShoulder + { 1,kRightUpperArm }, // kRightShoulder + { 1,kLeftLowerArm }, // kLeftUpperArm + { 1,kRightLowerArm }, // kRightUpperArm + { 1,kLeftHand }, // kLeftLowerArm + { 1,kRightHand }, // kRightLowerArm + { 0 }, // kLeftHand + { 0 }, // kRightHand + { 0 }, // kLeftToes + { 0 }, // kRightToes + { 0 }, // kLeftEye + { 0 }, // kRightEye + { 0 } // kJaw + }; + + static const int32_t DoF2Bone[human::kLastDoF] = { + kSpine, + kSpine, + kSpine, + kChest, + kChest, + kChest, + kNeck, + kNeck, + kNeck, + kHead, + kHead, + kHead, + kLeftEye, + kLeftEye, + kRightEye, + kRightEye, + kJaw, + kJaw, + kLeftUpperLeg, + kLeftUpperLeg, + kLeftUpperLeg, + kLeftLowerLeg, + kLeftLowerLeg, + kLeftFoot, + kLeftFoot, + kLeftToes, + kRightUpperLeg, + kRightUpperLeg, + kRightUpperLeg, + kRightLowerLeg, + kRightLowerLeg, + kRightFoot, + kRightFoot, + kRightToes, + kLeftShoulder, + kLeftShoulder, + kLeftUpperArm, + kLeftUpperArm, + kLeftUpperArm, + kLeftLowerArm, + kLeftLowerArm, + kLeftHand, + kLeftHand, + kRightShoulder, + kRightShoulder, + kRightUpperArm, + kRightUpperArm, + kRightUpperArm, + kRightLowerArm, + kRightLowerArm, + kRightHand, + kRightHand + }; + + static const int32_t DoF2BoneDoFIndex[human::kLastDoF] = { + 2, // kSpine, + 1, // kSpine, + 0, // kSpine, + 2, // kChest, + 1, // kChest, + 0, // kChest, + 2, // kNeck, + 1, // kNeck, + 0, // kNeck, + 2, // kHead, + 1, // kHead, + 0, // kHead, + 2, // kLeftEye, + 1, // kLeftEye, + 2, // kRightEye, + 1, // kRightEye, + 2, // kJaw, + 1, // kJaw, + 2, // kLeftUpperLeg, + 1, // kLeftUpperLeg, + 0, // kLeftUpperLeg, + 2, // kLeftLowerLeg, + 0, // kLeftLowerLeg, + 2, // kLeftFoot, + 1, // kLeftFoot, + 2, // kLeftToes, + 2, // kRightUpperLeg, + 1, // kRightUpperLeg, + 0, // kRightUpperLeg, + 2, // kRightLowerLeg, + 0, // kRightLowerLeg, + 2, // kRightFoot, + 1, // kRightFoot, + 2, // kRightToes, + 2, // kLeftShoulder, + 1, // kLeftShoulder, + 2, // kLeftUpperArm, + 1, // kLeftUpperArm, + 0, // kLeftUpperArm, + 2, // kLeftLowerArm, + 0, // kLeftLowerArm, + 2, // kLeftHand, + 1, // kLeftHand, + 2, // kRightShoulder, + 1, // kRightShoulder, + 2, // kRightUpperArm, + 1, // kRightUpperArm, + 0, // kRightUpperArm, + 2, // kRightLowerArm, + 0, // kRightLowerArm, + 2, // kRightHand, + 1, // kRightHand + }; + + const static float ATTRIBUTE_ALIGN(ALIGN4F) goalOrientationOffsetArray[kLastGoal][4] = {{0.5f,-0.5f,0.5f,0.5f},{0.5f,-0.5f,0.5f,0.5f},{0.707107f,0,0.707107f,0},{0,0.707107f,0,0.707107f}}; +} + +namespace human +{ + bool RequiredBone(uint32_t aBoneIndex) + { + static bool requiredBone[kLastBone] = { + true, //kHips + true, //kLeftUpperLeg + true, //kRightUpperLeg + true, //kLeftLowerLeg + true, //kRightLowerLeg + true, //kLeftFoot + true, //kRightFoot + true, //kSpine + false, //kChest + false, //kNeck + true, //kHead + false, //kLeftShoulder + false, //kRightShoulder + true, //kLeftUpperArm + true, //kRightUpperArm + true, //kLeftLowerArm + true, //kRightLowerArm + true, //kLeftHand + true, //kRightHand + false, //kLeftToes + false, //kRightToes + false, //kLeftEye, + false, //kRightEye, + false //kJaw, + }; + + return requiredBone[aBoneIndex]; + } + + const char* BoneName(uint32_t aBoneIndex) + { + static const char* boneName[kLastBone] = { + "Hips", + "LeftUpperLeg", + "RightUpperLeg", + "LeftLowerLeg", + "RightLowerLeg", + "LeftFoot", + "RightFoot", + "Spine", + "Chest", + "Neck", + "Head", + "LeftShoulder", + "RightShoulder", + "LeftUpperArm", + "RightUpperArm", + "LeftLowerArm", + "RightLowerArm", + "LeftHand", + "RightHand", + "LeftToes", + "RightToes", + "LeftEye", + "RightEye", + "Jaw" + }; + + return boneName[aBoneIndex]; + } + + const char* MuscleName(uint32_t aBoneIndex) + { + static const char* muscleName[human::kLastDoF] = { + + "Spine Front-Back", + "Spine Left-Right", + "Spine Twist Left-Right", + "Chest Front-Back", + "Chest Left-Right", + "Chest Twist Left-Right", + + "Neck Nod Down-Up", + "Neck Tilt Left-Right", + "Neck Turn Left-Right", + "Head Nod Down-Up", + "Head Tilt Left-Right", + "Head Turn Left-Right", + + "Left Eye Down-Up", + "Left Eye In-Out", + "Right Eye Down-Up", + "Right Eye In-Out", + + "Jaw Close", + "Jaw Left-Right", + + "Left Upper Leg Front-Back", + "Left Upper Leg In-Out", + "Left Upper Leg Twist In-Out", + "Left Lower Leg Stretch", + "Left Lower Leg Twist In-Out", + "Left Foot Up-Down", + "Left Foot Twist In-Out", + "Left Toes Up-Down", + + "Right Upper Leg Front-Back", + "Right Upper Leg In-Out", + "Right Upper Leg Twist In-Out", + "Right Lower Leg Stretch", + "Right Lower Leg Twist In-Out", + "Right Foot Up-Down", + "Right Foot Twist In-Out", + "Right Toes Up-Down", + + "Left Shoulder Down-Up", + "Left Shoulder Front-Back", + "Left Arm Down-Up", + "Left Arm Front-Back", + "Left Arm Twist In-Out", + "Left Forearm Stretch", + "Left Forearm Twist In-Out", + "Left Hand Down-Up", + "Left Hand In-Out", + + "Right Shoulder Down-Up", + "Right Shoulder Front-Back", + "Right Arm Down-Up", + "Right Arm Front-Back", + "Right Arm Twist In-Out", + "Right Forearm Stretch", + "Right Forearm Twist In-Out", + "Right Hand Down-Up", + "Right Hand In-Out" + }; + + return muscleName[aBoneIndex]; + } + + bool MaskHasLegs(const HumanPoseMask& mask) + { + for(int dofIter = 0; dofIter < mecanim::human::kLastLegDoF; dofIter++) + { + if(!mask.test(mecanim::human::kMaskDoFStartIndex+mecanim::human::kLeftLegDoFStart+dofIter)) + return false; + if(!mask.test(mecanim::human::kMaskDoFStartIndex+mecanim::human::kRightLegDoFStart+dofIter)) + return false; + } + + return true; + } + + int32_t MuscleFromBone(int32_t aBoneIndex, int32_t aDoFIndex) + { + return Bone2DoF[aBoneIndex][2-aDoFIndex]; + } + + int32_t BoneFromMuscle(int32_t aDoFIndex) + { + return DoF2Bone[aDoFIndex]; + } + + HumanPoseMask FullBodyMask() + { + return HumanPoseMask(~HumanPoseMask::type(0)); + } + + skeleton::SetupAxesInfo const& GetAxeInfo(uint32_t index) + { + const static skeleton::SetupAxesInfo setupAxesInfoArray[kLastBone] = + { + {{0,0,0,1},{-1,0,0,0},{-40,-40,-40},{40,40,40},{1,1,1,1},math::kZYRoll,0}, // kHips, + {{-0.268f,0,0,1},{1,0,0,0},{-60,-60,-90},{60,60,50},{1,1,1,1},math::kZYRoll,0}, // kLeftUpperLeg, + {{-0.268f,0,0,1},{1,0,0,0},{-60,-60,-90},{60,60,50},{-1,-1,1,1},math::kZYRoll,0}, // kRightUpperLeg, + {{0.839f,0,0,1},{1,0,0,0},{-90,0,-80},{90,0,80},{1,1,-1,1},math::kZYRoll,0}, // kLeftLeg, + {{0.839f,0,0,1},{1,0,0,0},{-90,0,-80},{90,0,80},{-1,1,-1,1},math::kZYRoll,0}, // kRightLeg, + {{0,0,0,1},{1,0,0,0},{0,-30,-50},{0,30,50},{1,1,1,1},math::kZYRoll,-2}, // kLeftFoot, + {{0,0,0,1},{1,0,0,0},{0,-30,-50},{0,30,50},{1,-1,1,1},math::kZYRoll,-2}, // kRightFoot, + {{0,0,0,1},{-1,0,0,0},{-40,-40,-40},{40,40,40},{1,1,1,1},math::kZYRoll,0}, // kSpine, + {{0,0,0,1},{-1,0,0,0},{-40,-40,-40},{40,40,40},{1,1,1,1},math::kZYRoll,0}, // kChest, + {{0,0,0,1},{-1,0,0,0},{-40,-40,-40},{40,40,40},{1,1,1,1},math::kZYRoll,0}, // kNeck, + {{0,0,0,1},{-1,0,0,0},{-40,-40,-40},{40,40,40},{1,1,1,1},math::kZYRoll,2}, // kHead, + {{0,0,0,1},{0,0,1,0},{0,-15,-15},{0,15,30},{1,1,-1,1},math::kZYRoll,0}, // kLeftShoulder, + {{0,0,0,1},{0,0,1,0},{0,-15,-15},{0,15,30},{1,1,1,1},math::kZYRoll,0}, // kRightShoulder, + {{0,0.268f,0.364f,1},{0,0,1,0},{-90,-100,-60},{90,100,100},{1,1,-1,1},math::kZYRoll,0}, // kLeftArm, + {{0,-0.268f,-0.364f,1},{0,0,1,0},{-90,-100,-60},{90,100,100},{-1,1,1,1},math::kZYRoll,0}, // kRightArm, + {{0,0.839f,0,1},{0,1,0,0},{-90,0,-80},{90,0,80},{1,1,-1,1},math::kZYRoll,0}, // kLeftForeArm, + {{0,-0.839f,0,1},{0,1,0,0},{-90,0,-80},{90,0,80},{-1,1,1,1},math::kZYRoll,0}, // kRightForeArm, + {{0,0,0,1},{0,0,1,0},{0,-40,-80},{0,40,80},{1,1,-1,1},math::kZYRoll,0}, // kLeftHand, + {{0,0,0,1},{0,0,1,0},{0,-40,-80},{0,40,80},{1,1,1,1},math::kZYRoll,0}, // kRightHand, + {{0,0,0,1},{1,0,0,0},{0,0,-50},{0,0,50},{1,1,1,1},math::kZYRoll,3}, // kLeftToes, + {{0,0,0,1},{1,0,0,0},{0,0,-50},{0,0,50},{1,1,1,1},math::kZYRoll,3}, // kRightToes, + {{0,0,0,1},{1,0,0,0},{0,-20,-10},{0,20,15},{1,1,-1,1},math::kZYRoll,3}, // kLeftEye, + {{0,0,0,1},{1,0,0,0},{0,-20,-10},{0,20,15},{1,-1,-1,1},math::kZYRoll,3}, // kRightEye, + {{0.09f,0,0,1},{1,0,0,0},{0,-10,-10},{0,10,10},{1,1,-1,1},math::kZYRoll,3} // kJaw, + }; + + return setupAxesInfoArray[index]; + + } + + + Human::Human() : m_HandlesCount(0), + m_HasLeftHand(false), + m_HasRightHand(false), + m_ColliderCount(0), + m_Scale(1), + m_RootX(math::xformIdentity()), + m_ArmTwist(0.5f), + m_ForeArmTwist(0.5f), + m_UpperLegTwist(0.5f), + m_LegTwist(0.5f), + m_ArmStretch(0.05f), + m_LegStretch(0.05f), + m_FeetSpacing(0.0f) + { + int32_t i; + + float mass = 0; + + for(i = 0; i < kLastBone; i++) + { + m_HumanBoneIndex[i] = -1; + m_HumanBoneMass[i] = HumanBoneDefaultMass[i]; + mass += m_HumanBoneMass[i]; + m_ColliderIndex[i] = -1; + } + + for(i = 0; i < kLastBone; i++) + { + m_HumanBoneMass[i] /= mass; + } + } + + Human* CreateHuman(skeleton::Skeleton *apSkeleton, skeleton::SkeletonPose *apSkeletonPose, uint32_t aHandlesCount, uint32_t aColliderCount, memory::Allocator& arAlloc) + { + Human* human = arAlloc.Construct<Human>(); + + human->m_Skeleton = apSkeleton; + human->m_SkeletonPose = apSkeletonPose; + human->m_Handles = arAlloc.ConstructArray<Handle>(aHandlesCount); + human->m_HandlesCount = aHandlesCount; + human->m_ColliderArray = arAlloc.ConstructArray<math::Collider>(aColliderCount); + human->m_ColliderCount = aColliderCount; + + memset(human->m_HumanBoneIndex, -1, sizeof(int32_t)*kLastBone); + memset(human->m_ColliderIndex, -1, sizeof(int32_t)*kLastBone); + + human->m_HasLeftHand = false; + human->m_HasRightHand = false; + + human->m_Scale = 1; + + return human; + } + + void DestroyHuman(Human *apHuman, memory::Allocator& arAlloc) + { + if(apHuman) + { + arAlloc.Deallocate(apHuman->m_Handles); + arAlloc.Deallocate(apHuman->m_ColliderArray); + + arAlloc.Deallocate(apHuman); + } + } + + HumanPose::HumanPose() + { + int32_t i; + + for(i = 0; i < kLastDoF; i++) + { + m_DoFArray[i] = 0; + } + + m_LookAtPosition = math::float4::zero(); + m_LookAtWeight = math::float4::zero(); + } + + void HumanAdjustMass(Human *apHuman) + { + if(apHuman->m_HumanBoneIndex[kNeck] < 0) + { + apHuman->m_HumanBoneMass[kChest] += apHuman->m_HumanBoneMass[kNeck]; + apHuman->m_HumanBoneMass[kNeck] = 0; + } + + if(apHuman->m_HumanBoneIndex[kLeftShoulder] < 0) + { + apHuman->m_HumanBoneMass[kChest] += apHuman->m_HumanBoneMass[kLeftShoulder]; + apHuman->m_HumanBoneMass[kLeftShoulder] = 0; + } + + if(apHuman->m_HumanBoneIndex[kRightShoulder] < 0) + { + apHuman->m_HumanBoneMass[kChest] += apHuman->m_HumanBoneMass[kRightShoulder]; + apHuman->m_HumanBoneMass[kRightShoulder] = 0; + } + + if(apHuman->m_HumanBoneIndex[kChest] < 0) + { + apHuman->m_HumanBoneMass[kSpine] += apHuman->m_HumanBoneMass[kChest]; + apHuman->m_HumanBoneMass[kChest] = 0; + } + + if(apHuman->m_HumanBoneIndex[kLeftToes] < 0) + { + apHuman->m_HumanBoneMass[kLeftFoot] += apHuman->m_HumanBoneMass[kLeftToes]; + apHuman->m_HumanBoneMass[kLeftToes] = 0; + } + + if(apHuman->m_HumanBoneIndex[kRightToes] < 0) + { + apHuman->m_HumanBoneMass[kRightFoot] += apHuman->m_HumanBoneMass[kRightToes]; + apHuman->m_HumanBoneMass[kRightToes] = 0; + } + } + + void HumanSetupAxes(Human *apHuman, skeleton::SkeletonPose const *apSkeletonPoseGlobal) + { + apHuman->m_RootX = math::xformIdentity(); + apHuman->m_RootX = HumanComputeRootXform(apHuman,apSkeletonPoseGlobal); + apHuman->m_Scale = apHuman->m_RootX.t.y().tofloat(); + + skeleton::SkeletonPoseComputeLocal(apHuman->m_Skeleton.Get(), apSkeletonPoseGlobal, apHuman->m_SkeletonPose.Get()); + + int32_t i; + + for(i = 0; i < kLastBone; i++) + { + int32_t skBoneIndex = apHuman->m_HumanBoneIndex[i]; + + int32_t skAxisBoneId = -1; + float len = 1.0f; + + switch(i) + { + case kLeftEye: + case kRightEye: + case kJaw: + len = 0.1f; + break; + + case kHead: + if(apHuman->m_HumanBoneIndex[kNeck] >= 0) + { + skAxisBoneId = apHuman->m_HumanBoneIndex[kNeck]; + len = -1.0f; + } + else if(apHuman->m_HumanBoneIndex[kChest] >= 0) + { + skAxisBoneId = apHuman->m_HumanBoneIndex[kChest]; + len = -0.5f; + } + else + { + skAxisBoneId = apHuman->m_HumanBoneIndex[kSpine]; + len = -0.25f; + } + break; + + case kLeftFoot: + len = -apSkeletonPoseGlobal->m_X[skBoneIndex].t.y().tofloat(); + break; + + case kRightFoot: + len = -apSkeletonPoseGlobal->m_X[skBoneIndex].t.y().tofloat(); + break; + + case kLeftHand: + skAxisBoneId = apHuman->m_HumanBoneIndex[kLeftLowerArm]; + len = -0.5f; + break; + + case kRightHand: + skAxisBoneId = apHuman->m_HumanBoneIndex[kRightLowerArm]; + len = -0.5f; + break; + + case kLeftToes: + skAxisBoneId = apHuman->m_HumanBoneIndex[kLeftFoot]; + len = 0.5f; + break; + + case kRightToes: + skAxisBoneId = apHuman->m_HumanBoneIndex[kRightFoot]; + len = 0.5f; + break; + + case kHips: + skAxisBoneId = apHuman->m_HumanBoneIndex[kSpine]; + break; + + case kLeftUpperLeg: + skAxisBoneId = apHuman->m_HumanBoneIndex[kLeftLowerLeg]; + break; + + case kRightUpperLeg: + skAxisBoneId = apHuman->m_HumanBoneIndex[kRightLowerLeg]; + break; + + case kLeftLowerLeg: + skAxisBoneId = apHuman->m_HumanBoneIndex[kLeftFoot]; + break; + + case kRightLowerLeg: + skAxisBoneId = apHuman->m_HumanBoneIndex[kRightFoot]; + break; + + case kSpine: + if(apHuman->m_HumanBoneIndex[kChest] >= 0) + { + skAxisBoneId = apHuman->m_HumanBoneIndex[kChest]; + } + else if(apHuman->m_HumanBoneIndex[kNeck] >= 0) + { + skAxisBoneId = apHuman->m_HumanBoneIndex[kNeck]; + } + else + { + skAxisBoneId = apHuman->m_HumanBoneIndex[kHead]; + } + break; + + case kChest: + if(apHuman->m_HumanBoneIndex[kNeck] >= 0) + { + skAxisBoneId = apHuman->m_HumanBoneIndex[kNeck]; + } + else + { + skAxisBoneId = apHuman->m_HumanBoneIndex[kHead]; + } + break; + + case kNeck: + skAxisBoneId = apHuman->m_HumanBoneIndex[kHead]; + break; + + case kLeftShoulder: + skAxisBoneId = apHuman->m_HumanBoneIndex[kLeftUpperArm]; + break; + + case kRightShoulder: + skAxisBoneId = apHuman->m_HumanBoneIndex[kRightUpperArm]; + break; + + case kLeftUpperArm: + skAxisBoneId = apHuman->m_HumanBoneIndex[kLeftLowerArm]; + break; + + case kRightUpperArm: + skAxisBoneId = apHuman->m_HumanBoneIndex[kRightLowerArm]; + break; + + case kLeftLowerArm: + skAxisBoneId = apHuman->m_HumanBoneIndex[kLeftHand]; + break; + + case kRightLowerArm: + skAxisBoneId = apHuman->m_HumanBoneIndex[kRightHand]; + break; + }; + + if(skBoneIndex >= 0) + { + skeleton::SetupAxes(apHuman->m_Skeleton.Get(), apSkeletonPoseGlobal, GetAxeInfo(i), skBoneIndex,skAxisBoneId,true,len); + } + } + } + + void HumanSetupCollider(Human *apHuman, skeleton::SkeletonPose const *apSkeletonPoseGlobal) + { + //float refLen = apSkeletonPoseGlobal->m_X[apHuman->m_HumanBoneIndex[kHead]].t.y(); + + float hipsWidth = math::length(apSkeletonPoseGlobal->m_X[apHuman->m_HumanBoneIndex[kLeftUpperLeg]].t - apSkeletonPoseGlobal->m_X[apHuman->m_HumanBoneIndex[kRightUpperLeg]].t).tofloat(); + float shouldersWidth = math::length(apSkeletonPoseGlobal->m_X[apHuman->m_HumanBoneIndex[kLeftUpperArm]].t - apSkeletonPoseGlobal->m_X[apHuman->m_HumanBoneIndex[kRightUpperArm]].t).tofloat(); + + int32_t colliderIndex = 0; + int32_t boneIndex; + + for(boneIndex = 0; boneIndex < kLastBone; boneIndex++) + { + int32_t skIndex = apHuman->m_HumanBoneIndex[boneIndex]; + + if(skIndex >= 0) + { + apHuman->m_ColliderIndex[boneIndex] = colliderIndex; + + math::Axes axes = GetAxes(apHuman,boneIndex); + apHuman->m_ColliderArray[colliderIndex].m_X.s.x() = axes.m_Length; + apHuman->m_ColliderArray[colliderIndex].m_X.t.x() = math::float1(0.5f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + + if(boneIndex == kHips) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kCube; + //apHuman->m_ColliderArray[colliderIndex].m_X.s.x() *= math::float1(3.0f); + apHuman->m_ColliderArray[colliderIndex].m_X.t.x() = math::float1::zero(); + + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(1.5f * hipsWidth); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(1.0f * hipsWidth); + + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kIgnored; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kIgnored; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kIgnored; + } + else if(boneIndex == kSpine) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kSphere; + + //apHuman->m_ColliderArray[colliderIndex].m_X.s.x() *= math::float1(1.5f); + apHuman->m_ColliderArray[colliderIndex].m_X.t.x() = math::float1(0.5f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(1.2f * hipsWidth); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(0.8f * hipsWidth); + + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = -7.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 7.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitY = 7.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitZ = 7.0f; + } + else if(boneIndex == kChest) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kCube; + + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(1.0f * shouldersWidth); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(0.6f * shouldersWidth); + + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = -11.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 11.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitY = 11.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitZ = 11.0f; + } + else if(boneIndex == kNeck) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kSphere; + + //apHuman->m_ColliderArray[colliderIndex].m_X.s.x() *= math::float1(1.5f); + apHuman->m_ColliderArray[colliderIndex].m_X.t.y() = math::float1(0.1f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(0.33f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(0.33f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = -10.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 10.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitY = 20.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitZ = 5.0f; + } + else if(boneIndex == kHead) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kSphere; + + apHuman->m_ColliderArray[colliderIndex].m_X.s.x() = math::float1(0.6f * shouldersWidth); + apHuman->m_ColliderArray[colliderIndex].m_X.t.y() = math::float1(0.2f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(0.4f * shouldersWidth); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(0.45f * shouldersWidth); + + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = -5.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 8.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitY = 20.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitZ = 10.0f; + } + else if(boneIndex == kLeftShoulder || boneIndex == kRightShoulder) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kNone; + + //apHuman->m_ColliderArray[colliderIndex].m_X.s.x() *= math::float1(1.2f); + apHuman->m_ColliderArray[colliderIndex].m_X.t.x() = math::float1(0.5f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(0.1f * shouldersWidth); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(0.2f * shouldersWidth); + + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kIgnored; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kIgnored; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kIgnored; + } + else if(boneIndex == kLeftHand || boneIndex == kRightHand) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kCube; + + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(0.5f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(0.2f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = -30.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 30.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitY = 40.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitZ = 25.0f; + } + else if(boneIndex == kLeftFoot || boneIndex == kRightFoot) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kCube; + + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(0.4f * shouldersWidth); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(0.85f * shouldersWidth); + apHuman->m_ColliderArray[colliderIndex].m_X.t.y() = math::float1(-0.25f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.y(); + + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = -45.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 20.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitY = 30.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitZ = 50.0f; + } + else if(boneIndex == kLeftToes || boneIndex == kRightToes) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kNone; + + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(0.4f * shouldersWidth); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(0.2f * shouldersWidth); + apHuman->m_ColliderArray[colliderIndex].m_X.t.y() = math::float1(-0.5f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.y(); + + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kIgnored; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kIgnored; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kIgnored; + } + else if( boneIndex == kLeftUpperArm || + boneIndex == kLeftLowerArm || + boneIndex == kRightUpperArm || + boneIndex == kRightLowerArm) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kCapsule; + + //apHuman->m_ColliderArray[colliderIndex].m_X.s.x() *= math::float1(1.2f); + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(0.25f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(0.25f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + + if(boneIndex == kLeftUpperArm || boneIndex == kRightUpperArm) + { + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = -100.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 100.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitY = 20.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitZ = 45.0f; + } + else if (boneIndex == kLeftLowerArm || boneIndex == kRightLowerArm) + { + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kLocked; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kLimited; + if (boneIndex == kLeftLowerArm) + { + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = -130.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 0.0f; + } + else if (boneIndex == kRightLowerArm) + { + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = 0.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 130.0f; + } + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitY = 5.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitZ = 20.0f; + } + } + else if( boneIndex == kLeftLowerLeg || + boneIndex == kLeftUpperLeg || + boneIndex == kRightLowerLeg || + boneIndex == kRightUpperLeg) + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kCapsule; + + //apHuman->m_ColliderArray[colliderIndex].m_X.s.x() *= math::float1(1.2f); + apHuman->m_ColliderArray[colliderIndex].m_X.s.z() = math::float1(0.175f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + apHuman->m_ColliderArray[colliderIndex].m_X.s.y() = math::float1(0.175f) * apHuman->m_ColliderArray[colliderIndex].m_X.s.x(); + + if (boneIndex == kLeftLowerLeg || boneIndex == kRightLowerLeg) + { + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kLocked; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = 0.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 130.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitY = 0.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitZ = 10.0f; + } + else if(boneIndex == kLeftUpperLeg || boneIndex == kRightUpperLeg) + { + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kLimited; + apHuman->m_ColliderArray[colliderIndex].m_MinLimitX = -70.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitX = 10.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitY = 45.0f; + apHuman->m_ColliderArray[colliderIndex].m_MaxLimitZ = 60.0f; + } + } + else + { + apHuman->m_ColliderArray[colliderIndex].m_Type = math::kNone; + apHuman->m_ColliderArray[colliderIndex].m_XMotionType = math::kIgnored; + apHuman->m_ColliderArray[colliderIndex].m_YMotionType = math::kIgnored; + apHuman->m_ColliderArray[colliderIndex].m_ZMotionType = math::kIgnored; + } + + colliderIndex++; + } + } + } + + void HumanCopyAxes(Human const *apSrcHuman, Human *apHuman) + { + int32_t i; + + for(i = 0; i < kLastBone; i++) + { + skeleton::Node const * srcNode = apSrcHuman->m_HumanBoneIndex[i] >= 0 ? &apSrcHuman->m_Skeleton->m_Node[apSrcHuman->m_HumanBoneIndex[i]] : 0; + skeleton::Node const * node = apHuman->m_HumanBoneIndex[i] >= 0 ? &apHuman->m_Skeleton->m_Node[apHuman->m_HumanBoneIndex[i]] : 0; + + if(srcNode != 0 && node != 0 && srcNode->m_AxesId != -1 && node->m_AxesId != -1) + { + apHuman->m_Skeleton->m_AxesArray[node->m_AxesId] = apSrcHuman->m_Skeleton->m_AxesArray[srcNode->m_AxesId]; + } + } + } + + math::Axes GetAxes(Human const *apHuman, int32_t aBoneIndex) + { + math::Axes ret; + + int32_t skIndex = apHuman->m_HumanBoneIndex[aBoneIndex]; + + if(skIndex >= 0) + { + int32_t axesIndex = apHuman->m_Skeleton->m_Node[skIndex].m_AxesId; + + if(axesIndex >= 0) + { + ret = apHuman->m_Skeleton->m_AxesArray[axesIndex]; + } + } + + return ret; + } + + void GetMuscleRange(Human const *apHuman, int32_t aDoFIndex, float &aMin, float &aMax) + { + math::Axes axes = GetAxes(apHuman,DoF2Bone[aDoFIndex]); + + switch(DoF2BoneDoFIndex[aDoFIndex]) + { + case 0: aMin = axes.m_Limit.m_Min.x().tofloat(); aMax = axes.m_Limit.m_Max.x().tofloat(); break; + case 1: aMin = axes.m_Limit.m_Min.y().tofloat(); aMax = axes.m_Limit.m_Max.y().tofloat(); break; + case 2: aMin = axes.m_Limit.m_Min.z().tofloat(); aMax = axes.m_Limit.m_Max.z().tofloat(); break; + } + } + + math::float4 AddAxis(Human const *apHuman, int32_t aIndex, math::float4 const &arQ) + { + math::Axes cAxes = apHuman->m_Skeleton->m_AxesArray[apHuman->m_Skeleton->m_Node[aIndex].m_AxesId]; + return math::normalize(math::quatMul(arQ,cAxes.m_PostQ)); + } + + math::float4 RemoveAxis(Human const *apHuman, int32_t aIndex, const math::float4 &arQ) + { + math::Axes cAxes = apHuman->m_Skeleton->m_AxesArray[apHuman->m_Skeleton->m_Node[aIndex].m_AxesId]; + return math::normalize(math::quatMul(arQ,math::quatConj(cAxes.m_PostQ))); + } + + math::xform NormalizedHandleX(Human const *apHuman, int32_t aHandleIndex) + { + int32_t pIndex = apHuman->m_HumanBoneIndex[apHuman->m_Handles[aHandleIndex].m_ParentHumanIndex]; + + math::xform px = apHuman->m_SkeletonPose->m_X[pIndex]; + math::xform hx = math::xformMul(px,apHuman->m_Handles[aHandleIndex].m_X); + + px.q = AddAxis(apHuman,pIndex,px.q); + px.s = math::float4::one(); + + return math::xformInvMul(px,hx); + } + + void HumanPoseAdjustForMissingBones(Human const *apHuman, HumanPose *apHumanPose) + { + if(apHuman->m_HumanBoneIndex[kNeck] < 0) + { + apHumanPose->m_DoFArray[kHeadDoFStart+kHeadFrontBack] += apHumanPose->m_DoFArray[kHeadDoFStart+kNeckFrontBack]; + apHumanPose->m_DoFArray[kHeadDoFStart+kNeckFrontBack] = 0; + + apHumanPose->m_DoFArray[kHeadDoFStart+kHeadLeftRight] += apHumanPose->m_DoFArray[kHeadDoFStart+kNeckLeftRight]; + apHumanPose->m_DoFArray[kHeadDoFStart+kNeckLeftRight] = 0; + + apHumanPose->m_DoFArray[kHeadDoFStart+kHeadRollLeftRight] += apHumanPose->m_DoFArray[kHeadDoFStart+kNeckRollLeftRight]; + apHumanPose->m_DoFArray[kHeadDoFStart+kNeckRollLeftRight] = 0; + } + + if(apHuman->m_HumanBoneIndex[kChest] < 0) + { + apHumanPose->m_DoFArray[kBodyDoFStart+kSpineFrontBack] += apHumanPose->m_DoFArray[kBodyDoFStart+kChestFrontBack]; + apHumanPose->m_DoFArray[kBodyDoFStart+kChestFrontBack] = 0; + + apHumanPose->m_DoFArray[kBodyDoFStart+kSpineLeftRight] += apHumanPose->m_DoFArray[kBodyDoFStart+kChestLeftRight]; + apHumanPose->m_DoFArray[kBodyDoFStart+kChestLeftRight] = 0; + + apHumanPose->m_DoFArray[kBodyDoFStart+kSpineRollLeftRight] += apHumanPose->m_DoFArray[kBodyDoFStart+kChestRollLeftRight]; + apHumanPose->m_DoFArray[kBodyDoFStart+kChestRollLeftRight] = 0; + } + + if(apHuman->m_HumanBoneIndex[kLeftShoulder] < 0) + { + apHumanPose->m_DoFArray[kLeftArmDoFStart+kArmDownUp] += (30.0f/200.0f) * apHumanPose->m_DoFArray[kLeftArmDoFStart+kShoulderDownUp]; + apHumanPose->m_DoFArray[kLeftArmDoFStart+kShoulderDownUp] = 0; + + apHumanPose->m_DoFArray[kLeftArmDoFStart+kArmFrontBack] += (45.0f/160.0f) * apHumanPose->m_DoFArray[kLeftArmDoFStart+kShoulderFrontBack]; + apHumanPose->m_DoFArray[kLeftArmDoFStart+kShoulderFrontBack] = 0; + } + + if(apHuman->m_HumanBoneIndex[kRightShoulder] < 0) + { + apHumanPose->m_DoFArray[kRightArmDoFStart+kArmDownUp] += (30.0f/200.0f) * apHumanPose->m_DoFArray[kRightArmDoFStart+kShoulderDownUp]; + apHumanPose->m_DoFArray[kRightArmDoFStart+kShoulderDownUp] = 0; + + apHumanPose->m_DoFArray[kRightArmDoFStart+kArmFrontBack] += (45.0f/160.0f) * apHumanPose->m_DoFArray[kRightArmDoFStart+kShoulderFrontBack]; + apHumanPose->m_DoFArray[kRightArmDoFStart+kShoulderFrontBack] = 0; + } + } + + void Human2SkeletonPose(Human const *apHuman, HumanPose const *apHumanPose, skeleton::SkeletonPose *apSkeletonPose, int32_t i) + { + if(apHuman->m_HumanBoneIndex[i] != -1) + { + math::float4 xyz = math::cond( math::bool4(Bone2DoF[i][2] != -1,Bone2DoF[i][1] != -1,Bone2DoF[i][0] != -1,false), + math::float4(apHumanPose->m_DoFArray[Bone2DoF[i][2]],apHumanPose->m_DoFArray[Bone2DoF[i][1]],apHumanPose->m_DoFArray[Bone2DoF[i][0]],0), + math::float4::zero()); + + skeleton::SkeletonSetDoF(apHuman->m_Skeleton.Get(),apSkeletonPose,xyz,apHuman->m_HumanBoneIndex[i]); + } + } + + void Human2SkeletonPose(Human const *apHuman, HumanPose const *apHumanPose, skeleton::SkeletonPose *apSkeletonPose) + { + int32_t i; + for(i = 1; i < kLastBone; i++) + { + Human2SkeletonPose(apHuman,apHumanPose,apSkeletonPose,i); + } + + if(apHuman->m_HasLeftHand) + { + hand::Hand2SkeletonPose(apHuman->m_LeftHand.Get(),apHuman->m_Skeleton.Get(),&apHumanPose->m_LeftHandPose,apSkeletonPose); + } + + if(apHuman->m_HasRightHand) + { + hand::Hand2SkeletonPose(apHuman->m_RightHand.Get(),apHuman->m_Skeleton.Get(),&apHumanPose->m_RightHandPose,apSkeletonPose); + } + } + + void Skeleton2HumanPose(Human const *apHuman, skeleton::SkeletonPose const *apSkeletonPose, HumanPose *apHumanPose, int32_t i) + { + if(apHuman->m_HumanBoneIndex[i] != -1) + { + const math::float4 xyz = skeleton::SkeletonGetDoF(apHuman->m_Skeleton.Get(),apSkeletonPose,apHuman->m_HumanBoneIndex[i]); + + if(Bone2DoF[i][2] != -1) apHumanPose->m_DoFArray[Bone2DoF[i][2]] = xyz.x().tofloat(); + if(Bone2DoF[i][1] != -1) apHumanPose->m_DoFArray[Bone2DoF[i][1]] = xyz.y().tofloat(); + if(Bone2DoF[i][0] != -1) apHumanPose->m_DoFArray[Bone2DoF[i][0]] = xyz.z().tofloat(); + } + } + + void Skeleton2HumanPose(Human const *apHuman, skeleton::SkeletonPose const *apSkeletonPose, HumanPose *apHumanPose) + { + int32_t i; + + for(i = 1; i < kLastBone; i++) + { + Skeleton2HumanPose(apHuman,apSkeletonPose,apHumanPose,i); + } + + if(apHuman->m_HasLeftHand) + { + hand::Skeleton2HandPose(apHuman->m_LeftHand.Get(),apHuman->m_Skeleton.Get(),apSkeletonPose,&apHumanPose->m_LeftHandPose); + } + + if(apHuman->m_HasRightHand) + { + hand::Skeleton2HandPose(apHuman->m_RightHand.Get(),apHuman->m_Skeleton.Get(),apSkeletonPose,&apHumanPose->m_RightHandPose); + } + } + + math::float4 HumanComputeBoneMassCenter(Human const *apHuman, skeleton::SkeletonPose const *apSkeletonPose,int32_t aBoneIndex) + { + math::float4 ret( math::float4::zero() ); + + switch(aBoneIndex) + { + case kHips: + ret = math::float1(1.0f/3.0f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftUpperLeg]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightUpperLeg]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kSpine]].t); + break; + + case kSpine: + if(apHuman->m_HumanBoneIndex[kChest] >= 0) + { + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kSpine]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kChest]].t); + } + else + { + ret = math::float1(0.1f) * apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kSpine]].t + math::float1(0.9f * 0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftUpperArm]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightUpperArm]].t); + } + break; + + case kChest: + if(apHuman->m_HumanBoneIndex[kNeck] >= 0 && apHuman->m_HumanBoneIndex[kLeftShoulder] >= 0 && apHuman->m_HumanBoneIndex[kRightShoulder] >= 0) + { + ret = math::float1(0.25f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kChest]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kNeck]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftShoulder]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightShoulder]].t); + } + else + { + ret = math::float1(1.0f/3.0f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kChest]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftUpperArm]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightUpperArm]].t); + } + break; + + case kNeck: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kNeck]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kHead]].t); + break; + + case kLeftUpperLeg: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftUpperLeg]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftLowerLeg]].t); + break; + + case kLeftLowerLeg: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftLowerLeg]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftFoot]].t); + break; + + case kLeftShoulder: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftShoulder]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftUpperArm]].t); + break; + + case kLeftUpperArm: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftUpperArm]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftLowerArm]].t); + break; + + case kLeftLowerArm: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftUpperArm]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kLeftHand]].t); + break; + + case kRightUpperLeg: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightUpperLeg]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightLowerLeg]].t); + break; + + case kRightLowerLeg: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightLowerLeg]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightFoot]].t); + break; + + case kRightShoulder: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightShoulder]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightUpperArm]].t); + break; + + case kRightUpperArm: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightUpperArm]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightLowerArm]].t); + break; + + case kRightLowerArm: + ret = math::float1(0.5f) * (apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightLowerArm]].t + apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[kRightHand]].t); + break; + + default: + ret = apSkeletonPose->m_X[apHuman->m_HumanBoneIndex[aBoneIndex]].t; + break; + }; + + return ret; + } + + math::float4 HumanComputeMassCenter(Human const *apHuman, skeleton::SkeletonPose const *apSkeletonPoseGlobal) + { + math::float4 ret(math::float4::zero()); + + int32_t i; + + float mass = 0; + + for(i = 0; i < kLastBone; i++) + { + int32_t index = apHuman->m_HumanBoneIndex[i]; + + if(index >=0) + { + float boneMass = apHuman->m_HumanBoneMass[i]; + ret += HumanComputeBoneMassCenter(apHuman,apSkeletonPoseGlobal,i) * math::float1(boneMass); + mass += boneMass; + } + } + + return ret / math::float1(mass); + } + + float HumanComputeMomentumOfInertia(Human const *apHuman, skeleton::SkeletonPose const *apSkeletonPoseGlobal) + { + float ret = 0; + + math::float4 mc = HumanComputeMassCenter(apHuman,apSkeletonPoseGlobal); + + int32_t i; + + for(i = 0; i < kLastBone; i++) + { + int32_t index = apHuman->m_HumanBoneIndex[i]; + + if(index >= 0) + { + float r = math::length(HumanComputeBoneMassCenter(apHuman,apSkeletonPoseGlobal,index) - mc).tofloat(); + ret += apHuman->m_HumanBoneMass[i] * r * r; + } + } + + return ret; + + } + + math::float4 HumanComputeOrientation(Human const* apHuman,skeleton::SkeletonPose const* apPoseGlobal) + { + int32_t llIndex = apHuman->m_HumanBoneIndex[kLeftUpperLeg]; + int32_t rlIndex = apHuman->m_HumanBoneIndex[kRightUpperLeg]; + + int32_t laIndex = apHuman->m_HumanBoneIndex[kLeftUpperArm]; + int32_t raIndex = apHuman->m_HumanBoneIndex[kRightUpperArm]; + + math::float4 legMC = math::float1(0.5f) * (apPoseGlobal->m_X[llIndex].t + apPoseGlobal->m_X[rlIndex].t); + math::float4 armMC = math::float1(0.5f) * (apPoseGlobal->m_X[laIndex].t + apPoseGlobal->m_X[raIndex].t); + + math::float4 upV = math::normalize(armMC-legMC); + + math::float4 legV = apPoseGlobal->m_X[rlIndex].t - apPoseGlobal->m_X[llIndex].t; + math::float4 armV = apPoseGlobal->m_X[raIndex].t - apPoseGlobal->m_X[laIndex].t; + + math::float4 rightV = math::normalize(legV+armV); + math::float4 frontV = math::cross(rightV, upV); + + rightV = math::cross(upV,frontV); + + return math::normalize(math::quatMul(math::quatMatrixToQuat(rightV,upV,frontV),math::quatConj(apHuman->m_RootX.q))); + } + + math::xform HumanComputeRootXform(Human const* apHuman,skeleton::SkeletonPose const* apPoseGlobal) + { + return math::xform(HumanComputeMassCenter(apHuman,apPoseGlobal),HumanComputeOrientation(apHuman,apPoseGlobal),math::float4(1.0f)); + } + + float HumanGetFootHeight(Human const* apHuman, bool aLeft) + { + return apHuman->m_Skeleton->m_AxesArray[apHuman->m_Skeleton->m_Node[apHuman->m_HumanBoneIndex[aLeft ? kLeftFoot : kRightFoot]].m_AxesId].m_Length; + } + + math::float4 HumanGetFootBottom(Human const* apHuman, bool aLeft) + { + return math::float4(HumanGetFootHeight(apHuman,aLeft),0,0,0); + } + + math::xform HumanGetColliderXform(Human const* apHuman, math::xform const& x, int32_t aBoneIndex) + { + math::xform ret; + + int32_t skIndex = apHuman->m_HumanBoneIndex[aBoneIndex]; + + if(skIndex >= 0) + { + int32_t axesIndex = apHuman->m_Skeleton->m_Node[skIndex].m_AxesId; + int32_t colliderIndex = apHuman->m_ColliderIndex[aBoneIndex]; + + if(axesIndex >= 0 && colliderIndex >= 0) + { + ret = x; + + ret.q = math::normalize(math::quatMul(ret.q,apHuman->m_Skeleton->m_AxesArray[axesIndex].m_PostQ)); + + ret = math::xformMul(ret,apHuman->m_ColliderArray[colliderIndex].m_X); + + //ret.q = math::normalize(math::quatMul(ret.q,math::float4(0,1,0,1))); // to math physX axis setup + } + } + + return ret; + } + + math::xform HumanSubColliderXform(Human const* apHuman, math::xform const& x, int32_t aBoneIndex) + { + math::xform ret; + + int32_t skIndex = apHuman->m_HumanBoneIndex[aBoneIndex]; + + if(skIndex >= 0) + { + int32_t axesIndex = apHuman->m_Skeleton->m_Node[skIndex].m_AxesId; + int32_t colliderIndex = apHuman->m_ColliderIndex[aBoneIndex]; + + if(axesIndex >= 0 && colliderIndex >= 0) + { + ret = x; + + ret = math::xformMulInv(ret,apHuman->m_ColliderArray[colliderIndex].m_X); + + ret.q = math::normalize(math::quatMul(ret.q, math::quatConj(apHuman->m_Skeleton->m_AxesArray[axesIndex].m_PostQ))); + } + } + + return ret; + } + + math::float4 HumanGetGoalOrientationOffset(Goal goalIndex) + { + return math::load(goalOrientationOffsetArray[goalIndex]); + } + + void HumanPoseClear(HumanPose& arPose) + { + uint32_t i; + + arPose.m_RootX = math::xformIdentity(); + + for(i = 0; i < kLastGoal; i++) + { + arPose.m_GoalArray[i].m_X = math::xformIdentity(); + } + + for(i = 0; i < kLastDoF; i++) + { + arPose.m_DoFArray[i] = 0; + } + + for(i = 0; i < hand::s_DoFCount; i++) + { + arPose.m_LeftHandPose.m_DoFArray[i] = 0; + arPose.m_RightHandPose.m_DoFArray[i] = 0; + } + } + + void HumanPoseCopy(HumanPose &arPose,HumanPose const &arPoseA, bool aDoFOnly) + { + uint32_t i; + + if(!aDoFOnly) + { + arPose.m_RootX = arPoseA.m_RootX; + + for(i = 0; i < kLastGoal; i++) + { + arPose.m_GoalArray[i].m_X = arPoseA.m_GoalArray[i].m_X; + } + } + + for(i = 0; i < kLastDoF; i++) + { + arPose.m_DoFArray[i] = arPoseA.m_DoFArray[i]; + } + + hand::HandPoseCopy(&arPoseA.m_LeftHandPose,&arPose.m_LeftHandPose); + hand::HandPoseCopy(&arPoseA.m_RightHandPose,&arPose.m_RightHandPose); + } + + void HumanPoseCopy(HumanPose &arPose,HumanPose const &arPoseA, HumanPoseMask const &arHumanPoseMask) + { + if( arHumanPoseMask == FullBodyMask()) + { + HumanPoseCopy(arPose,arPoseA); + } + else + { + int32_t i; + for(i = 0; i < kLastDoF; i++) + { + if(arHumanPoseMask.test(kMaskDoFStartIndex+i)) + { + arPose.m_DoFArray[i] = arPoseA.m_DoFArray[i]; + } + else + { + arPose.m_DoFArray[i] = 0; + } + } + + if(arHumanPoseMask.test(kMaskLeftHand)) + { + for(i = 0; i < hand::s_DoFCount; i++) + { + arPose.m_LeftHandPose.m_DoFArray[i] = arPoseA.m_LeftHandPose.m_DoFArray[i]; + } + } + else + { + for(i = 0; i < hand::s_DoFCount; i++) + { + arPose.m_LeftHandPose.m_DoFArray[i] = 0; + } + } + + if(arHumanPoseMask.test(kMaskRightHand)) + { + for(i = 0; i < hand::s_DoFCount; i++) + { + arPose.m_RightHandPose.m_DoFArray[i] = arPoseA.m_RightHandPose.m_DoFArray[i]; + } + } + else + { + for(i = 0; i < hand::s_DoFCount; i++) + { + arPose.m_RightHandPose.m_DoFArray[i] = 0; + } + } + + for(i = 0; i < kLastGoal; i++) + { + if(arHumanPoseMask.test(kMaskGoalStartIndex+i)) + { + arPose.m_GoalArray[i].m_X = arPoseA.m_GoalArray[i].m_X; + } + else + { + arPose.m_GoalArray[i].m_X = math::xformIdentity(); + } + } + + if(arHumanPoseMask.test(0)) + { + arPose.m_RootX = arPoseA.m_RootX; + } + else + { + arPose.m_RootX = math::xformIdentity(); + } + + } + } + + void HumanPoseAdd(HumanPose &arPose,HumanPose const &arPoseA,HumanPose const &arPoseB) + { + uint32_t i; + + for(i = 0; i < kLastGoal; i++) + { + arPose.m_GoalArray[i].m_X = math::xformMul(arPoseA.m_GoalArray[i].m_X,arPoseB.m_GoalArray[i].m_X); + } + + for(i = 0; i < kLastDoF; i++) + { + arPose.m_DoFArray[i] = arPoseA.m_DoFArray[i] + arPoseB.m_DoFArray[i]; + } + + for(i = 0; i < hand::s_DoFCount; i++) + { + arPose.m_LeftHandPose.m_DoFArray[i] = arPoseA.m_LeftHandPose.m_DoFArray[i] + arPoseB.m_LeftHandPose.m_DoFArray[i]; + arPose.m_RightHandPose.m_DoFArray[i] = arPoseA.m_RightHandPose.m_DoFArray[i] + arPoseB.m_RightHandPose.m_DoFArray[i]; + } + + arPose.m_RootX = math::xformMul(arPoseA.m_RootX,arPoseB.m_RootX); + } + + void HumanPoseSub(HumanPose &arPose,HumanPose const &arPoseA,HumanPose const &arPoseB) + { + uint32_t i; + + for(i = 0; i < kLastGoal; i++) + { + arPose.m_GoalArray[i].m_X = math::xformInvMulNS(arPoseB.m_GoalArray[i].m_X,arPoseA.m_GoalArray[i].m_X); + } + + for(i = 0; i < kLastDoF; i++) + { + arPose.m_DoFArray[i] = arPoseA.m_DoFArray[i] - arPoseB.m_DoFArray[i]; + } + + for(i = 0; i < hand::s_DoFCount; i++) + { + arPose.m_LeftHandPose.m_DoFArray[i] = arPoseA.m_LeftHandPose.m_DoFArray[i] - arPoseB.m_LeftHandPose.m_DoFArray[i]; + arPose.m_RightHandPose.m_DoFArray[i] = arPoseA.m_RightHandPose.m_DoFArray[i] - arPoseB.m_RightHandPose.m_DoFArray[i]; + } + + arPose.m_RootX = math::xformInvMulNS(arPoseB.m_RootX,arPoseA.m_RootX); + } + + void HumanPoseWeight(HumanPose &arPose,HumanPose const &arPoseA, float aWeight) + { + uint32_t i; + + math::float1 w(aWeight); + + for(i = 0; i < kLastGoal; i++) + { + arPose.m_GoalArray[i].m_X = math::xformWeight(arPoseA.m_GoalArray[i].m_X,w); + } + + for(i = 0; i < kLastDoF; i++) + { + arPose.m_DoFArray[i] = arPoseA.m_DoFArray[i] * aWeight; + } + + for(i = 0; i < hand::s_DoFCount; i++) + { + arPose.m_LeftHandPose.m_DoFArray[i] = arPoseA.m_LeftHandPose.m_DoFArray[i] * aWeight; + arPose.m_RightHandPose.m_DoFArray[i] = arPoseA.m_RightHandPose.m_DoFArray[i] * aWeight; + } + + arPose.m_RootX = math::xformWeight(arPoseA.m_RootX,w); + } + + void HumanPoseMirror(HumanPose &arPose,HumanPose const &arPoseA) + { + uint32_t i; + + for(i = 0; i < kLastBodyDoF; i++) + { + arPose.m_DoFArray[kBodyDoFStart + i] *= BodyDoFMirror[i]; + } + + for(i = 0; i < kLastHeadDoF; i++) + { + arPose.m_DoFArray[kHeadDoFStart + i] *= HeadDoFMirror[i]; + // bobtodo + } + + for(i = 0; i < kLastArmDoF; i++) + { + float dof = arPose.m_DoFArray[kLeftArmDoFStart + i]; + arPose.m_DoFArray[kLeftArmDoFStart + i] = arPose.m_DoFArray[kRightArmDoFStart + i]; + arPose.m_DoFArray[kRightArmDoFStart + i] = dof; + } + + for(i = 0; i < kLastLegDoF; i++) + { + float dof = arPose.m_DoFArray[kLeftLegDoFStart + i]; + arPose.m_DoFArray[kLeftLegDoFStart + i] = arPose.m_DoFArray[kRightLegDoFStart + i]; + arPose.m_DoFArray[kRightLegDoFStart + i] = dof; + } + + math::xform x = arPose.m_GoalArray[kLeftFootGoal].m_X; + arPose.m_GoalArray[kLeftFootGoal].m_X = arPose.m_GoalArray[kRightFootGoal].m_X; + arPose.m_GoalArray[kRightFootGoal].m_X = x; + + x = arPose.m_GoalArray[kLeftHandGoal].m_X; + arPose.m_GoalArray[kLeftHandGoal].m_X = arPose.m_GoalArray[kRightHandGoal].m_X; + arPose.m_GoalArray[kRightHandGoal].m_X = x; + + constant_float4(offsetQY,0,1,0,0); + constant_float4(offsetQZ,0,0,1,0); + + for(i = 0; i < kLastGoal; i++) + { + arPose.m_GoalArray[i].m_X = math::mirror(arPose.m_GoalArray[i].m_X); + } + + arPose.m_GoalArray[kLeftFootGoal].m_X.q = math::normalize(math::quatMul(arPose.m_GoalArray[kLeftFootGoal].m_X.q,offsetQY)); + arPose.m_GoalArray[kRightFootGoal].m_X.q = math::normalize(math::quatMul(arPose.m_GoalArray[kRightFootGoal].m_X.q,offsetQY)); + arPose.m_GoalArray[kLeftHandGoal].m_X.q = math::normalize(math::quatMul(arPose.m_GoalArray[kLeftHandGoal].m_X.q,offsetQZ)); + arPose.m_GoalArray[kRightHandGoal].m_X.q = math::normalize(math::quatMul(arPose.m_GoalArray[kRightHandGoal].m_X.q,offsetQZ)); + + arPose.m_RootX = math::mirror(arPose.m_RootX); + + for(i = 0; i < hand::s_DoFCount; i++) + { + float leftdof = arPose.m_LeftHandPose.m_DoFArray[i]; + arPose.m_LeftHandPose.m_DoFArray[i] = arPose.m_RightHandPose.m_DoFArray[i]; + arPose.m_RightHandPose.m_DoFArray[i] = leftdof; + } + } + + void HumanPoseBlend(HumanPose &arPose,HumanPose **apPoseArray, float *apWeightArray, uint32_t aCount) + { + uint32_t poseIter,i; + + for(i = 0; i < kLastDoF; i++) + { + arPose.m_DoFArray[i] = 0; + } + + for(i = 0; i < hand::s_DoFCount; i++) + { + arPose.m_LeftHandPose.m_DoFArray[i] = 0; + arPose.m_RightHandPose.m_DoFArray[i] = 0; + } + + for(i = 0; i < kLastGoal; i++) + { + arPose.m_GoalArray[i].m_X.t = math::float4::zero(); + arPose.m_GoalArray[i].m_X.q = math::float4::zero(); + arPose.m_GoalArray[i].m_X.s = math::float4::one(); + } + + arPose.m_RootX.t = math::float4::zero(); + arPose.m_RootX.q = math::float4::zero(); + arPose.m_RootX.s = math::float4::one(); + + float sumW = 0; + + for(poseIter = 0; poseIter < aCount; poseIter++) + { + float w = apWeightArray[poseIter]; + math::float1 w1(w); + + sumW += w; + + for(i = 0; i < kLastDoF; i++) + { + arPose.m_DoFArray[i] += apPoseArray[poseIter]->m_DoFArray[i]*w; + } + + for(i = 0; i < hand::s_DoFCount; i++) + { + arPose.m_LeftHandPose.m_DoFArray[i] += apPoseArray[poseIter]->m_LeftHandPose.m_DoFArray[i] * w; + arPose.m_RightHandPose.m_DoFArray[i] += apPoseArray[poseIter]->m_RightHandPose.m_DoFArray[i] * w; + } + + for(i = 0; i < kLastGoal; i++) + { + arPose.m_GoalArray[i].m_X.t += apPoseArray[poseIter]->m_GoalArray[i].m_X.t*w1; + arPose.m_GoalArray[i].m_X.q += math::cond(math::dot(arPose.m_GoalArray[i].m_X.q,apPoseArray[poseIter]->m_GoalArray[i].m_X.q) < math::float1::zero(),apPoseArray[poseIter]->m_GoalArray[i].m_X.q * -w1,apPoseArray[poseIter]->m_GoalArray[i].m_X.q * w1); + arPose.m_GoalArray[i].m_X.s *= scaleWeight(apPoseArray[poseIter]->m_GoalArray[i].m_X.s,w1); + } + + arPose.m_RootX.t += apPoseArray[poseIter]->m_RootX.t*w1; + arPose.m_RootX.q += math::cond(math::dot(arPose.m_RootX.q,apPoseArray[poseIter]->m_RootX.q) < math::float1::zero(),apPoseArray[poseIter]->m_RootX.q * -w1,apPoseArray[poseIter]->m_RootX.q * w1); + arPose.m_RootX.s *= scaleWeight(apPoseArray[poseIter]->m_RootX.s,w1); + } + + math::float4 q(0,0,0,math::saturate(1.0f-sumW)); + + for(i = 0; i < kLastGoal; i++) + { + arPose.m_GoalArray[i].m_X.q = math::normalize(arPose.m_GoalArray[i].m_X.q+q); + } + + arPose.m_RootX.q = math::normalize(arPose.m_RootX.q+q); + + } + + void HumanPoseAddOverrideLayer(HumanPose &arPoseBase,HumanPose const &arPose, float aWeight, HumanPoseMask const &arHumanPoseMask) + { + if(aWeight > 0.0f) + { + float weightInv = 1.0f - aWeight; + math::float1 w(aWeight); + + int32_t i; + for(i = 0; i < kLastDoF; i++) + { + if(arHumanPoseMask.test(kMaskDoFStartIndex+i)) + { + if(aWeight < 1.0f) + { + arPoseBase.m_DoFArray[i] = weightInv * arPoseBase.m_DoFArray[i] + aWeight * arPose.m_DoFArray[i]; + } + else + { + arPoseBase.m_DoFArray[i] = arPose.m_DoFArray[i]; + } + } + } + + if(arHumanPoseMask.test(kMaskLeftHand)) + { + for(i = 0; i < hand::s_DoFCount; i++) + { + if(aWeight < 1.0f) + { + arPoseBase.m_LeftHandPose.m_DoFArray[i] = weightInv * arPoseBase.m_LeftHandPose.m_DoFArray[i] + aWeight * arPose.m_LeftHandPose.m_DoFArray[i]; + } + else + { + arPoseBase.m_LeftHandPose.m_DoFArray[i] = arPose.m_LeftHandPose.m_DoFArray[i]; + } + } + } + + if(arHumanPoseMask.test(kMaskRightHand)) + { + for(i = 0; i < hand::s_DoFCount; i++) + { + if(aWeight < 1.0f) + { + arPoseBase.m_RightHandPose.m_DoFArray[i] = weightInv * arPoseBase.m_RightHandPose.m_DoFArray[i] + aWeight * arPose.m_RightHandPose.m_DoFArray[i]; + } + else + { + arPoseBase.m_RightHandPose.m_DoFArray[i] = arPose.m_RightHandPose.m_DoFArray[i]; + } + } + } + + for(i = 0; i < kLastGoal; i++) + { + if(arHumanPoseMask.test(kMaskGoalStartIndex+i)) + { + if(aWeight < 1.0f) + { + arPoseBase.m_GoalArray[i].m_X = math::xformBlend(arPoseBase.m_GoalArray[i].m_X,arPose.m_GoalArray[i].m_X,w); + } + else + { + arPoseBase.m_GoalArray[i].m_X = arPose.m_GoalArray[i].m_X; + } + } + } + + if(arHumanPoseMask.test(0)) + { + if(aWeight < 1.0f) + { + arPoseBase.m_RootX = math::xformBlend(arPoseBase.m_RootX,arPose.m_RootX,w); + } + else + { + arPoseBase.m_RootX = arPose.m_RootX; + } + } + } + } + + void HumanPoseAddAdditiveLayer(HumanPose &arPoseBase,HumanPose const &arPose, float aWeight, HumanPoseMask const &arHumanPoseMask) + { + if(aWeight > 0.0f) + { + math::float1 w(aWeight); + + int32_t i; + for(i = 0; i < kLastDoF; i++) + { + if(arHumanPoseMask.test(kMaskDoFStartIndex+i)) + { + arPoseBase.m_DoFArray[i] += aWeight * arPose.m_DoFArray[i]; + } + } + + if(arHumanPoseMask.test(kMaskLeftHand)) + { + for(i = 0; i < hand::s_DoFCount; i++) + { + arPoseBase.m_LeftHandPose.m_DoFArray[i] += aWeight * arPose.m_LeftHandPose.m_DoFArray[i]; + } + } + + if(arHumanPoseMask.test(kMaskRightHand)) + { + for(i = 0; i < hand::s_DoFCount; i++) + { + arPoseBase.m_RightHandPose.m_DoFArray[i] += aWeight * arPose.m_RightHandPose.m_DoFArray[i]; + } + } + + for(i = 0; i < kLastGoal; i++) + { + if(arHumanPoseMask.test(kMaskGoalStartIndex+i)) + { + arPoseBase.m_GoalArray[i].m_X = math::xformMul(arPoseBase.m_GoalArray[i].m_X, math::xformWeight(arPose.m_GoalArray[i].m_X, w)); + } + } + + if(arHumanPoseMask.test(0)) + { + arPoseBase.m_RootX = math::xformMul(arPoseBase.m_RootX, math::xformWeight(arPose.m_RootX,w)); + } + } + } + + void HumanFixMidDoF(Human const *apHuman, skeleton::SkeletonPose *apSkeletonPose, skeleton::SkeletonPose *apSkeletonPoseWs, int32_t aPIndex, int32_t aCIndex) + { + int32_t pNodeIndex = apHuman->m_HumanBoneIndex[aPIndex]; + int32_t cNodeIndex = apHuman->m_HumanBoneIndex[aCIndex]; + int32_t aNodeIndex = apHuman->m_Skeleton->m_Node[pNodeIndex].m_ParentId; + + math::Axes pAxes = apHuman->m_Skeleton->m_AxesArray[apHuman->m_Skeleton->m_Node[pNodeIndex].m_AxesId]; + math::Axes cAxes = apHuman->m_Skeleton->m_AxesArray[apHuman->m_Skeleton->m_Node[cNodeIndex].m_AxesId]; + + apSkeletonPoseWs->m_X[aNodeIndex].q = math::quatIdentity(); + skeleton::SkeletonPoseComputeGlobalQ(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWs,cNodeIndex,pNodeIndex); + + math::float4 pq = apSkeletonPose->m_X[pNodeIndex].q; + math::float4 cqg = apSkeletonPoseWs->m_X[cNodeIndex].q; + + math::float4 cql = AxesProject(cAxes,apSkeletonPose->m_X[cNodeIndex].q); + + math::float4 xyz = math::quat2ZYRoll(cql); + xyz.y() = math::float1::zero(); + cql = math::ZYRoll2Quat(xyz); + + cql = math::AxesUnproject(cAxes,cql); + + apSkeletonPose->m_X[cNodeIndex].q = cql; + skeleton::SkeletonPoseComputeGlobalQ(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWs,cNodeIndex,cNodeIndex); + + math::float4 qdiff = math::quatMul(cqg,math::quatConj(apSkeletonPoseWs->m_X[cNodeIndex].q)); + + apSkeletonPose->m_X[pNodeIndex].q = math::normalize(math::quatMul(qdiff,apSkeletonPose->m_X[pNodeIndex].q)); + + skeleton::SkeletonAlign(apHuman->m_Skeleton.Get(),pq,apSkeletonPose->m_X[pNodeIndex].q,pNodeIndex); + + skeleton::SkeletonPoseComputeGlobalQ(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWs,cNodeIndex,pNodeIndex); + + apSkeletonPoseWs->m_X[cNodeIndex].q = cqg; + + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseWs,apSkeletonPose,cNodeIndex,cNodeIndex); + } + + void HumanFixEndDoF(Human const *apHuman, skeleton::SkeletonPose *apSkeletonPose, skeleton::SkeletonPose *apSkeletonPoseWs, int32_t aPIndex, int32_t aCIndex) + { + int32_t pNodeIndex = apHuman->m_HumanBoneIndex[aPIndex]; + int32_t cNodeIndex = apHuman->m_HumanBoneIndex[aCIndex]; + int32_t aNodeIndex = apHuman->m_Skeleton->m_Node[pNodeIndex].m_ParentId; + + math::Axes pAxes = apHuman->m_Skeleton->m_AxesArray[apHuman->m_Skeleton->m_Node[pNodeIndex].m_AxesId]; + math::Axes cAxes = apHuman->m_Skeleton->m_AxesArray[apHuman->m_Skeleton->m_Node[cNodeIndex].m_AxesId]; + + apSkeletonPoseWs->m_X[aNodeIndex].q = math::quatIdentity(); + skeleton::SkeletonPoseComputeGlobalQ(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWs,cNodeIndex,pNodeIndex); + + math::float4 pq = apSkeletonPose->m_X[pNodeIndex].q; + math::float4 cqg = apSkeletonPoseWs->m_X[cNodeIndex].q; + + math::float4 pq0 = FromAxes(pAxes,math::float4::zero()); + math::float4 cql0 = FromAxes(cAxes,math::float4::zero()); + + apSkeletonPose->m_X[pNodeIndex].q = pq0; + apSkeletonPose->m_X[cNodeIndex].q = cql0; + skeleton::SkeletonPoseComputeGlobalQ(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWs,cNodeIndex,pNodeIndex); + + math::float4 qdiff = math::quatMul(cqg,math::quatConj(apSkeletonPoseWs->m_X[cNodeIndex].q)); + + apSkeletonPose->m_X[pNodeIndex].q = math::normalize(math::quatMul(qdiff,pq0)); + + skeleton::SkeletonAlign(apHuman->m_Skeleton.Get(),pq,apSkeletonPose->m_X[pNodeIndex].q,pNodeIndex); + + skeleton::SkeletonPoseComputeGlobalQ(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWs,cNodeIndex,pNodeIndex); + + apSkeletonPoseWs->m_X[cNodeIndex].q = cqg; + + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseWs,apSkeletonPose,cNodeIndex,cNodeIndex); + } + + void HumanFixTwist(Human const *apHuman, skeleton::SkeletonPose *apSkeletonPose, skeleton::SkeletonPose *apSkeletonPoseWs, int32_t aPIndex, int32_t aCIndex, const math::float1& aTwist) + { + int32_t pNodeIndex = apHuman->m_HumanBoneIndex[aPIndex]; + int32_t cNodeIndex = apHuman->m_HumanBoneIndex[aCIndex]; + int32_t aNodeIndex = apHuman->m_Skeleton->m_Node[pNodeIndex].m_ParentId; + + math::Axes pAxes = apHuman->m_Skeleton->m_AxesArray[apHuman->m_Skeleton->m_Node[pNodeIndex].m_AxesId]; + + apSkeletonPoseWs->m_X[aNodeIndex].q = math::quatIdentity(); + skeleton::SkeletonPoseComputeGlobalQ(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWs,cNodeIndex,pNodeIndex); + + math::float4 pq = apSkeletonPose->m_X[pNodeIndex].q; + math::float4 cqg = apSkeletonPoseWs->m_X[cNodeIndex].q; + + math::float4 pxyz = math::ToAxes(pAxes,apSkeletonPose->m_X[pNodeIndex].q); + pxyz.x() *= aTwist; + + apSkeletonPose->m_X[pNodeIndex].q = math::FromAxes(pAxes,pxyz); + + skeleton::SkeletonAlign(apHuman->m_Skeleton.Get(),pq,apSkeletonPose->m_X[pNodeIndex].q,pNodeIndex); + + skeleton::SkeletonPoseComputeGlobalQ(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWs,cNodeIndex,pNodeIndex); + + apSkeletonPoseWs->m_X[cNodeIndex].q = cqg; + + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseWs,apSkeletonPose,cNodeIndex,cNodeIndex); + } + + void ReachGoalRotation(Human const *apHuman,math::float4 const &arEndQ, int32_t aGoalIndex, skeleton::SkeletonPose *apSkeletonPose, skeleton::SkeletonPose *apSkeletonPoseGbl, skeleton::SkeletonPose *apSkeletonPoseWorkspace) + { + int32_t index = apHuman->m_HumanBoneIndex[s_HumanGoalInfo[aGoalIndex].m_Index]; + int32_t parentIndex = apHuman->m_Skeleton->m_Node[index].m_ParentId; + apSkeletonPose->m_X[index].q = math::normalize(math::quatMul(math::quatConj(apSkeletonPoseGbl->m_X[parentIndex].q),arEndQ)); + + HumanFixEndDoF(apHuman,apSkeletonPose,apSkeletonPoseWorkspace,s_HumanGoalInfo[aGoalIndex].m_MidIndex,s_HumanGoalInfo[aGoalIndex].m_EndIndex); + } + + void HumanFixEndPointsSkeletonPose(Human const *apHuman, skeleton::SkeletonPose const*apSkeletonPoseRef, HumanPose *apHumanPose, skeleton::SkeletonPose *apSkeletonPoseGbl, skeleton::SkeletonPose *apSkeletonPoseLcl, skeleton::SkeletonPose *apSkeletonPoseWs,int32_t cIndex, int32_t pIndex) + { + skeleton::SkeletonPoseComputeGlobalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseLcl,apSkeletonPoseGbl,apHuman->m_HumanBoneIndex[cIndex],apHuman->m_HumanBoneIndex[pIndex]); + apSkeletonPoseGbl->m_X[apHuman->m_HumanBoneIndex[cIndex]].q = apSkeletonPoseRef->m_X[apHuman->m_HumanBoneIndex[cIndex]].q; + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseGbl,apSkeletonPoseLcl,apHuman->m_HumanBoneIndex[cIndex],apHuman->m_HumanBoneIndex[pIndex]); + + HumanFixEndDoF(apHuman,apSkeletonPoseLcl,apSkeletonPoseWs,pIndex,cIndex); + } + + void HumanAlignSkeletonPose(Human const *apHuman, skeleton::SkeletonPose const*apSkeletonPoseRef, HumanPose *apHumanPose, skeleton::SkeletonPose *apSkeletonPoseGbl, skeleton::SkeletonPose *apSkeletonPoseLcl,int32_t cIndex, int32_t pIndex) + { + Skeleton2HumanPose(apHuman,apSkeletonPoseLcl,apHumanPose,cIndex); + Human2SkeletonPose(apHuman,apHumanPose,apSkeletonPoseLcl,cIndex); + + skeleton::SkeletonPoseComputeGlobalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseLcl,apSkeletonPoseGbl,apHuman->m_HumanBoneIndex[cIndex],apHuman->m_HumanBoneIndex[pIndex]); + skeleton::SkeletonAlign(apHuman->m_Skeleton.Get(),apSkeletonPoseRef,apSkeletonPoseGbl,apHuman->m_HumanBoneIndex[cIndex]); + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseGbl,apSkeletonPoseLcl,apHuman->m_HumanBoneIndex[cIndex],apHuman->m_HumanBoneIndex[pIndex]); + } + + void RetargetFrom( Human const *apHuman, + skeleton::SkeletonPose const *apSkeletonPose, + HumanPose *apHumanPose, + skeleton::SkeletonPose *apSkeletonPoseRef, + skeleton::SkeletonPose *apSkeletonPoseGbl, + skeleton::SkeletonPose *apSkeletonPoseLcl, + skeleton::SkeletonPose *apSkeletonPoseWs, + mecanim::int32_t maxFixIter) + + { + const int32_t hipsIndex = apHuman->m_HumanBoneIndex[human::kHips]; + const math::float1 scale(apHuman->m_Scale); + + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseRef); + skeleton::SkeletonPoseCopy(apSkeletonPoseRef,apSkeletonPoseGbl); + + // force dummy bones to their default rotation + int32_t nodeIter; + for(nodeIter = 1; nodeIter < apHuman->m_Skeleton->m_Count; nodeIter++) + { + if(apHuman->m_Skeleton->m_Node[nodeIter].m_AxesId == -1) + { + apSkeletonPoseGbl->m_X[nodeIter].q = math::quatMul(apSkeletonPoseGbl->m_X[apHuman->m_Skeleton->m_Node[nodeIter].m_ParentId].q,apHuman->m_SkeletonPose->m_X[nodeIter].q); + } + } + + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseGbl,apSkeletonPoseLcl); + + int32_t fixIter; + + // align shoulders + for(fixIter = 0; fixIter < maxFixIter; fixIter++) + { + if(apHuman->m_HumanBoneIndex[kLeftShoulder] != -1) + { + HumanAlignSkeletonPose(apHuman,apSkeletonPoseRef,apHumanPose,apSkeletonPoseGbl,apSkeletonPoseLcl,kLeftShoulder, kLeftShoulder); + } + + if(apHuman->m_HumanBoneIndex[kRightShoulder] != -1) + { + HumanAlignSkeletonPose(apHuman,apSkeletonPoseRef,apHumanPose,apSkeletonPoseGbl,apSkeletonPoseLcl,kRightShoulder, kRightShoulder); + } + } + + // align upper limbs + for(fixIter = 0; fixIter < maxFixIter; fixIter++) + { + HumanAlignSkeletonPose(apHuman,apSkeletonPoseRef,apHumanPose,apSkeletonPoseGbl,apSkeletonPoseLcl,kLeftUpperArm, apHuman->m_HumanBoneIndex[kLeftShoulder] != -1 ? kLeftShoulder : kLeftUpperArm); + HumanAlignSkeletonPose(apHuman,apSkeletonPoseRef,apHumanPose,apSkeletonPoseGbl,apSkeletonPoseLcl,kRightUpperArm, apHuman->m_HumanBoneIndex[kRightShoulder] != -1 ? kRightShoulder : kRightUpperArm); + HumanAlignSkeletonPose(apHuman,apSkeletonPoseRef,apHumanPose,apSkeletonPoseGbl,apSkeletonPoseLcl,kLeftUpperLeg, kLeftUpperLeg); + HumanAlignSkeletonPose(apHuman,apSkeletonPoseRef,apHumanPose,apSkeletonPoseGbl,apSkeletonPoseLcl,kRightUpperLeg, kRightUpperLeg); + } + + // align & fix lower limbs + for(fixIter = 0; fixIter < maxFixIter; fixIter++) + { + HumanFixMidDoF(apHuman,apSkeletonPoseLcl,apSkeletonPoseWs,kLeftUpperArm,kLeftLowerArm); + HumanFixMidDoF(apHuman,apSkeletonPoseLcl,apSkeletonPoseWs,kRightUpperArm,kRightLowerArm); + HumanFixMidDoF(apHuman,apSkeletonPoseLcl,apSkeletonPoseWs,kLeftUpperLeg,kLeftLowerLeg); + HumanFixMidDoF(apHuman,apSkeletonPoseLcl,apSkeletonPoseWs,kRightUpperLeg,kRightLowerLeg); + + HumanAlignSkeletonPose(apHuman,apSkeletonPoseRef,apHumanPose,apSkeletonPoseGbl,apSkeletonPoseLcl,kLeftLowerArm, kLeftUpperArm); + HumanAlignSkeletonPose(apHuman,apSkeletonPoseRef,apHumanPose,apSkeletonPoseGbl,apSkeletonPoseLcl,kRightLowerArm, kRightUpperArm); + HumanAlignSkeletonPose(apHuman,apSkeletonPoseRef,apHumanPose,apSkeletonPoseGbl,apSkeletonPoseLcl,kLeftLowerLeg, kLeftUpperLeg); + HumanAlignSkeletonPose(apHuman,apSkeletonPoseRef,apHumanPose,apSkeletonPoseGbl,apSkeletonPoseLcl,kRightLowerLeg, kRightUpperLeg); + } + + HumanFixEndPointsSkeletonPose(apHuman,apSkeletonPoseRef, apHumanPose, apSkeletonPoseGbl, apSkeletonPoseLcl, apSkeletonPoseWs, kLeftHand, kLeftLowerArm); + HumanFixEndPointsSkeletonPose(apHuman,apSkeletonPoseRef, apHumanPose, apSkeletonPoseGbl, apSkeletonPoseLcl, apSkeletonPoseWs, kRightHand, kRightLowerArm); + HumanFixEndPointsSkeletonPose(apHuman,apSkeletonPoseRef, apHumanPose, apSkeletonPoseGbl, apSkeletonPoseLcl, apSkeletonPoseWs, kLeftFoot, kLeftLowerLeg); + HumanFixEndPointsSkeletonPose(apHuman,apSkeletonPoseRef, apHumanPose, apSkeletonPoseGbl, apSkeletonPoseLcl, apSkeletonPoseWs, kRightFoot, kRightLowerLeg); + + Skeleton2HumanPose(apHuman,apSkeletonPoseLcl,apHumanPose); + skeleton::SkeletonPoseCopy(apHuman->m_SkeletonPose.Get(), apSkeletonPoseLcl); + Human2SkeletonPose(apHuman, apHumanPose, apSkeletonPoseLcl); + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(), apSkeletonPoseLcl,apSkeletonPoseGbl); + + apHumanPose->m_RootX = HumanComputeRootXform(apHuman,apSkeletonPoseGbl); + apHumanPose->m_RootX = math::xformInvMul(apSkeletonPoseGbl->m_X[hipsIndex],apHumanPose->m_RootX); + apHumanPose->m_RootX = math::xformMul(apSkeletonPoseRef->m_X[hipsIndex],apHumanPose->m_RootX); + apHumanPose->m_RootX.s = math::float4::one(); + + int32_t goalIter; + for(goalIter = 0; goalIter < kLastGoal; goalIter++) + { + int32_t index = apHuman->m_HumanBoneIndex[s_HumanGoalInfo[goalIter].m_Index]; + apHumanPose->m_GoalArray[goalIter].m_X.t = apSkeletonPoseRef->m_X[index].t; + apHumanPose->m_GoalArray[goalIter].m_X.q = AddAxis(apHuman,index,apSkeletonPoseRef->m_X[index].q); + apHumanPose->m_GoalArray[goalIter].m_X.s = math::float4::one(); + + if(goalIter < 2) apHumanPose->m_GoalArray[goalIter].m_X.t = math::xformMulVec(apHumanPose->m_GoalArray[goalIter].m_X,human::HumanGetFootBottom(apHuman,goalIter==0)); + apHumanPose->m_GoalArray[goalIter].m_X = math::xformInvMulNS(apHumanPose->m_RootX,apHumanPose->m_GoalArray[goalIter].m_X); + apHumanPose->m_GoalArray[goalIter].m_X.t /= scale; + } + + apHumanPose->m_RootX.t /= scale; + } + + void RetargetTo( Human const *apHuman, + HumanPose const *apHumanPoseBase, + HumanPose const *apHumanPose, + const math::xform &arX, + HumanPose *apHumanPoseOut, + skeleton::SkeletonPose *apSkeletonPose, + skeleton::SkeletonPose *apSkeletonPoseWs) + { + const int32_t rootIndex = 0; + const int32_t hipsIndex = apHuman->m_HumanBoneIndex[human::kHips]; + const math::float1 scale(apHuman->m_Scale); + + human::HumanPoseCopy(*apHumanPoseOut,*apHumanPoseBase); + + apHumanPoseOut->m_RootX.t *= scale; + apHumanPoseOut->m_RootX = math::xformMul(arX,apHumanPoseOut->m_RootX); + + int32_t goalIter; + for(goalIter = 0; goalIter < kLastGoal; goalIter++) + { + apHumanPoseOut->m_GoalArray[goalIter].m_X = apHumanPose ? apHumanPose->m_GoalArray[goalIter].m_X : apHumanPoseBase->m_GoalArray[goalIter].m_X; + apHumanPoseOut->m_GoalArray[goalIter].m_X.t *= scale; + apHumanPoseOut->m_GoalArray[goalIter].m_X = math::xformMul(arX,apHumanPoseOut->m_GoalArray[goalIter].m_X); + + if(goalIter < 2) apHumanPoseOut->m_GoalArray[goalIter].m_X.t = math::xformMulVec(apHumanPoseOut->m_GoalArray[goalIter].m_X,-human::HumanGetFootBottom(apHuman,goalIter==0)); + } + + ////////////////////////////////////////////////// + // + // transfer muscle space for base pose + // + skeleton::SkeletonPoseCopy(apHuman->m_SkeletonPose.Get(), apSkeletonPose); + HumanPoseAdjustForMissingBones(apHuman,apHumanPoseOut); + Human2SkeletonPose(apHuman, apHumanPoseOut, apSkeletonPose); + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(), apSkeletonPose,apSkeletonPoseWs); + + /////////////////////////////////////////////////////// + // + // adjust hips local + // + math::xform rootX = HumanComputeRootXform(apHuman,apSkeletonPoseWs); + apSkeletonPose->m_X[hipsIndex] = math::xformInvMulNS(rootX,apSkeletonPoseWs->m_X[hipsIndex]); + apSkeletonPose->m_X[hipsIndex].s = apSkeletonPoseWs->m_X[hipsIndex].s; + + //////////////////////////////////////////////////////// + // + // transfer muscle space + // + if(apHumanPose) + { + human::HumanPoseCopy(*apHumanPoseOut,*apHumanPose,true); + HumanPoseAdjustForMissingBones(apHuman,apHumanPoseOut); + Human2SkeletonPose(apHuman, apHumanPoseOut, apSkeletonPose); + } + + ////////////////////////////////////////////////// + // + // root + // + apSkeletonPose->m_X[rootIndex] = apHumanPoseOut->m_RootX; + } + + math::float4 GetLookAtDeltaQ(math::float4 const &pivot,math::float4 const &eyesT, math::float4 const &eyesQ, math::float4 const &eyesDir, math::float4 const &target, math::float1 const &weight) + { + math::float1 len = math::length(target - eyesT); + math::float4 dstV = target - pivot; + math::float4 srcV = eyesT - math::quatMulVec(eyesQ,eyesDir*math::float1(len)) - pivot; + + return math::quatWeight(math::normalize(math::quatArcRotate(srcV,dstV)),weight); + } + + void FullBodySolve(Human const *apHuman, HumanPose const *apHumanPose, skeleton::SkeletonPose *apSkeletonPose, skeleton::SkeletonPose *apSkeletonPoseWorkspaceA, skeleton::SkeletonPose *apSkeletonPoseWorkspaceB) + { + const int32_t hipsIndex = apHuman->m_HumanBoneIndex[kHips]; + const int32_t chestIndex = apHuman->m_HumanBoneIndex[kChest]; + const int32_t spineIndex = apHuman->m_HumanBoneIndex[kSpine]; + const int32_t neckIndex = apHuman->m_HumanBoneIndex[kNeck]; + const int32_t headIndex = apHuman->m_HumanBoneIndex[kHead]; + const int32_t leftEyeIndex = apHuman->m_HumanBoneIndex[kLeftEye]; + const int32_t rightEyeIndex = apHuman->m_HumanBoneIndex[kRightEye]; + + math::float1 lcw = math::saturate(math::float1(apHumanPose->m_LookAtWeight.x())); + math::float1 lbw = math::saturate(math::float1(apHumanPose->m_LookAtWeight.y())); + math::float1 lhw = math::saturate(math::float1(apHumanPose->m_LookAtWeight.z())); + math::float1 lew = math::saturate(math::float1(apHumanPose->m_LookAtWeight.w())); + + math::float4 headGoalT = apHumanPose->m_LookAtPosition; + + if(lcw > math::float1::zero()) + { + math::float4 eyesPos = apSkeletonPoseWorkspaceA->m_X[headIndex].t; + math::float4 eyesRot = AddAxis(apHuman,headIndex,apSkeletonPoseWorkspaceA->m_X[headIndex].q); + if(leftEyeIndex != -1 && rightEyeIndex != -1) eyesPos = math::xformMulVec(apSkeletonPoseWorkspaceA->m_X[headIndex],(apHuman->m_SkeletonPose->m_X[leftEyeIndex].t + apHuman->m_SkeletonPose->m_X[rightEyeIndex].t) * math::float1(0.5f)); + + math::float4 dstV = headGoalT - eyesPos; + math::float4 v = math::float4::zero(); + v.y() = -math::length(dstV); + math::float4 srcV = math::quatMulVec(eyesRot,v); + + math::float4 deltaQ = math::quatClamp(math::normalize(math::quatArcRotate(srcV,dstV)),math::radians(180.f*(1.f - lcw.tofloat()))); + + headGoalT = eyesPos + math::quatMulVec(deltaQ,srcV); + } + + if(lbw > math::float1::zero()) + { + math::float1 lsw = chestIndex != -1 ? math::float1(0.5) * lbw : lbw; + + math::float4 eyesPos = apSkeletonPoseWorkspaceA->m_X[headIndex].t; + math::float4 eyesRot = AddAxis(apHuman,headIndex,apSkeletonPoseWorkspaceA->m_X[headIndex].q); + + math::float4 deltaQ = GetLookAtDeltaQ(apSkeletonPoseWorkspaceA->m_X[spineIndex].t,eyesPos,eyesRot,math::float4(0,1,0,0),headGoalT,lsw); + + apSkeletonPoseWorkspaceA->m_X[spineIndex].q = math::normalize(math::quatMul(deltaQ,apSkeletonPoseWorkspaceA->m_X[spineIndex].q)); + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseWorkspaceA,apSkeletonPose,spineIndex,spineIndex); + + if(chestIndex != -1) + { + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWorkspaceA,headIndex,spineIndex); + + math::float4 eyesPos = apSkeletonPoseWorkspaceA->m_X[headIndex].t; + math::float4 eyesRot = AddAxis(apHuman,headIndex,apSkeletonPoseWorkspaceA->m_X[headIndex].q); + + math::float4 deltaQ = GetLookAtDeltaQ(apSkeletonPoseWorkspaceA->m_X[chestIndex].t,eyesPos,eyesRot,math::float4(0,1,0,0),headGoalT,lbw); + + apSkeletonPoseWorkspaceA->m_X[chestIndex].q = math::normalize(math::quatMul(deltaQ,apSkeletonPoseWorkspaceA->m_X[chestIndex].q)); + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseWorkspaceA,apSkeletonPose,chestIndex,chestIndex); + } + } + + if(lhw > math::float1::zero()) + { + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWorkspaceA,headIndex,spineIndex); + + if(neckIndex != -1) + { + math::float4 eyesPos = apSkeletonPoseWorkspaceA->m_X[headIndex].t; + math::float4 eyesRot = AddAxis(apHuman,headIndex,apSkeletonPoseWorkspaceA->m_X[headIndex].q); + + if(leftEyeIndex != -1 && rightEyeIndex != -1) eyesPos = math::xformMulVec(apSkeletonPoseWorkspaceA->m_X[headIndex],(apHuman->m_SkeletonPose->m_X[leftEyeIndex].t + apHuman->m_SkeletonPose->m_X[rightEyeIndex].t) * math::float1(0.5f)); + + math::float4 deltaQ = GetLookAtDeltaQ(apSkeletonPoseWorkspaceA->m_X[neckIndex].t,eyesPos,eyesRot,math::float4(0,1,0,0),headGoalT,lhw * math::float1(0.5)); + + apSkeletonPoseWorkspaceA->m_X[neckIndex].q = math::normalize(math::quatMul(deltaQ,apSkeletonPoseWorkspaceA->m_X[neckIndex].q)); + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseWorkspaceA,apSkeletonPose,neckIndex,neckIndex); + } + + int32_t iter; + for(iter = 0; iter < 3; iter++) + { + math::float4 eyesPos = apSkeletonPoseWorkspaceA->m_X[headIndex].t; + math::float4 eyesRot = AddAxis(apHuman,headIndex,apSkeletonPoseWorkspaceA->m_X[headIndex].q); + + if(leftEyeIndex != -1 && rightEyeIndex != -1) eyesPos = math::xformMulVec(apSkeletonPoseWorkspaceA->m_X[headIndex],(apHuman->m_SkeletonPose->m_X[leftEyeIndex].t + apHuman->m_SkeletonPose->m_X[rightEyeIndex].t) * math::float1(0.5f)); + + math::float4 deltaQ = GetLookAtDeltaQ(apSkeletonPoseWorkspaceA->m_X[headIndex].t,eyesPos,eyesRot,math::float4(0,1,0,0),headGoalT,lhw*lhw); + + apSkeletonPoseWorkspaceA->m_X[headIndex].q = math::normalize(math::quatMul(deltaQ,apSkeletonPoseWorkspaceA->m_X[headIndex].q)); + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseWorkspaceA,apSkeletonPose,headIndex,headIndex); + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWorkspaceA,headIndex,headIndex); + } + } + + if(lew > math::float1::zero()) + { + if(leftEyeIndex != -1) + { + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWorkspaceA,leftEyeIndex,spineIndex); + + math::float4 eyesPos = apSkeletonPoseWorkspaceA->m_X[leftEyeIndex].t; + math::float4 eyesRot = AddAxis(apHuman,leftEyeIndex,apSkeletonPoseWorkspaceA->m_X[leftEyeIndex].q); + + math::float4 deltaQ = GetLookAtDeltaQ(apSkeletonPoseWorkspaceA->m_X[leftEyeIndex].t,eyesPos,eyesRot,math::float4(-1,0,0,0),headGoalT,lew); + + apSkeletonPoseWorkspaceA->m_X[leftEyeIndex].q = math::normalize(math::quatMul(deltaQ,apSkeletonPoseWorkspaceA->m_X[leftEyeIndex].q)); + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseWorkspaceA,apSkeletonPose,leftEyeIndex,leftEyeIndex); + } + + if(rightEyeIndex != -1) + { + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWorkspaceA,rightEyeIndex,spineIndex); + + math::float4 eyesPos = apSkeletonPoseWorkspaceA->m_X[rightEyeIndex].t; + math::float4 eyesRot = AddAxis(apHuman,rightEyeIndex,apSkeletonPoseWorkspaceA->m_X[rightEyeIndex].q); + + math::float4 deltaQ = GetLookAtDeltaQ(apSkeletonPoseWorkspaceA->m_X[rightEyeIndex].t,eyesPos,eyesRot,math::float4(-1,0,0,0),headGoalT,lew); + + apSkeletonPoseWorkspaceA->m_X[rightEyeIndex].q = math::normalize(math::quatMul(deltaQ,apSkeletonPoseWorkspaceA->m_X[rightEyeIndex].q)); + skeleton::SkeletonPoseComputeLocalQ(apHuman->m_Skeleton.Get(),apSkeletonPoseWorkspaceA,apSkeletonPose,rightEyeIndex,rightEyeIndex); + } + } + + int32_t goalIter; + for(goalIter = kLeftFootGoal; goalIter <= kRightHandGoal; goalIter++) + { + int32_t topIndex = apHuman->m_HumanBoneIndex[s_HumanGoalInfo[goalIter].m_TopIndex]; + int32_t midIndex = apHuman->m_HumanBoneIndex[s_HumanGoalInfo[goalIter].m_MidIndex]; + int32_t endIndex = apHuman->m_HumanBoneIndex[s_HumanGoalInfo[goalIter].m_EndIndex]; + + if(apHumanPose->m_GoalArray[goalIter].m_WeightT > 0.f) + { + float weightT = math::saturate(apHumanPose->m_GoalArray[goalIter].m_WeightT); + + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWorkspaceA,endIndex,hipsIndex); + + // adjust len + math::float4 t = math::lerp(apSkeletonPoseWorkspaceA->m_X[endIndex].t,apHumanPose->m_GoalArray[goalIter].m_X.t, math::float1(weightT)); + skeleton::Skeleton2BoneAdjustLength(apHuman->m_Skeleton.Get(),topIndex,midIndex,endIndex,t, math::float1( (goalIter < kLeftHandGoal ? apHuman->m_LegStretch : apHuman->m_ArmStretch) * weightT),apSkeletonPose,apSkeletonPoseWorkspaceA); + + // 2 bone ik + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWorkspaceA,endIndex,topIndex); + skeleton::Skeleton2BoneIK(apHuman->m_Skeleton.Get(),topIndex,midIndex,endIndex,apHumanPose->m_GoalArray[goalIter].m_X.t,weightT,apSkeletonPose,apSkeletonPoseWorkspaceA); + } + } + + // end rotation + for(goalIter = kLeftFootGoal; goalIter <= kRightHandGoal; goalIter++) + { + int32_t endIndex = apHuman->m_HumanBoneIndex[s_HumanGoalInfo[goalIter].m_EndIndex]; + + if(apHumanPose->m_GoalArray[goalIter].m_WeightR > 0) + { + float weightR = math::saturate(apHumanPose->m_GoalArray[goalIter].m_WeightR); + + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWorkspaceA,endIndex,hipsIndex); + + int32_t index = apHuman->m_HumanBoneIndex[s_HumanGoalInfo[goalIter].m_Index]; + + math::float4 q = AddAxis(apHuman,index,apSkeletonPoseWorkspaceA->m_X[index].q); + + q = math::quatLerp(q,apHumanPose->m_GoalArray[goalIter].m_X.q,math::float1(weightR)); + + q = RemoveAxis(apHuman,index,q); + + ReachGoalRotation(apHuman,q,goalIter,apSkeletonPose,apSkeletonPoseWorkspaceA,apSkeletonPoseWorkspaceB); + } + } + + /* no finger ik for 4.0 + if(apHuman->m_HasLeftHand) + { + if(apHumanPose->m_LeftHandPose.m_Grab > 0) + { + hand::HandPose pose; + hand::HandPoseSolve(&apHumanPose->m_LeftHandPose,&pose); + hand::Hand2SkeletonPose(apHuman->m_LeftHand,apHuman->m_Skeleton.Get(),&pose,apSkeletonPose); + + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWorkspace); + + hand::Hand *hand = apHuman->m_LeftHand; + + float wArray[hand::kLastFinger]; + math::float4 posArray[hand::kLastFinger]; + + hand::FingerTipsFromPose(hand,apHuman->m_Skeleton.Get(),apSkeletonPoseWorkspace,posArray); + + for(int i = 0; i < hand::kLastFinger; i++) + { + posArray[i] = SphereCollide(apHumanPose->m_LeftHandPose.m_GrabX,posArray[i]); + wArray[i] = apHumanPose->m_LeftHandPose.m_Grab; + } + + hand::FingersIKSolve(hand,apHuman->m_Skeleton.Get(),posArray,wArray,apSkeletonPose,apSkeletonPoseWorkspace); + } + } + + if(apHuman->m_HasRightHand) + { + if(apHumanPose->m_RightHandPose.m_Grab > 0) + { + hand::HandPose pose; + hand::HandPoseSolve(&apHumanPose->m_RightHandPose,&pose); + hand::Hand2SkeletonPose(apHuman->m_RightHand,apHuman->m_Skeleton.Get(),&pose,apSkeletonPose); + + skeleton::SkeletonPoseComputeGlobal(apHuman->m_Skeleton.Get(),apSkeletonPose,apSkeletonPoseWorkspace); + + hand::Hand *hand = apHuman->m_RightHand; + + float wArray[hand::kLastFinger]; + math::float4 posArray[hand::kLastFinger]; + + hand::FingerTipsFromPose(hand,apHuman->m_Skeleton.Get(),apSkeletonPoseWorkspace,posArray); + + for(int i = 0; i < hand::kLastFinger; i++) + { + posArray[i] = SphereCollide(apHumanPose->m_RightHandPose.m_GrabX,posArray[i]); + wArray[i] = apHumanPose->m_RightHandPose.m_Grab; + } + + hand::FingersIKSolve(hand,apHuman->m_Skeleton.Get(),posArray,wArray,apSkeletonPose,apSkeletonPoseWorkspace); + } + } + */ + } + + void TwistSolve(Human const *apHuman, skeleton::SkeletonPose *apSkeletonPose, skeleton::SkeletonPose *skeletonPoseWorkspace) + { + const math::float1 foreArmTwist(apHuman->m_ForeArmTwist); + const math::float1 armTwist(apHuman->m_ArmTwist); + const math::float1 legTwist(apHuman->m_LegTwist); + const math::float1 upperLegTwist(apHuman->m_UpperLegTwist); + + HumanFixTwist(apHuman,apSkeletonPose,skeletonPoseWorkspace,kLeftLowerArm,kLeftHand,foreArmTwist); + HumanFixTwist(apHuman,apSkeletonPose,skeletonPoseWorkspace,kLeftUpperArm,kLeftLowerArm,armTwist); + + HumanFixTwist(apHuman,apSkeletonPose,skeletonPoseWorkspace,kRightLowerArm,kRightHand,foreArmTwist); + HumanFixTwist(apHuman,apSkeletonPose,skeletonPoseWorkspace,kRightUpperArm,kRightLowerArm,armTwist); + + HumanFixTwist(apHuman,apSkeletonPose,skeletonPoseWorkspace,kLeftLowerLeg,kLeftFoot,legTwist); + HumanFixTwist(apHuman,apSkeletonPose,skeletonPoseWorkspace,kLeftUpperLeg,kLeftLowerLeg,upperLegTwist); + + HumanFixTwist(apHuman,apSkeletonPose,skeletonPoseWorkspace,kRightLowerLeg,kRightFoot,legTwist); + HumanFixTwist(apHuman,apSkeletonPose,skeletonPoseWorkspace,kRightUpperLeg,kRightLowerLeg,upperLegTwist); + } + + float ComputeHierarchicMass(int32_t aBoneIndex,float *apMassArray) + { + apMassArray[aBoneIndex] = HumanBoneDefaultMass[aBoneIndex]; + + for(int childIter = 0; childIter < BoneChildren[aBoneIndex][0]; childIter++) + { + apMassArray[aBoneIndex] += ComputeHierarchicMass(BoneChildren[aBoneIndex][1+childIter],apMassArray); + } + + return apMassArray[aBoneIndex]; + } + + float DeltaPoseQuality(HumanPose &arDeltaPose, float aTol) + { + float massArray[kLastBone]; + ComputeHierarchicMass(0,massArray); + + float q = 0; + float sumW = 0; + + for(int dofIter = 0; dofIter < kLastDoF; dofIter++) + { + int32_t boneIndex = DoF2Bone[dofIter]; + + float v = math::saturate((aTol - math::abs(arDeltaPose.m_DoFArray[dofIter]))/aTol); + + q += v * massArray[boneIndex]; + + sumW += massArray[boneIndex]; + } + + return q / sumW; + } + +} // namespace human + +} diff --git a/Runtime/mecanim/human/human.h b/Runtime/mecanim/human/human.h new file mode 100644 index 0000000..5f990b5 --- /dev/null +++ b/Runtime/mecanim/human/human.h @@ -0,0 +1,390 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/mecanim/bitset.h" +#include "Runtime/Math/Simd/xform.h" +#include "Runtime/mecanim/math/axes.h" +#include "Runtime/mecanim/math/collider.h" + +#include "Runtime/mecanim/human/handle.h" +#include "Runtime/mecanim/human/hand.h" + +#include "Runtime/Serialize/Blobification/offsetptr.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Animation/MecanimArraySerialization.h" + +namespace mecanim +{ + +namespace skeleton { struct Skeleton; struct SkeletonPose; } + +namespace human +{ + enum Bones + { + kHips = 0, + kLeftUpperLeg, + kRightUpperLeg, + kLeftLowerLeg, + kRightLowerLeg, + kLeftFoot, + kRightFoot, + kSpine, + kChest, + kNeck, + kHead, + kLeftShoulder, + kRightShoulder, + kLeftUpperArm, + kRightUpperArm, + kLeftLowerArm, + kRightLowerArm, + kLeftHand, + kRightHand, + kLeftToes, + kRightToes, + kLeftEye, + kRightEye, + kJaw, + kLastBone + }; + + enum BodyDoF + { + kSpineFrontBack = 0, + kSpineLeftRight, + kSpineRollLeftRight, + kChestFrontBack, + kChestLeftRight, + kChestRollLeftRight, + kLastBodyDoF + }; + + enum HeadDoF + { + kNeckFrontBack = 0, + kNeckLeftRight, + kNeckRollLeftRight, + kHeadFrontBack, + kHeadLeftRight, + kHeadRollLeftRight, + kLeftEyeDownUp, + kLeftEyeLeftRight, + kRightEyeDownUp, + kRightEyeLeftRight, + kJawDownUp, + kJawLeftRight, + kLastHeadDoF + }; + + enum LegDoF + { + kUpperLegFrontBack = 0, + kUpperLegInOut, + kUpperLegRollInOut, + kLegCloseOpen, + kLegRollInOut, + kFootCloseOpen, + kFootInOut, + kToesUpDown, + kLastLegDoF + }; + + enum ArmDoF + { + kShoulderDownUp = 0, + kShoulderFrontBack, + kArmDownUp, + kArmFrontBack, + kArmRollInOut, + kForeArmCloseOpen, + kForeArmRollInOut, + kHandDownUp, + kHandInOut, + kLastArmDoF + }; + + enum DoF + { + kBodyDoFStart = 0, + kHeadDoFStart = kBodyDoFStart + kLastBodyDoF, + kLeftLegDoFStart = kHeadDoFStart + kLastHeadDoF, + kRightLegDoFStart = kLeftLegDoFStart + kLastLegDoF, + kLeftArmDoFStart = kRightLegDoFStart + kLastLegDoF, + kRightArmDoFStart = kLeftArmDoFStart + kLastArmDoF, + kLastDoF = kRightArmDoFStart + kLastArmDoF + }; + + enum Goal + { + kLeftFootGoal, + kRightFootGoal, + kLeftHandGoal, + kRightHandGoal, + kLastGoal + }; + + struct GoalInfo + { + int32_t m_Index; + int32_t m_TopIndex; + int32_t m_MidIndex; + int32_t m_EndIndex; + }; + + const static GoalInfo s_HumanGoalInfo[kLastGoal] = + { + { kLeftFoot, kLeftUpperLeg, kLeftLowerLeg, kLeftFoot }, + { kRightFoot, kRightUpperLeg, kRightLowerLeg, kRightFoot }, + { kLeftHand, kLeftUpperArm, kLeftLowerArm, kLeftHand }, + { kRightHand, kRightUpperArm, kRightLowerArm, kRightHand } + }; + + enum HumanPoseMaskInfo + { + kMaskRootIndex = 0, + kMaskDoFStartIndex = kMaskRootIndex + 1, + kMaskGoalStartIndex = kMaskDoFStartIndex + kLastDoF, + kMaskLeftHand = kMaskGoalStartIndex + kLastGoal, + kMaskRightHand = kMaskLeftHand + 1, + kLastMaskIndex = kMaskRightHand +1 + }; + + typedef mecanim::bitset<kLastMaskIndex> HumanPoseMask; + + bool MaskHasLegs(const HumanPoseMask& mask); + + HumanPoseMask FullBodyMask(); + + struct Human + { + DEFINE_GET_TYPESTRING(Human) + + Human(); + + math::xform m_RootX; + + OffsetPtr<skeleton::Skeleton> m_Skeleton; + OffsetPtr<skeleton::SkeletonPose> m_SkeletonPose; + OffsetPtr<hand::Hand> m_LeftHand; + OffsetPtr<hand::Hand> m_RightHand; + + uint32_t m_HandlesCount; + OffsetPtr<human::Handle> m_Handles; + + uint32_t m_ColliderCount; + OffsetPtr<math::Collider> m_ColliderArray; + + int32_t m_HumanBoneIndex[kLastBone]; + float m_HumanBoneMass[kLastBone]; + int32_t m_ColliderIndex[kLastBone]; + + + float m_Scale; + + float m_ArmTwist; + float m_ForeArmTwist; + float m_UpperLegTwist; + float m_LegTwist; + + float m_ArmStretch; + float m_LegStretch; + + float m_FeetSpacing; + + bool m_HasLeftHand; + bool m_HasRightHand; + + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_RootX); + + TRANSFER(m_Skeleton); + TRANSFER(m_SkeletonPose); + TRANSFER(m_LeftHand); + TRANSFER(m_RightHand); + + TRANSFER_BLOB_ONLY(m_HandlesCount); + MANUAL_ARRAY_TRANSFER2(Handle, m_Handles, m_HandlesCount); + + TRANSFER_BLOB_ONLY(m_ColliderCount); + MANUAL_ARRAY_TRANSFER2(math::Collider, m_ColliderArray, m_ColliderCount); + + STATIC_ARRAY_TRANSFER(mecanim::int32_t, m_HumanBoneIndex, kLastBone); + STATIC_ARRAY_TRANSFER(float, m_HumanBoneMass, kLastBone); + STATIC_ARRAY_TRANSFER(mecanim::int32_t, m_ColliderIndex, kLastBone); + + TRANSFER(m_Scale); + + TRANSFER(m_ArmTwist); + TRANSFER(m_ForeArmTwist); + TRANSFER(m_UpperLegTwist); + TRANSFER(m_LegTwist); + + TRANSFER(m_ArmStretch); + TRANSFER(m_LegStretch); + + TRANSFER(m_FeetSpacing); + + TRANSFER(m_HasLeftHand); + TRANSFER(m_HasRightHand); + transfer.Align(); + } + }; + + struct HumanGoal + { + DEFINE_GET_TYPESTRING(HumanGoal) + + HumanGoal() : m_WeightT(0.0f), m_WeightR(0.0f) {}; + + math::xform m_X; + float m_WeightT; + float m_WeightR; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_X); + TRANSFER(m_WeightT); + TRANSFER(m_WeightR); + } + }; + + struct HumanPose + { + DEFINE_GET_TYPESTRING(HumanPose) + + HumanPose(); + + math::xform m_RootX; + math::float4 m_LookAtPosition; + math::float4 m_LookAtWeight; + + HumanGoal m_GoalArray[kLastGoal]; + hand::HandPose m_LeftHandPose; + hand::HandPose m_RightHandPose; + + float m_DoFArray[kLastDoF]; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_RootX); + TRANSFER(m_LookAtPosition); + TRANSFER(m_LookAtWeight); + + STATIC_ARRAY_TRANSFER(HumanGoal, m_GoalArray, kLastGoal); + TRANSFER(m_LeftHandPose); + TRANSFER(m_RightHandPose); + + STATIC_ARRAY_TRANSFER(float, m_DoFArray, kLastDoF); + } + }; + + int32_t MuscleFromBone(int32_t boneIndex, int32_t doFIndex); + int32_t BoneFromMuscle(int32_t doFIndex); + + bool RequiredBone(uint32_t boneIndex); + const char* BoneName(uint32_t boneIndex); + const char* MuscleName(uint32_t boneIndex); + + Human* CreateHuman(skeleton::Skeleton *skeleton,skeleton::SkeletonPose *skeletonPose, uint32_t handlesCount, uint32_t colliderCount, memory::Allocator& alloc); + void DestroyHuman(Human *human, memory::Allocator& alloc); + + void HumanAdjustMass(Human *human); + void HumanSetupAxes(Human *human, skeleton::SkeletonPose const *skeletonPoseGlobal); + void HumanSetupCollider(Human *human, skeleton::SkeletonPose const *skeletonPoseGlobal); + void HumanCopyAxes(Human const *srcHuman, Human *human); + math::Axes GetAxes(Human const *human, int32_t boneIndex); + void GetMuscleRange(Human const *apHuman, int32_t aDoFIndex, float &aMin, float &aMax); + math::float4 AddAxis(Human const *human, int32_t index, math::float4 const &q); + math::float4 RemoveAxis(Human const *human, int32_t index, const math::float4 &q); + math::xform NormalizedHandleX(Human const *human, int32_t handleIndex); + + math::float4 HumanComputeBoneMassCenter(Human const *human, skeleton::SkeletonPose const *skeletonPoseGlobal, int32_t boneIndex); + math::float4 HumanComputeMassCenter(Human const *human, skeleton::SkeletonPose const *skeletonPoseGlobal); + float HumanComputeMomentumOfInertia(Human const *human, skeleton::SkeletonPose const *skeletonPoseGlobal); + math::float4 HumanComputeOrientation(Human const* human,skeleton::SkeletonPose const* apPoseGlobal); + math::xform HumanComputeRootXform(Human const* human,skeleton::SkeletonPose const* apPoseGlobal); + float HumanGetFootHeight(Human const* human, bool left); + math::float4 HumanGetFootBottom(Human const* human, bool left); + math::xform HumanGetColliderXform(Human const* human, math::xform const& x, int32_t boneIndex); + math::xform HumanSubColliderXform(Human const* human, math::xform const& x, int32_t boneIndex); + math::float4 HumanGetGoalOrientationOffset(Goal goalIndex); + + void HumanPoseClear(HumanPose& pose); + void HumanPoseCopy(HumanPose &pose,HumanPose const &poseA, bool doFOnly = false); + void HumanPoseCopy(HumanPose &pose,HumanPose const &poseA, HumanPoseMask const &humanPoseMask); + void HumanPoseAdd(HumanPose &pose,HumanPose const &poseA,HumanPose const &poseB); + void HumanPoseSub(HumanPose &pose,HumanPose const &poseA,HumanPose const &poseB); + void HumanPoseWeight(HumanPose &pose,HumanPose const &poseA, float weight); + void HumanPoseMirror(HumanPose &pose,HumanPose const &poseA); + void HumanPoseBlend(HumanPose &pose, HumanPose **poseArray, float *weightArray, uint32_t count); + void HumanPoseAddOverrideLayer(HumanPose &poseBase,HumanPose const &pose, float weight, HumanPoseMask const &humanPoseMask); + void HumanPoseAddAdditiveLayer(HumanPose &poseBase,HumanPose const &pose, float weight, HumanPoseMask const &humanPoseMask); + + void RetargetFrom( Human const *human, + skeleton::SkeletonPose const *skeletonPose, + HumanPose *humanPose, + skeleton::SkeletonPose *skeletonPoseWsRef, + skeleton::SkeletonPose *skeletonPoseWsGbl, + skeleton::SkeletonPose *skeletonPoseWsLcl, + skeleton::SkeletonPose *skeletonPoseWsWs, + int32_t maxFixIter = 5); + + void RetargetTo( Human const *human, + HumanPose const *humanPoseBase, + HumanPose const *humanPose, + const math::xform &x, + HumanPose *humanPoseOut, + skeleton::SkeletonPose *skeletonPose, + skeleton::SkeletonPose *skeletonPoseWs); + + // apSkeletonPoseWorkspace must be set to global pose before calling + void FullBodySolve(Human const *human, HumanPose const *humanPose, skeleton::SkeletonPose *skeletonPose, skeleton::SkeletonPose *skeletonPoseWorkspaceA, skeleton::SkeletonPose *skeletonPoseWorkspaceB); + void TwistSolve(Human const *human, skeleton::SkeletonPose *skeletonPose, skeleton::SkeletonPose *skeletonPoseWorkspace); + + float DeltaPoseQuality(HumanPose &deltaPose, float tolerance = 0.15f); + + skeleton::SetupAxesInfo const& GetAxeInfo(uint32_t index); + +}// namespace human + +} + +template<> +class SerializeTraits< mecanim::human::HumanPoseMask > : public SerializeTraitsBase< mecanim::human::HumanPoseMask > +{ +public: + + inline static const char* GetTypeString (value_type*) { return "HumanPoseMask"; } + inline static bool IsAnimationChannel () { return false; } + inline static bool MightContainPPtr () { return false; } + inline static bool AllowTransferOptimization () { return true; } + inline static bool IsContinousMemoryArray () { return true; } + + typedef mecanim::human::HumanPoseMask value_type; + + template<class TransferFunction> inline + static void Transfer (value_type& data, TransferFunction& transfer) + { + transfer.Transfer(data.word(0), "word0"); + if(1<value_type::Words+1) + transfer.Transfer(data.word(1), "word1"); + if(2<value_type::Words+1) + transfer.Transfer(data.word(2), "word2"); + if(3<value_type::Words+1) + transfer.Transfer(data.word(3), "word3"); + } + + static void resource_image_assign_external (value_type& data, void* begin, void* end) + { + } +}; + + diff --git a/Runtime/mecanim/math/axes.h b/Runtime/mecanim/math/axes.h new file mode 100644 index 0000000..b495ea7 --- /dev/null +++ b/Runtime/mecanim/math/axes.h @@ -0,0 +1,128 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/Math/Simd/math.h" +#include "Runtime/Math/Simd/quaternion.h" + +namespace math +{ + struct Limit + { + DEFINE_GET_TYPESTRING(Limit) + + float4 m_Min; // m_Min > 0 -> free, m_Min == 0 -> lock, m_Min < 0 -> limit + float4 m_Max; // m_Max < 0 -> free, m_Max == 0 -> lock, m_Max > 0 -> limit + + inline Limit() : m_Min(1,1,1,1), m_Max(-1,-1,-1,-1) {} + inline Limit(float4 const& aMin, float4 const& aMax, float aRange) { m_Min = aMin; m_Max = aMax; } + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_Min); + TRANSFER(m_Max); + } + }; + + enum AxesType { kFull, kZYRoll, kRollZY, kEulerXYZ }; + + struct Axes + { + DEFINE_GET_TYPESTRING(Axes) + + float4 m_PreQ; + float4 m_PostQ; + float4 m_Sgn; + Limit m_Limit; + float m_Length; + mecanim::uint32_t m_Type; // AxesType + + inline Axes() : m_PreQ(0,0,0,1), m_PostQ(0,0,0,1), m_Sgn(1,1,1,1), m_Length(1), m_Type(kEulerXYZ) {} + inline Axes(float4 const& aPreQ, float4 const& aPostQ, float4 const& aSgn, float const &aLength, AxesType const& aType) { m_PreQ = aPreQ; m_PostQ = aPostQ; m_Sgn = aSgn; m_Length = aLength; m_Type = aType; } + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_PreQ); + TRANSFER(m_PostQ); + TRANSFER(m_Sgn); + TRANSFER(m_Limit); + TRANSFER(m_Length); + TRANSFER(m_Type); + } + }; + + STATIC_INLINE float4 LimitSmootClamp(Limit const& l, float4 const& v, float1 const& smoothRange) + { + const float4 min = cond(l.m_Min<float1::zero(), -smoothClamp(-v,float4::one(),smoothRange), cond(l.m_Min>float1::zero(),v,float4::zero())); + const float4 max = cond(l.m_Max>float1::zero(), smoothClamp(v,float4::one(),smoothRange), cond(l.m_Max<float1::zero(),v,float4::zero())); + return math::cond(v<float1::zero(),min,max); + } + + STATIC_INLINE float LimitProject(float min, float max, float v) + { + float i = min < 0 ? -v / min : min > 0 ? v : 0; + float a = max > 0 ? +v / max : max < 0 ? v : 0; + return v < 0 ? i : a; + } + + STATIC_INLINE float LimitUnproject(float min, float max, float v) + { + float i = min < 0 ? -v * min : min > 0 ? v : 0; + float a = max > 0 ? +v * max : max < 0 ? v : 0; + return v < 0 ? i : a; + } + + STATIC_INLINE float4 LimitProject(Limit const& l, float4 const& v) + { + const float4 min = cond(l.m_Min<float1::zero(),-v/l.m_Min,cond(l.m_Min>float1::zero(),v,float4::zero())); + const float4 max = cond(l.m_Max>float1::zero(),+v/l.m_Max,cond(l.m_Max<float1::zero(),v,float4::zero())); + return math::cond(v<float1::zero(),min,max); + } + + STATIC_INLINE float4 LimitUnproject(Limit const& l, float4 const& v) + { + const float4 min = cond(l.m_Min<float1::zero(),-v*l.m_Min,cond(l.m_Min>float1::zero(),v,float4::zero())); + const float4 max = cond(l.m_Max>float1::zero(),+v*l.m_Max,cond(l.m_Max<float1::zero(),v,float4::zero())); + return math::cond(v<float1::zero(),min,max); + } + + STATIC_INLINE float4 AxesProject(Axes const& a, float4 const& q) + { + return normalize(quatMul(quatConj(a.m_PreQ),quatMul(q,a.m_PostQ))); + } + + STATIC_INLINE float4 AxesUnproject(Axes const& a, float4 const& q) + { + return normalize(quatMul(a.m_PreQ,quatMul(q,quatConj(a.m_PostQ)))); + } + + STATIC_INLINE float4 ToAxes(Axes const& a, float4 const& q) + { + const float4 qp = AxesProject(a,q); + float4 xyz; + switch(a.m_Type) + { + case kEulerXYZ: xyz = LimitProject(a.m_Limit, quatQuatToEuler(qp)); break; + case kZYRoll: xyz = LimitProject(a.m_Limit,doubleAtan(quat2ZYRoll(qp)*sgn(a.m_Sgn))); break; + case kRollZY: xyz = LimitProject(a.m_Limit,doubleAtan(quat2RollZY(qp)*sgn(a.m_Sgn))); break; + default: xyz = LimitProject(a.m_Limit,doubleAtan(quat2Qtan(qp)*sgn(a.m_Sgn))); break; + }; + return xyz; + } + + STATIC_INLINE float4 FromAxes(Axes const& a, float4 const& uvw) + { + float4 q; + switch(a.m_Type) + { + case kEulerXYZ: q = quatEulerToQuat(uvw); break; + case kZYRoll: q = ZYRoll2Quat(halfTan(LimitUnproject(a.m_Limit,uvw))*sgn(a.m_Sgn)); break; + case kRollZY: q = RollZY2Quat(halfTan(LimitUnproject(a.m_Limit,uvw))*sgn(a.m_Sgn)); break; + default: q = qtan2Quat(halfTan(LimitUnproject(a.m_Limit,uvw))*sgn(a.m_Sgn)); break; + }; + + return AxesUnproject(a,q); + } +} diff --git a/Runtime/mecanim/math/collider.h b/Runtime/mecanim/math/collider.h new file mode 100644 index 0000000..234281a --- /dev/null +++ b/Runtime/mecanim/math/collider.h @@ -0,0 +1,55 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/Math/Simd/xform.h" + +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" + +namespace math +{ + enum ColliderType { kNone = 0, kCube, kSphere, kCylinder, kCapsule }; + enum JointType { kIgnored = 0, kLocked, kLimited }; + + struct Collider + { + DEFINE_GET_TYPESTRING(Collider) + + xform m_X; + mecanim::uint32_t m_Type; // ColliderType + + inline Collider() : m_Type(kCube) {} + + // Joint information + mecanim::uint32_t m_XMotionType; + mecanim::uint32_t m_YMotionType; + mecanim::uint32_t m_ZMotionType; + float m_MinLimitX; + float m_MaxLimitX; + float m_MaxLimitY; + float m_MaxLimitZ; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_X); + TRANSFER(m_Type); + TRANSFER(m_XMotionType); + TRANSFER(m_YMotionType); + TRANSFER(m_ZMotionType); + TRANSFER(m_MinLimitX); + TRANSFER(m_MaxLimitX); + TRANSFER(m_MaxLimitY); + TRANSFER(m_MaxLimitZ); + } + }; + + STATIC_INLINE float4 SphereCollide(math::xform const &sphereX,math::float4 const &pos) + { + math::float4 ret = math::xformInvMulVec(sphereX,pos); + + ret *= math::rcp(math::length(ret)); + + return math::xformMulVec(sphereX,ret); + } +} diff --git a/Runtime/mecanim/memory.h b/Runtime/mecanim/memory.h new file mode 100644 index 0000000..5331051 --- /dev/null +++ b/Runtime/mecanim/memory.h @@ -0,0 +1,618 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/Utilities/dynamic_array.h" +#include "Runtime/Utilities/FileUtilities.h" + +#include "Runtime/Allocator/MemoryManager.h" + +#include <assert.h> +#include <new> + +#if ENABLE_MECANIM_PROFILER +#include <fstream> +#include <stack> +#include <string.h> +#include <stdlib.h> +#include <ctime> +#endif + +template <typename TYPE> class OffsetPtr; + +namespace mecanim +{ + +namespace memory +{ +#if ENABLE_MECANIM_PROFILER + class Profiler + { + public: + + struct AllocationInfo + { + AllocationInfo():m_Size(0),m_Allocator("Unknow"){} + AllocationInfo(size_t size, char const* allocator):m_Size(size),m_Allocator(allocator){} + bool operator==(AllocationInfo const& allocationInfo)const + { + return m_Size == allocationInfo.m_Size && strcmp(m_Allocator, allocationInfo.m_Allocator) == 0; + } + size_t m_Size; + char const* m_Allocator; + }; + + + typedef std::map<void*, AllocationInfo> AllocationInfos; + struct ProfilerLabel + { + ProfilerLabel():m_Label("Unknow"), m_ExclusiveSize(0),m_InclusiveSize(0),m_Parent(0),m_Count(0){} + ProfilerLabel(char const* labelId):m_Label(labelId), m_ExclusiveSize(0),m_InclusiveSize(0),m_Parent(0),m_Count(0){} + ~ProfilerLabel() + { + for(int i=0;i<m_Children.size();i++) + { + m_Children[i]->~ProfilerLabel(); + MemoryManager::LowLevelFree(m_Children[i]); + } + + m_Children.clear(); + } + + char const* m_Label; + size_t m_ExclusiveSize; + size_t m_InclusiveSize; + size_t m_Count; + AllocationInfos m_Allocations; + + ProfilerLabel* m_Parent; + std::vector<ProfilerLabel*> m_Children; + }; + + Profiler():m_Root("Root"){} + ~Profiler(){} + + static void StaticInitialize() + { + s_Profiler = UNITY_NEW(Profiler, kMemDefault)(); + s_Profiler->m_Current = &s_Profiler->m_Root; + } + + static void StaticDestroy() + { + UNITY_DELETE(s_Profiler, kMemDefault); + } + + void PushLabel(char const * label) + { + m_LabelStack.push(label); + LabelMap::iterator it = m_Label.find(label); + if(it == m_Label.end()) + it = m_Label.insert( std::make_pair(label, ProfilerLabel(label) ) ).first; + + AssertIf(it == m_Label.end()); + it->second.m_Count++; + + s_Profiler->m_Current->m_Children.push_back( new (MemoryManager::LowLevelAllocate(sizeof(ProfilerLabel))) ProfilerLabel(label) ); + ProfilerLabel* newLabel = s_Profiler->m_Current->m_Children.back(); + newLabel->m_Parent = s_Profiler->m_Current; + s_Profiler->m_Current = newLabel; + } + + void PopLabel() + { + m_LabelStack.pop(); + + ProfilerLabel* parentLabel = s_Profiler->m_Current->m_Parent; + + // If not a single allocation had been made, remove it + if(s_Profiler->m_Current->m_Allocations.size() == 0 && s_Profiler->m_Current->m_Children.size() == 0) + { + parentLabel->m_Children[parentLabel->m_Children.size()-1]->~ProfilerLabel(); + MemoryManager::LowLevelFree(parentLabel->m_Children[parentLabel->m_Children.size()-1]); + + parentLabel->m_Children.resize( parentLabel->m_Children.size()-1 ); + } + + s_Profiler->m_Current = parentLabel; + + assert(s_Profiler->m_Current != NULL); + } + + void RegisterAllocation(void* ptr, char const* alloc, size_t size = 0) + { + AssertIf(m_LabelStack.empty()); + char const* label = m_LabelStack.empty() ? "Unknow" : m_LabelStack.top(); + + std::map<char const*, ProfilerLabel>::iterator it = m_Label.find(label); + if(it == m_Label.end()) + m_Label.insert( std::make_pair(label, ProfilerLabel(label) ) ); + + m_Label[label].m_Allocations.insert( std::make_pair(ptr, AllocationInfo(size, alloc) ) ); + m_Label[label].m_ExclusiveSize += size; + + s_Profiler->m_Current->m_Allocations.insert( std::make_pair(ptr, AllocationInfo(size, alloc) ) ); + s_Profiler->m_Current->m_ExclusiveSize += size; + s_Profiler->m_Current->m_InclusiveSize += size; + + ProfilerLabel* profilerLabel = s_Profiler->m_Current->m_Parent; + while(profilerLabel!=NULL) + { + profilerLabel->m_InclusiveSize += size; + profilerLabel = profilerLabel->m_Parent; + } + + m_PointerRegister.insert( std::make_pair(ptr, std::make_pair( &m_Label[label], s_Profiler->m_Current))); + } + + void UnregisterAllocation(void* ptr) + { + if(ptr == NULL) + return; + + PointerRegister::iterator it = m_PointerRegister.find(ptr); + + assert(it!=m_PointerRegister.end()); + + if(it!=m_PointerRegister.end()) + { + ProfilerLabel* label = it->second.first; + AllocationInfos::iterator itAlloc = label->m_Allocations.find(ptr); + if(itAlloc!=label->m_Allocations.end()) + { + label->m_ExclusiveSize -= itAlloc->second.m_Size; + label->m_Allocations.erase(itAlloc); + if(label->m_Allocations.size() == 0 ) + m_Label.erase( m_Label.find(label->m_Label) ); + } + + label = it->second.second; + itAlloc = label->m_Allocations.find(ptr); + if(itAlloc != label->m_Allocations.end()) + { + size_t size = itAlloc->second.m_Size; + label->m_ExclusiveSize -= size; + label->m_InclusiveSize -= size; + label->m_Allocations.erase(itAlloc); + + ProfilerLabel* parentLabel = label->m_Parent; + + if(parentLabel != NULL && label->m_Allocations.size() == 0 && label->m_Children.size() == 0) + { + parentLabel->m_Children.erase( std::find(parentLabel->m_Children.begin(), parentLabel->m_Children.end(), label )); + label->~ProfilerLabel(); + MemoryManager::LowLevelFree(label); + } + + while(parentLabel!=NULL) + { + label = parentLabel; + parentLabel = label->m_Parent; + + label->m_InclusiveSize -= size; + if(parentLabel != NULL && label->m_Allocations.size() == 0 && label->m_Children.size() == 0) + { + parentLabel->m_Children.erase( std::find(parentLabel->m_Children.begin(), parentLabel->m_Children.end(), label )); + label->~ProfilerLabel(); + MemoryManager::LowLevelFree(label); + } + } + } + + m_PointerRegister.erase(it); + } + } + + static Profiler* GetProfiler() + { + return s_Profiler; + } + + void DumpObjectTypeMemoryInfo() + { + time_t rawtime; + struct tm * timeinfo; + + time (&rawtime); + timeinfo = localtime (&rawtime); + + string tempDir = Format("%s/ObjectTypeMemoryInfo_%.2d_%.2d_%.2d_%.2dh%.2dm%.2ds.csv", + GetApplicationFolder().c_str(), + 1900 + timeinfo->tm_year, + timeinfo->tm_mon, + timeinfo->tm_mday, + timeinfo->tm_hour, + timeinfo->tm_min, + timeinfo->tm_sec); + std::ofstream file(tempDir); + + file << "Object, Size, Count, Allocation Count" << std::endl; + + LabelMap::iterator it; + for(it=m_Label.begin();it!=m_Label.end();it++) + file << it->second.m_Label << "," << it->second.m_ExclusiveSize << "," << it->second.m_Count << "," << it->second.m_Allocations.size() << std::endl; + + file.close(); + } + + void DumpCallSiteMemoryInfo() + { + time_t rawtime; + struct tm * timeinfo; + + time (&rawtime); + timeinfo = localtime (&rawtime); + + string tempDir = Format("%s/CallSiteMemoryInfo_%.2d_%.2d_%.2d_%.2dh%.2dm%.2ds.csv", + GetApplicationFolder().c_str(), + 1900 + timeinfo->tm_year, + timeinfo->tm_mon, + timeinfo->tm_mday, + timeinfo->tm_hour, + timeinfo->tm_min, + timeinfo->tm_sec); + //std::ofstream file(tempDir); + //file.close(); + } + + protected: + static Profiler* s_Profiler; + + std::stack<char const*> m_LabelStack; + + typedef std::map<char const*, ProfilerLabel> LabelMap; + LabelMap m_Label; + + typedef std::map<void *, std::pair<ProfilerLabel*, ProfilerLabel*> > PointerRegister; + PointerRegister m_PointerRegister; + + ProfilerLabel m_Root; + ProfilerLabel* m_Current; + }; +#else + class Profiler + { + public:Profiler(){} + ~Profiler(){} + + static void StaticInitialize() + { + s_Profiler = UNITY_NEW(Profiler, kMemDefault)(); + } + + static void StaticDestroy() + { + UNITY_DELETE(s_Profiler, kMemDefault); + } + + void PushLabel(char const * label){ } + + void PopLabel(){} + + void RegisterAllocation(void* ptr, char const* alloc, size_t size = 0){ } + + void UnregisterAllocation(void* ptr){} + + void DumpObjectTypeMemoryInfo(){} + + void DumpCallSiteMemoryInfo(){} + + static Profiler* GetProfiler() + { + return s_Profiler; + } + protected: + static Profiler* s_Profiler; + }; +#endif + + class AutoScopeProfiler + { + public: + AutoScopeProfiler(char const* label) { Profiler::GetProfiler()->PushLabel(label); } + ~AutoScopeProfiler() { Profiler::GetProfiler()->PopLabel();} + }; + + #define SETPROFILERLABEL(type) mecanim::memory::AutoScopeProfiler scopeProfiler##type(#type) + + class Allocator + { + public: + std::size_t AlignAddress(std::size_t aAddr, std::size_t aAlign) + { + return aAddr + ((~aAddr + 1U) & (aAlign - 1U)); + } + + virtual void* Allocate(std::size_t size, std::size_t align) = 0 ; + virtual void Deallocate(void * p) = 0; + + template <typename TYPE> + TYPE* Construct(std::size_t align = ALIGN_OF(TYPE)) + { + + char *ptr = reinterpret_cast<char *>(Allocate(sizeof(TYPE), align)); + return new( (void *) ptr) TYPE; + } + + template <typename TYPE> + TYPE* ConstructArray(std::size_t count, std::size_t align = ALIGN_OF(TYPE)) + { + if(count > 0) + { + char *ptr = reinterpret_cast<char *>(Allocate(sizeof(TYPE)*count, align)); + return new( (void *) ptr) TYPE[count]; + } + else + { + return 0; + } + } + + template <typename TYPE> + TYPE* ConstructArray(const TYPE* input, std::size_t count, std::size_t align = ALIGN_OF(TYPE)) + { + if(count > 0) + { + TYPE *ptr = reinterpret_cast<TYPE*> (Allocate(sizeof(TYPE)*count, align)); + memcpy(ptr, input, sizeof(TYPE)*count); + return ptr; + } + else + { + return 0; + } + } + + template <typename TYPE> + void Deallocate(OffsetPtr<TYPE>& p) + { + if(!p.IsNull()) + Deallocate(p.Get()); + } + }; + + // Should be constructed with either: + // kMemAnimation + // kMemAnimationTemp + class MecanimAllocator : public Allocator + { + MemLabelId m_Label; + protected: + MecanimAllocator(MecanimAllocator const& e):Allocator(){ m_Label = e.m_Label; m_Label.SetRootHeader(GET_CURRENT_ALLOC_ROOT_HEADER());} + MecanimAllocator& operator=(MecanimAllocator const &){ return *this; } + public: + MecanimAllocator(MemLabelId label):m_Label(label){ m_Label.SetRootHeader(GET_CURRENT_ALLOC_ROOT_HEADER());} + + virtual void* Allocate(std::size_t size, std::size_t align) + { + void* p = UNITY_MALLOC_ALIGNED(m_Label, size, align); + + Profiler::GetProfiler()->RegisterAllocation(p, "MecanimAllocator", size); + + return p; + } + + virtual void Deallocate(void * p) + { + Profiler::GetProfiler()->UnregisterAllocation(p); + UNITY_FREE(m_Label, p); + } + }; + + class InPlaceAllocator : public Allocator + { + protected: + char* mP; + char* mHead; + std::size_t mMaxSize; + InPlaceAllocator(InPlaceAllocator const &):Allocator(){} + InPlaceAllocator& operator=(InPlaceAllocator const &){ return *this; } + public: + + + InPlaceAllocator(void *p, std::size_t aSize):mP(reinterpret_cast<char *>(p)),mHead(reinterpret_cast<char *>(p)), mMaxSize(aSize){}; + +// InPlaceAllocator(size_t size, MemoryLabelId label) +// { +// mHead = mP = UNITY_MALLOC_ALIGNED(label, size, 16); +// mMaxSize = aSize; +// +// } + + ~InPlaceAllocator() { } + + inline std::size_t TotalMemorySize()const { return mMaxSize; } + inline std::size_t UsedMemorySize()const { return reinterpret_cast<std::size_t>(mP) - reinterpret_cast<std::size_t>(mHead); } + inline std::size_t FreeMemorySize()const { return TotalMemorySize() - UsedMemorySize(); } + + void* GetMemory () + { + return mP; + } + + virtual void* Allocate(std::size_t n, std::size_t align) + { + char *p = reinterpret_cast<char *>(AlignAddress(reinterpret_cast<std::size_t>(mP), align)); + + // The first allocation must always be aligned already + AssertIf(mHead == mP && p != mHead); + + // assert(p+n <= mHead+mMaxSize); + + if(p+n <= mHead+mMaxSize) + { + mP = p+n; + return reinterpret_cast<void *>(p); + } + else + { + return 0; + } + } + + virtual void Deallocate(void * p) + { + AssertString("Not supported"); + } + }; + + class ChainedAllocator : public Allocator + { + ProfilerAllocationHeader* rootHeader; + protected: + + enum { + BlockAlign = 16 + }; + + struct MemoryBlock { + MemoryBlock* next; + uint8_t* headPtr; + std::size_t blockSize; + }; + + ChainedAllocator(ChainedAllocator const&):Allocator(){rootHeader = GET_CURRENT_ALLOC_ROOT_HEADER();} + ChainedAllocator& operator=(ChainedAllocator const &){ return *this; } + + std::size_t GetMemBlockSize(std::size_t aSize, std::size_t aAlign) + { + return sizeof(MemoryBlock) + aSize + aAlign - 1U; + } + + std::size_t GetAllocateSize(std::size_t aSize, std::size_t aAlign) + { + return aSize + aAlign - 1U; + } + + MemoryBlock* first; + MemoryBlock* current; + uint8_t* heapPtr; + std::size_t blockSize; + public: + ChainedAllocator(std::size_t aBlockSize):first(0),current(0),blockSize(aBlockSize) + { + rootHeader = GET_CURRENT_ALLOC_ROOT_HEADER(); + } + + ~ChainedAllocator() + { + Reset(); + } + + void Init() + { + if(first == 0) + { + size_t size = GetMemBlockSize(blockSize, BlockAlign); + void *p = UNITY_MALLOC(MemLabelId(kMemAnimationId,rootHeader), size); + Profiler::GetProfiler()->RegisterAllocation(p, "ChainedAllocator", size); + if(p) + { + current = first = new(p) MemoryBlock; + current->next = 0; + current->blockSize = blockSize; + + uint8_t* head = reinterpret_cast<uint8_t*>(p); + heapPtr = current->headPtr = reinterpret_cast<uint8_t*>( AlignAddress( reinterpret_cast<std::size_t>(head + sizeof(MemoryBlock)), BlockAlign)); + } + } + } + + void Reset() + { + MemoryBlock* c = first; + while(c != 0) + { + void* p = reinterpret_cast<void*>(c); + c = c->next; + Profiler::GetProfiler()->UnregisterAllocation(p); + UNITY_FREE(kMemAnimation, p); + } + current = first = 0; + heapPtr = 0; + } + + void Reserve(std::size_t size) + { + if(size > 0) + { + if(first == 0) + { + size_t size1 = GetMemBlockSize(size, BlockAlign); + void *p = UNITY_MALLOC(MemLabelId(kMemAnimationId,rootHeader), size1); + Profiler::GetProfiler()->RegisterAllocation(p, "ChainedAllocator", size1); + + if(p) + { + current = first = new(p) MemoryBlock; + current->next = 0; + current->blockSize = size; + + uint8_t* head = reinterpret_cast<uint8_t*>(p); + heapPtr = current->headPtr = reinterpret_cast<uint8_t*>( AlignAddress( reinterpret_cast<std::size_t>(head + sizeof(MemoryBlock)), BlockAlign)); + } + } + else + { + size_t size1 = GetMemBlockSize(size, BlockAlign); + void *p = UNITY_MALLOC(MemLabelId(kMemAnimationId,rootHeader), size1); + Profiler::GetProfiler()->RegisterAllocation(p, "ChainedAllocator", size1); + if(p) + { + current->next = new(p) MemoryBlock; + current = current->next; + current->next = 0; + current->blockSize = size > blockSize ? size : blockSize; + + uint8_t* head = reinterpret_cast<uint8_t*>(p); + heapPtr = current->headPtr = reinterpret_cast<uint8_t*>( AlignAddress( reinterpret_cast<std::size_t>(head + sizeof(MemoryBlock)), BlockAlign)); + } + } + } + } + + // Limit case are + // no memory left in current block, allocate a new block + // no memory left in system, bail out + virtual void* Allocate(std::size_t size, std::size_t align) + { + Init(); + + std::size_t s = GetAllocateSize(size, align); + + // Not enough memory left in current block, allocate a new block + if( heapPtr + s > current->headPtr + current->blockSize) + { + // If not enough memory left in system, bail out + // If the requested size is bigger than the block size, allocate at least a block big enough for this request. + size_t size1 = GetMemBlockSize(size > blockSize ? size : blockSize, BlockAlign); + void *p = UNITY_MALLOC(MemLabelId(kMemAnimationId,rootHeader), size1); + Profiler::GetProfiler()->RegisterAllocation(p, "ChainedAllocator", size1); + if(p) + { + current->next = new(p) MemoryBlock; + current = current->next; + current->next = 0; + current->blockSize = size > blockSize ? size : blockSize; + + uint8_t* head = reinterpret_cast<uint8_t*>(p); + heapPtr = current->headPtr = reinterpret_cast<uint8_t*>( AlignAddress( reinterpret_cast<std::size_t>(head + sizeof(MemoryBlock)), BlockAlign)); + } + else + return 0; + } + + uint8_t *p = reinterpret_cast<uint8_t *>(AlignAddress(reinterpret_cast<std::size_t>(heapPtr), align)); + heapPtr = p+size; + return reinterpret_cast<void *>(p); + } + + virtual void Deallocate(void * p) + { + assert(true); + } + }; + +} + +} diff --git a/Runtime/mecanim/skeleton/skeleton.cpp b/Runtime/mecanim/skeleton/skeleton.cpp new file mode 100644 index 0000000..5b78677 --- /dev/null +++ b/Runtime/mecanim/skeleton/skeleton.cpp @@ -0,0 +1,515 @@ +#include "UnityPrefix.h" +#include "Runtime/mecanim/skeleton/skeleton.h" + +namespace mecanim +{ + +namespace skeleton +{ + Skeleton* CreateSkeleton(int32_t aNodeCount, int32_t aAxesCount, memory::Allocator& arAlloc) + { + Skeleton* skeleton = arAlloc.Construct<Skeleton>(); + + skeleton->m_Count = aNodeCount; + skeleton->m_Node = arAlloc.ConstructArray<Node>(aNodeCount); + skeleton->m_ID = arAlloc.ConstructArray<uint32_t>(aNodeCount); + + skeleton->m_AxesCount = aAxesCount; + if(skeleton->m_AxesCount) + { + skeleton->m_AxesArray = arAlloc.ConstructArray<math::Axes>(aAxesCount); + } + + return skeleton; + } + + SkeletonPose* CreateSkeletonPose(Skeleton const* apSkeleton, memory::Allocator& arAlloc) + { + SkeletonPose* skeletonPose = arAlloc.Construct<SkeletonPose>(); + + skeletonPose->m_Count = apSkeleton->m_Count; + skeletonPose->m_X = arAlloc.ConstructArray<math::xform>(apSkeleton->m_Count); + + return skeletonPose; + } + + void DestroySkeleton(Skeleton* apSkeleton, memory::Allocator& arAlloc) + { + if(apSkeleton) + { + arAlloc.Deallocate(apSkeleton->m_Node); + arAlloc.Deallocate(apSkeleton->m_ID); + arAlloc.Deallocate(apSkeleton->m_AxesArray); + + arAlloc.Deallocate(apSkeleton); + } + } + + void DestroySkeletonPose(SkeletonPose* apSkeletonPose, memory::Allocator& arAlloc) + { + if(apSkeletonPose) + { + arAlloc.Deallocate(apSkeletonPose->m_X); + arAlloc.Deallocate(apSkeletonPose); + } + } + + SkeletonMask* CreateSkeletonMask(uint32_t aNodeCount, SkeletonMaskElement* elements, memory::Allocator& arAlloc) + { + SkeletonMask* skeletonMask = arAlloc.Construct<SkeletonMask>(); + + skeletonMask->m_Count = aNodeCount; + skeletonMask->m_Data = arAlloc.ConstructArray<SkeletonMaskElement>(aNodeCount); + + memcpy(skeletonMask->m_Data.Get(), elements, sizeof(SkeletonMaskElement)*aNodeCount); + + return skeletonMask; + } + + void DestroySkeletonMask(SkeletonMask* skeletonMask, memory::Allocator& arAlloc) + { + if(skeletonMask) + { + arAlloc.Deallocate(skeletonMask->m_Data); + arAlloc.Deallocate(skeletonMask); + } + } + + void SkeletonCopy(Skeleton const* apSrc, Skeleton* apDst) + { + apDst->m_Count = apSrc->m_Count; + for(int nodeIter = 0; nodeIter < apDst->m_Count; nodeIter++) + { + apDst->m_Node[nodeIter] = apSrc->m_Node[nodeIter]; + apDst->m_ID[nodeIter] = apSrc->m_ID[nodeIter]; + } + + apDst->m_AxesCount = apSrc->m_AxesCount; + for(int axesIter = 0; axesIter < apDst->m_AxesCount; axesIter++) + { + apDst->m_AxesArray[axesIter] = apSrc->m_AxesArray[axesIter]; + } + } + + void SkeletonPoseCopy(SkeletonPose const* apSrcPose, SkeletonPose* apDstPose) + { + uint32_t count = math::minimum(apSrcPose->m_Count, apDstPose->m_Count); + memcpy(&apDstPose->m_X[0], &apSrcPose->m_X[0], count*sizeof(math::xform)); + } + + void SkeletonPoseCopy(SkeletonPose const* apSrcPose, SkeletonPose* apDstPose, uint32_t aIndexCount, int32_t const *apIndexArray) + { + for(uint32_t i = 0 ; i < aIndexCount; i++) + { + int32_t j = apIndexArray[i]; + apDstPose->m_X[j] = apSrcPose->m_X[i]; + } + } + + int32_t SkeletonFindNode(Skeleton const* apSkeleton, uint32_t aID ) + { + int32_t ret = -1; + + int32_t i; + for(i = 0; ret == -1 && i < apSkeleton->m_Count; i++) + { + if(apSkeleton->m_ID[i] == aID) + { + ret = i; + } + } + + return ret; + } + + void SkeletonBuildIndexArray(int32_t *indexArray,Skeleton const* apSrcSkeleton,Skeleton const* apDstSkeleton) + { + for(uint32_t i = 0; i < apSrcSkeleton->m_Count; i++) + { + indexArray[i] = SkeletonFindNode(apDstSkeleton,apSrcSkeleton->m_ID[i]); + } + } + + void SkeletonBuildReverseIndexArray(int32_t *reverseIndexArray,int32_t const*indexArray,Skeleton const* apSrcSkeleton,Skeleton const* apDstSkeleton) + { + for(uint32_t dstIter = 0; dstIter < apDstSkeleton->m_Count; dstIter++) + { + reverseIndexArray[dstIter] = -1; + } + + for(uint32_t srcIter = 0; srcIter < apSrcSkeleton->m_Count; srcIter++) + { + if(indexArray[srcIter] != -1) + { + reverseIndexArray[indexArray[srcIter]] = srcIter; + } + } + } + + void SkeletonPoseCopy(Skeleton const* apSrcSkeleton, SkeletonPose const* apSrcPose, Skeleton const* apDstSkeleton, SkeletonPose* apDstPose) + { + uint32_t i; + for(i = 0; i < apSrcSkeleton->m_Count; i++) + { + uint32_t j; + for(j = 0; j < apDstSkeleton->m_Count; j++) + { + if( apSrcSkeleton->m_ID[i] == apDstSkeleton->m_ID[j] ) + { + apDstPose->m_X[j] = apSrcPose->m_X[i]; + break; + } + } + } + } + + void SkeletonPoseSetDirty(Skeleton const* apSkeleton, uint32_t* apSkeletonPoseMask, int aIndex, int aStopIndex, uint32_t aMask) + { + int parentIndex = apSkeleton->m_Node[aIndex].m_ParentId; + + if(parentIndex != -1) + { + if(aIndex != aStopIndex) + { + SkeletonPoseSetDirty(apSkeleton,apSkeletonPoseMask, parentIndex, aStopIndex, aMask); + } + } + + apSkeletonPoseMask[aIndex] = apSkeletonPoseMask[aIndex] | aMask; + } + + void SkeletonPoseComputeGlobal(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseLocal, SkeletonPose* apSkeletonPoseGlobal) + { + apSkeletonPoseGlobal->m_X[0] = apSkeletonPoseLocal->m_X[0]; + + uint32_t i; + for(i=1;i < apSkeleton->m_Count; i++) + { + apSkeletonPoseGlobal->m_X[i] = xformMul( apSkeletonPoseGlobal->m_X[apSkeleton->m_Node[i].m_ParentId], apSkeletonPoseLocal->m_X[i]); + } + } + + void SkeletonPoseComputeLocal(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseGlobal, SkeletonPose* apSkeletonPoseLocal) + { + uint32_t i; + for(i=apSkeleton->m_Count-1;i > 0; i--) + { + apSkeletonPoseLocal->m_X[i] = xformInvMul( apSkeletonPoseGlobal->m_X[apSkeleton->m_Node[i].m_ParentId], apSkeletonPoseGlobal->m_X[i]); + } + + apSkeletonPoseLocal->m_X[0] = apSkeletonPoseGlobal->m_X[0]; + } + + void SkeletonPoseComputeGlobal(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseLocal, SkeletonPose* apSkeletonPoseGlobal, int aIndex, int aStopIndex) + { + int parentIndex = apSkeleton->m_Node[aIndex].m_ParentId; + + if(parentIndex != -1) + { + if(aIndex != aStopIndex) + { + SkeletonPoseComputeGlobal(apSkeleton, apSkeletonPoseLocal, apSkeletonPoseGlobal, parentIndex, aStopIndex); + } + + apSkeletonPoseGlobal->m_X[aIndex] = xformMul( apSkeletonPoseGlobal->m_X[parentIndex], apSkeletonPoseLocal->m_X[aIndex]); + } + else + { + apSkeletonPoseGlobal->m_X[aIndex] = apSkeletonPoseLocal->m_X[aIndex]; + } + } + + void SkeletonPoseComputeGlobalQ(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseLocal, SkeletonPose* apSkeletonPoseGlobal) + { + apSkeletonPoseGlobal->m_X[0].q = apSkeletonPoseLocal->m_X[0].q; + + uint32_t i; + for(i=1;i < apSkeleton->m_Count; i++) + { + apSkeletonPoseGlobal->m_X[i].q = normalize(quatMul( apSkeletonPoseGlobal->m_X[apSkeleton->m_Node[i].m_ParentId].q, apSkeletonPoseLocal->m_X[i].q)); + } + } + + void SkeletonPoseComputeGlobalQ(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseLocal, SkeletonPose* apSkeletonPoseGlobal, int aIndex, int aStopIndex) + { + int parentIndex = apSkeleton->m_Node[aIndex].m_ParentId; + + if(parentIndex != -1) + { + if(aIndex != aStopIndex) + { + SkeletonPoseComputeGlobalQ(apSkeleton, apSkeletonPoseLocal, apSkeletonPoseGlobal, parentIndex, aStopIndex); + } + + apSkeletonPoseGlobal->m_X[aIndex].q = normalize(quatMul( apSkeletonPoseGlobal->m_X[parentIndex].q, apSkeletonPoseLocal->m_X[aIndex].q)); + } + else + { + apSkeletonPoseGlobal->m_X[aIndex].q = apSkeletonPoseLocal->m_X[aIndex].q; + } + } + + void SkeletonPoseComputeLocal(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseGlobal, SkeletonPose* apSkeletonPoseLocal, int aIndex, int aStopIndex) + { + int parentIndex = apSkeleton->m_Node[aIndex].m_ParentId; + + if(parentIndex != -1) + { + apSkeletonPoseLocal->m_X[aIndex] = xformInvMul( apSkeletonPoseGlobal->m_X[parentIndex], apSkeletonPoseGlobal->m_X[aIndex]); + + if(aIndex != aStopIndex) + { + SkeletonPoseComputeLocal(apSkeleton, apSkeletonPoseGlobal, apSkeletonPoseLocal, parentIndex, aStopIndex); + } + } + else + { + apSkeletonPoseLocal->m_X[aIndex] = apSkeletonPoseGlobal->m_X[aIndex]; + } + } + + void SkeletonPoseComputeLocalQ(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseGlobal, SkeletonPose* apSkeletonPoseLocal) + { + uint32_t i; + for(i=apSkeleton->m_Count-1;i > 0; i--) + { + apSkeletonPoseLocal->m_X[i].q = normalize(quatMul( quatConj(apSkeletonPoseGlobal->m_X[apSkeleton->m_Node[i].m_ParentId].q), apSkeletonPoseGlobal->m_X[i].q)); + } + + apSkeletonPoseLocal->m_X[0].q = apSkeletonPoseGlobal->m_X[0].q; + } + + void SkeletonPoseComputeLocalQ(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseGlobal, SkeletonPose* apSkeletonPoseLocal, int aIndex, int aStopIndex) + { + int parentIndex = apSkeleton->m_Node[aIndex].m_ParentId; + + if(parentIndex != -1) + { + apSkeletonPoseLocal->m_X[aIndex].q = normalize(quatMul( quatConj(apSkeletonPoseGlobal->m_X[parentIndex].q), apSkeletonPoseGlobal->m_X[aIndex].q)); + + if(aIndex != aStopIndex) + { + SkeletonPoseComputeLocalQ(apSkeleton, apSkeletonPoseGlobal, apSkeletonPoseLocal, parentIndex, aStopIndex); + } + } + else + { + apSkeletonPoseLocal->m_X[aIndex].q = apSkeletonPoseGlobal->m_X[aIndex].q; + } + } + + math::float4 SkeletonGetDoF(Skeleton const* apSkeleton,SkeletonPose const *apSkeletonPose, int32_t aIndex) + { + const int32_t axesIndex = apSkeleton->m_Node[aIndex].m_AxesId; + return math::cond(math::bool4(axesIndex != -1),math::ToAxes(apSkeleton->m_AxesArray[axesIndex],apSkeletonPose->m_X[aIndex].q),math::quat2Qtan(apSkeletonPose->m_X[aIndex].q)); + } + + void SkeletonSetDoF(Skeleton const* apSkeleton,SkeletonPose * apSkeletonPose, math::float4 const& aDoF, int32_t aIndex) + { + const int32_t axesIndex = apSkeleton->m_Node[aIndex].m_AxesId; + apSkeletonPose->m_X[aIndex].q = math::cond(math::bool4(axesIndex != -1),math::FromAxes(apSkeleton->m_AxesArray[axesIndex],aDoF),qtan2Quat(aDoF)); + } + + math::float4 SkeletonNodeEndPoint(Skeleton const *apSkeleton, int32_t aIndex, SkeletonPose const *apSkeletonPose) + { + return math::xformMulVec(apSkeletonPose->m_X[aIndex],math::quatXcos(apSkeleton->m_AxesArray[aIndex].m_PostQ) * math::float1(apSkeleton->m_AxesArray[aIndex].m_Length)); + } + + void SkeletonAlign(skeleton::Skeleton const *apSkeleton, math::float4 const &arRefQ, math::float4 & arQ, int32_t aIndex) + { + const int32_t axesIndex = apSkeleton->m_Node[aIndex].m_AxesId; + + if(axesIndex != -1) + { + math::Axes axes = apSkeleton->m_AxesArray[axesIndex]; + + math::float4 refV = math::quatXcos(math::normalize(math::quatMul(arRefQ,axes.m_PostQ))); + math::float4 v = math::quatXcos(math::normalize(math::quatMul(arQ,axes.m_PostQ))); + math::float4 dq = math::quatArcRotate(v,refV); + + arQ = math::normalize(math::quatMul(dq,arQ)); + } + } + + void SkeletonAlign(skeleton::Skeleton const *apSkeleton, skeleton::SkeletonPose const*apSkeletonPoseRef, skeleton::SkeletonPose *apSkeletonPose, int32_t aIndex) + { + SkeletonAlign(apSkeleton,apSkeletonPoseRef->m_X[aIndex].q,apSkeletonPose->m_X[aIndex].q,aIndex); + } + + void Skeleton2BoneAdjustLength(Skeleton const *apSkeleton, int32_t aIndexA, int32_t aIndexB, int32_t aIndexC, math::float4 const &aTarget, math::float1 const& aRatio, SkeletonPose *apSkeletonPose, SkeletonPose *apSkeletonPoseWorkspace) + { + math::float4 vAB = apSkeletonPoseWorkspace->m_X[aIndexB].t - apSkeletonPoseWorkspace->m_X[aIndexA].t; + math::float4 vBC = apSkeletonPoseWorkspace->m_X[aIndexC].t - apSkeletonPoseWorkspace->m_X[aIndexB].t; + math::float4 vAD = aTarget - apSkeletonPoseWorkspace->m_X[aIndexA].t; + + math::float1 lenABC = math::length(vAB) + math::length(vBC); + math::float1 lenAD = math::length(vAD); + math::float1 ratio = lenAD / lenABC; + math::float1 invARatio = math::float1::one() - aRatio; + + if(ratio > invARatio) + { + ratio = math::saturate( (ratio-(invARatio))/(math::float1(2)*aRatio) ); + math::float1 r = math::float1::one() + aRatio * ratio * ratio; + + apSkeletonPose->m_X[aIndexB].t *= r; + apSkeletonPose->m_X[aIndexC].t *= r; + } + } + + void Skeleton2BoneIK(Skeleton const *apSkeleton, int32_t aIndexA, int32_t aIndexB, int32_t aIndexC, math::float4 const &aTarget, float aWeight, SkeletonPose *apSkeletonPose, SkeletonPose *apSkeletonPoseWorkspace) + { + math::float4 qA0 = apSkeletonPose->m_X[aIndexA].q; + math::float4 qB0 = apSkeletonPose->m_X[aIndexB].q; + + math::float4 dof = skeleton::SkeletonGetDoF(apSkeleton,apSkeletonPose,aIndexB) * math::float4(1.0f,0,0.9f,0); + skeleton::SkeletonSetDoF(apSkeleton,apSkeletonPose,dof,aIndexB); + skeleton::SkeletonPoseComputeGlobal(apSkeleton,apSkeletonPose,apSkeletonPoseWorkspace,aIndexC,aIndexB); + + math::float4 vAB = apSkeletonPoseWorkspace->m_X[aIndexB].t - apSkeletonPoseWorkspace->m_X[aIndexA].t; + math::float4 vBC = apSkeletonPoseWorkspace->m_X[aIndexC].t - apSkeletonPoseWorkspace->m_X[aIndexB].t; + math::float4 vAC = apSkeletonPoseWorkspace->m_X[aIndexC].t - apSkeletonPoseWorkspace->m_X[aIndexA].t; + math::float4 vAD = aTarget - apSkeletonPoseWorkspace->m_X[aIndexA].t; + + math::float1 lenAB = math::length(vAB); + math::float1 lenBC = math::length(vBC); + math::float1 lenAC = math::length(vAC); + math::float1 lenAD = math::length(vAD); + + math::float1 angleAC = math::triangleAngle(lenAC,lenAB,lenBC); + math::float1 angleAD = math::triangleAngle(lenAD,lenAB,lenBC); + + math::float4 axis = math::normalize(math::cross(vAB,vBC)); + + math::float1 a = math::float1(0.5f) * (angleAC-angleAD); + math::float1 s,c; + math::sincos(a,s,c); + math::float4 q = axis*s; + q.w() = c; + apSkeletonPoseWorkspace->m_X[aIndexB].q = math::normalize(math::quatMul(q,apSkeletonPoseWorkspace->m_X[aIndexB].q)); + + skeleton::SkeletonPoseComputeLocal(apSkeleton,apSkeletonPoseWorkspace,apSkeletonPose,aIndexB,aIndexB); + apSkeletonPose->m_X[aIndexB].q = math::quatLerp(qB0,apSkeletonPose->m_X[aIndexB].q,math::float1(aWeight)); + skeleton::SkeletonPoseComputeGlobal(apSkeleton,apSkeletonPose,apSkeletonPoseWorkspace,aIndexC,aIndexB); + + vAC = apSkeletonPoseWorkspace->m_X[aIndexC].t - apSkeletonPoseWorkspace->m_X[aIndexA].t; + + q = math::normalize(math::quatArcRotate(vAC,vAD)); + apSkeletonPoseWorkspace->m_X[aIndexA].q = math::normalize(math::quatMul(q,apSkeletonPoseWorkspace->m_X[aIndexA].q)); + skeleton::SkeletonPoseComputeLocal(apSkeleton,apSkeletonPoseWorkspace,apSkeletonPose,aIndexA,aIndexA); + apSkeletonPose->m_X[aIndexA].q = math::quatLerp(apSkeletonPose->m_X[aIndexA].q,qA0,math::float1(math::pow(1-aWeight,4))); + } + + void Skeleton3BoneIK(Skeleton const *apSkeleton, int32_t aIndexA, int32_t aIndexB, int32_t aIndexC, math::float4 const &aTarget, float weight, SkeletonPose *apSkeletonPose, SkeletonPose *apSkeletonPoseWorkspace) + { + if(weight > 0) + { + math::float4 qA = apSkeletonPose->m_X[aIndexA].q; + math::float4 qB = apSkeletonPose->m_X[aIndexB].q; + math::float4 qC = apSkeletonPose->m_X[aIndexC].q; + + float fingerLen = apSkeleton->m_AxesArray[aIndexA].m_Length + apSkeleton->m_AxesArray[aIndexB].m_Length + apSkeleton->m_AxesArray[aIndexC].m_Length; + math::float1 targetDist = math::length(apSkeletonPoseWorkspace->m_X[aIndexA].t - aTarget); + float fact = math::pow(math::clamp(targetDist.tofloat()/fingerLen,0.f,1.f),4.0f); + math::float4 dof(0,0,2.0f*fact-1.0f,0); + + skeleton::SkeletonSetDoF(apSkeleton,apSkeletonPose,dof,aIndexB); + skeleton::SkeletonSetDoF(apSkeleton,apSkeletonPose,dof,aIndexC); + skeleton::SkeletonPoseComputeGlobal(apSkeleton,apSkeletonPose,apSkeletonPoseWorkspace,aIndexC,aIndexB); + + math::float4 endT = SkeletonNodeEndPoint(apSkeleton,aIndexC,apSkeletonPoseWorkspace); + math::float4 endV = endT - apSkeletonPoseWorkspace->m_X[aIndexA].t; + math::float4 targetV = aTarget - apSkeletonPoseWorkspace->m_X[aIndexA].t; + + math::float4 q = math::normalize(math::quatArcRotate(endV,targetV)); + apSkeletonPoseWorkspace->m_X[aIndexA].q = math::quatMul(q,apSkeletonPoseWorkspace->m_X[aIndexA].q); + skeleton::SkeletonPoseComputeLocal(apSkeleton,apSkeletonPoseWorkspace,apSkeletonPose,aIndexA,aIndexA); + + math::float1 w(weight); + + apSkeletonPose->m_X[aIndexA].q = math::quatLerp(qA,apSkeletonPose->m_X[aIndexA].q,w); + apSkeletonPose->m_X[aIndexB].q = math::quatLerp(qB,apSkeletonPose->m_X[aIndexB].q,w); + apSkeletonPose->m_X[aIndexC].q = math::quatLerp(qC,apSkeletonPose->m_X[aIndexC].q,w); + } + } + + void SetupAxes(skeleton::Skeleton *apSkeleton, skeleton::SkeletonPose const *apSkeletonPoseGlobal, SetupAxesInfo const& apSetupAxesInfo, int32_t aIndex, int32_t aAxisIndex, bool aLeft, float aLen) + { + skeleton::Node &node = apSkeleton->m_Node[aIndex]; + int32_t parentIndex = node.m_ParentId; + + if(node.m_AxesId != -1) + { + math::Axes &axes = apSkeleton->m_AxesArray[node.m_AxesId]; + + math::xform boneX = apSkeletonPoseGlobal->m_X[aIndex]; + + axes.m_Limit.m_Min = math::radians(math::float4(apSetupAxesInfo.m_Min[0],apSetupAxesInfo.m_Min[1],apSetupAxesInfo.m_Min[2],0)); + axes.m_Limit.m_Max = math::radians(math::float4(apSetupAxesInfo.m_Max[0],apSetupAxesInfo.m_Max[1],apSetupAxesInfo.m_Max[2],0)); + axes.m_Sgn = math::float4(apSetupAxesInfo.m_Sgn[0],apSetupAxesInfo.m_Sgn[1],apSetupAxesInfo.m_Sgn[2],1) * (aLeft ? math::float4(1) : math::float4(-1,1,-1,-1)); + + math::float4 mainAxis = math::float4(apSetupAxesInfo.m_MainAxis[0],apSetupAxesInfo.m_MainAxis[1],apSetupAxesInfo.m_MainAxis[2],0); + math::float4 zeroQ = math::float4(apSetupAxesInfo.m_PreQ[0],apSetupAxesInfo.m_PreQ[1],apSetupAxesInfo.m_PreQ[2],apSetupAxesInfo.m_PreQ[3]) * (aLeft ? math::float4(1) : math::float4(1,1,1,-1)); + + math::float4 u(1,0,0,0); + math::float4 w(0,1,0,0); + math::float4 v(0,0,1,0); + + axes.m_Type = apSetupAxesInfo.m_Type; + + axes.m_Length = 1.0f; + + if(aAxisIndex != -1) + { + math::xform axisX = apSkeletonPoseGlobal->m_X[aAxisIndex]; + + u = math::normalize((axisX.t - boneX.t) * math::float1(aLen)); + + w = mainAxis; + v = math::normalize(math::cross(w,u)); + w = cross(u,v); + + axes.m_Length = math::length(axisX.t - boneX.t).tofloat(); + } + + if(apSetupAxesInfo.m_ForceAxis) + { + switch(apSetupAxesInfo.m_ForceAxis) + { + case +1: u = math::float4(+1,0,0,0); break; + case -1: u = math::float4(-1,0,0,0); break; + case +2: u = math::float4(0,+1,0,0); break; + case -2: u = math::float4(0,-1,0,0); break; + case +3: u = math::float4(0,0,+1,0); break; + default: u = math::float4(0,0,-1,0); break; + }; + + w = mainAxis; + v = math::normalize(math::cross(w,u)); + w = cross(u,v); + } + + axes.m_Length *= fabs(aLen); + + math::float4 parentQ = math::cond(math::bool4(parentIndex != -1),apSkeletonPoseGlobal->m_X[parentIndex].q, math::quatIdentity()); + + axes.m_PreQ = math::quatMatrixToQuat(u,v,w); + axes.m_PostQ = math::normalize(math::quatMul(math::quatConj(boneX.q),axes.m_PreQ)); + axes.m_PreQ = math::normalize(math::quatMul(math::quatConj(parentQ),math::quatMul(zeroQ,axes.m_PreQ))); + } + } + + static int GetSkeletonNodeDepth(mecanim::skeleton::Skeleton const& skeleton, uint32_t boneIndex) + { + uint32_t parentIndex = skeleton.m_Node[boneIndex].m_ParentId; + int depth = 0; + while (parentIndex != -1) + { + depth++; + parentIndex = skeleton.m_Node[parentIndex].m_ParentId; + } + return depth; + } +} +} + diff --git a/Runtime/mecanim/skeleton/skeleton.h b/Runtime/mecanim/skeleton/skeleton.h new file mode 100644 index 0000000..e9e9c4b --- /dev/null +++ b/Runtime/mecanim/skeleton/skeleton.h @@ -0,0 +1,186 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/memory.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/Math/Simd/xform.h" +#include "Runtime/mecanim/math/axes.h" + + +#include "Runtime/Serialize/Blobification/offsetptr.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Animation/MecanimArraySerialization.h" + +namespace mecanim +{ +namespace skeleton +{ + struct Node + { + DEFINE_GET_TYPESTRING(Node) + + Node() : m_ParentId(-1), m_AxesId(-1) {} + int32_t m_ParentId; + int32_t m_AxesId; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_ParentId); + TRANSFER(m_AxesId); + } + }; + + struct Skeleton + { + DEFINE_GET_TYPESTRING(Skeleton) + + Skeleton() : m_Count(0), m_AxesCount(0) {} + + uint32_t m_Count; + OffsetPtr<Node> m_Node; + OffsetPtr<uint32_t> m_ID; // CRC(path) + + uint32_t m_AxesCount; + OffsetPtr<math::Axes> m_AxesArray; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_Count); + MANUAL_ARRAY_TRANSFER2(Node, m_Node, m_Count); + MANUAL_ARRAY_TRANSFER2(uint32_t, m_ID, m_Count); + TRANSFER_BLOB_ONLY(m_AxesCount); + MANUAL_ARRAY_TRANSFER2(math::Axes, m_AxesArray, m_AxesCount); + } + }; + + struct ATTRIBUTE_ALIGN(ALIGN4F) SkeletonPose + { + DEFINE_GET_TYPESTRING(SkeletonPose) + + SkeletonPose() : m_Count(0) {} + + uint32_t m_Count; + OffsetPtr<math::xform> m_X; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_Count); + MANUAL_ARRAY_TRANSFER2(math::xform, m_X, m_Count); + } + }; + + struct SkeletonMaskElement + { + DEFINE_GET_TYPESTRING(SkeletonMaskElement) + + SkeletonMaskElement(): m_PathHash(0), m_Weight(0.f){} + + uint32_t m_PathHash; + float m_Weight; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_PathHash); + TRANSFER(m_Weight); + } + }; + + struct SkeletonMask + { + DEFINE_GET_TYPESTRING(SkeletonMask) + + SkeletonMask():m_Count(0){} + + uint32_t m_Count; + OffsetPtr<SkeletonMaskElement> m_Data; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_Count); + MANUAL_ARRAY_TRANSFER2(SkeletonMaskElement, m_Data, m_Count); + } + }; + + Skeleton* CreateSkeleton(int32_t aNodeCount, int32_t aAxesCount, memory::Allocator& arAlloc); + void DestroySkeleton(Skeleton* apSkeleton, memory::Allocator& arAlloc); + + SkeletonPose* CreateSkeletonPose(Skeleton const* apSkeleton, memory::Allocator& arAlloc); + void DestroySkeletonPose(SkeletonPose* apSkeletonPose, memory::Allocator& arAlloc); + + SkeletonMask* CreateSkeletonMask(uint32_t aNodeCount, SkeletonMaskElement* elements, memory::Allocator& arAlloc); + void DestroySkeletonMask(SkeletonMask* skeletonMask, memory::Allocator& arAlloc); + + // copy skeleton + void SkeletonCopy(Skeleton const* apSrc, Skeleton* apDst); + + // copy pose + void SkeletonPoseCopy(SkeletonPose const* apSrcPose, SkeletonPose* apDstPose); + void SkeletonPoseCopy(SkeletonPose const* apSrcPose, SkeletonPose* apDstPose, uint32_t aIndexCount, int32_t const *apIndexArray); + + // Find & Copy pose based on name binding + int32_t SkeletonFindNode(Skeleton const *apSkeleton, uint32_t aID); + void SkeletonBuildIndexArray(int32_t *indexArray,Skeleton const* apSrcSkeleton,Skeleton const* apDstSkeleton); + void SkeletonBuildReverseIndexArray(int32_t *reverseIndexArray,int32_t const*indexArray,Skeleton const* apSrcSkeleton,Skeleton const* apDstSkeleton); + void SkeletonPoseCopy(Skeleton const* apSrcSkeleton, SkeletonPose const* apSrcPose, Skeleton const* apDstSkeleton, SkeletonPose* apDstPose); + + // set mask for a skeleton mask array + void SkeletonPoseSetDirty(Skeleton const* apSkeleton, uint32_t* apSkeletonPoseMask, int aIndex, int aStopIndex, uint32_t aMask); + + // those functions work in place + // computes a global pose from a local pose + void SkeletonPoseComputeGlobal(Skeleton const* apSkeleton, SkeletonPose const* apLocalPose, SkeletonPose* apGlobalPose); + // computes a global pose from a local pose for part of the skeleton starting at aIndex (child) to aStopIndex (ancestor) + void SkeletonPoseComputeGlobal(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseLocal, SkeletonPose* apSkeletonPoseGlobal, int aIndex, int aStopIndex); + // computes a local pose from a global pose + void SkeletonPoseComputeLocal(Skeleton const* apSkeleton, SkeletonPose const* apGlobalPose, SkeletonPose* apLocalPose); + // computes a local pose from a global pose for part of the skeleton starting at aIndex (child) to aStopIndex (ancestor) + void SkeletonPoseComputeLocal(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseGlobal, SkeletonPose* apSkeletonPoseLocal, int aIndex, int aStopIndex); + // computes a global Q pose from a local Q pose + void SkeletonPoseComputeGlobalQ(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseLocal, SkeletonPose* apSkeletonPoseGlobal); + // computes a global Q pose from a local Q pose for part of the skeleton starting at aIndex (child) to aStopIndex (ancestor) + void SkeletonPoseComputeGlobalQ(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseLocal, SkeletonPose* apSkeletonPoseGlobal, int aIndex, int aStopIndex); + // computes a local Q pose from a global Q pose + void SkeletonPoseComputeLocalQ(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseGlobal, SkeletonPose* apSkeletonPoseLocal); + // computes a local Q pose from a global Q pose for part of the skeleton starting at aIndex (child) to aStopIndex (ancestor) + void SkeletonPoseComputeLocalQ(Skeleton const* apSkeleton, SkeletonPose const* apSkeletonPoseGlobal, SkeletonPose* apSkeletonPoseLocal, int aIndex, int aStopIndex); + + // get dof for bone index in pose + math::float4 SkeletonGetDoF(Skeleton const* apSkeleton,SkeletonPose const *apSkeletonPose, int32_t aIndex); + // set dof for bone index in pose + void SkeletonSetDoF(Skeleton const* apSkeleton, SkeletonPose * apSkeletonPose, math::float4 const& aDoF, int32_t aIndex); + // algin x axis of skeleton node quaternion to ref node quaternion + void SkeletonAlign(skeleton::Skeleton const *apSkeleton, math::float4 const &arRefQ, math::float4 & arQ, int32_t aIndex); + // algin x axis of skeleton pose node to ref pose node + void SkeletonAlign(skeleton::Skeleton const *apSkeleton, skeleton::SkeletonPose const*apSkeletonPoseRef, skeleton::SkeletonPose *apSkeletonPose, int32_t aIndex); + + // ik + // compute end point of a node which is x * xcos * lenght. + math::float4 SkeletonNodeEndPoint(Skeleton const *apSkeleton, int32_t aIndex, SkeletonPose const*apSkeletonPose); + // The apSkeletonPoseWorkspace parameter has to be a valid global pose, otherwise unexpected result may occur + void Skeleton2BoneAdjustLength(Skeleton const *apSkeleton, int32_t aIndexA, int32_t aIndexB, int32_t aIndexC, math::float4 const &aTarget, math::float1 const& aRatio, SkeletonPose *apSkeletonPose, SkeletonPose *apSkeletonPoseWorkspace); + // The apSkeletonPoseWorkspace parameter has to be a valid global pose, otherwise unexpected result may occur + void Skeleton2BoneIK(Skeleton const *apSkeleton, int32_t aIndexA, int32_t aIndexB, int32_t aIndexC, math::float4 const &aTarget, float aWeight, SkeletonPose *apSkeletonPose, SkeletonPose *apSkeletonPoseWorkspace); + // The apSkeletonPoseWorkspace parameter has to be a valid global pose, otherwise unexpected result may occur + void Skeleton3BoneIK(Skeleton const *apSkeleton, int32_t aIndexA, int32_t aIndexB, int32_t aIndexC, math::float4 const &aTarget, float weight, SkeletonPose *apSkeletonPose, SkeletonPose *apSkeletonPoseWorkspace); + + // setup axes utilities + struct SetupAxesInfo + { + float m_PreQ[4]; + float m_MainAxis[4]; + float m_Min[4]; + float m_Max[4]; + float m_Sgn[4]; + math::AxesType m_Type; + int32_t m_ForceAxis; + }; + + void SetupAxes(skeleton::Skeleton *apSkeleton, skeleton::SkeletonPose const *apSkeletonPoseGlobal, SetupAxesInfo const& apSetupAxesInfo, int32_t aIndex, int32_t aAxisIndex, bool aLeft, float aLen = 1.0f); +} + +} diff --git a/Runtime/mecanim/statemachine/statemachine.cpp b/Runtime/mecanim/statemachine/statemachine.cpp new file mode 100644 index 0000000..3bd1a25 --- /dev/null +++ b/Runtime/mecanim/statemachine/statemachine.cpp @@ -0,0 +1,967 @@ +#include "UnityPrefix.h" + + +#include "Runtime/Serialize/TransferFunctions/TransferNameConversions.h" + +#include "Runtime/Misc/BuildSettings.h" +#include "Runtime/mecanim/generic/stringtable.h" + +#include "Runtime/mecanim/statemachine/statemachine.h" + +#include "Runtime/Math/Simd/math.h" + +namespace mecanim +{ +namespace statemachine +{ + + int32_t GetStateIndex(StateMachineConstant const* apStateMachineConstant, uint32_t id) + { + for(uint32_t i=0;i<apStateMachineConstant->m_StateConstantCount;++i) + { + if (CompareStateID (apStateMachineConstant->m_StateConstantArray[i].Get(), id)) + return i; + } + return -1; + } + + + + TransitionConstant const* GetTransitionConstant(StateMachineConstant const* apStateMachineConstant, StateConstant const* apStateConstant, uint32_t id) + { + if( id >= s_DynamicTransitionEncodeKey) + return 0; + else if( id >= s_AnyTransitionEncodeKey) + return apStateMachineConstant->m_AnyStateTransitionConstantArray[id-s_AnyTransitionEncodeKey].Get(); + else + return apStateConstant->m_TransitionConstantArray[id].Get(); + } + + + bool IsCurrentTransitionAtomic(StateMachineConstant const* apStateMachineConstant, StateMachineMemory *apStateMachineMemory) + { + TransitionConstant const* transition = GetTransitionConstant(apStateMachineConstant, apStateMachineConstant->m_StateConstantArray[apStateMachineMemory->m_CurrentStateIndex].Get(), apStateMachineMemory->m_TransitionId); + return transition->m_Atomic; + } + + TransitionWorkspace* GetTransitionWorkspace(StateMachineWorkspace const* apStateMachineWorkspace, StateWorkspace const* apStateWorkspace, uint32_t id) + { + if( id > s_AnyTransitionEncodeKey) + return apStateMachineWorkspace->m_AnyStateTransitionWorkspaceArray[id-s_AnyTransitionEncodeKey]; + + else + return apStateWorkspace->m_TransitionWorkspaceArray[id]; + } + + TransitionConstant *CreateTransitionConstant(ConditionConstant** apConditionsConstantArray, uint32_t aConditionConstantCount, + uint32_t aDestinationState,float aTransitionDuration, float aTransitionOffset, bool aAtomic, uint32_t aID, uint32_t aUserID, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(TransitionConstant); + + TransitionConstant * transitionConstant = arAlloc.Construct<TransitionConstant >(); + transitionConstant->m_ConditionConstantArray = arAlloc.ConstructArray< OffsetPtr<ConditionConstant> >(aConditionConstantCount); + transitionConstant->m_ConditionConstantCount = aConditionConstantCount; + + transitionConstant->m_TransitionDuration = aTransitionDuration; + transitionConstant->m_TransitionOffset = aTransitionOffset; + transitionConstant->m_Atomic = aAtomic; + transitionConstant->m_ID = aID; + transitionConstant->m_UserID= aUserID; + + + transitionConstant->m_DestinationState = aDestinationState; + + + uint32_t i; + for(i=0;i<aConditionConstantCount;i++) + transitionConstant->m_ConditionConstantArray[i] = apConditionsConstantArray[i]; + + return transitionConstant; + } + + void DestroyTransitionConstant(TransitionConstant *apTransitionConstant, memory::Allocator& arAlloc) + { + if(apTransitionConstant) + { + arAlloc.Deallocate(apTransitionConstant->m_ConditionConstantArray); + arAlloc.Deallocate(apTransitionConstant); + } + } + + + TransitionWorkspace* CreateTransitionWorkspace(TransitionConstant const* apTransitionConstant, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(TransitionWorkspace); + TransitionWorkspace* transitionWorkspace = arAlloc.Construct<TransitionWorkspace>(); + transitionWorkspace->m_ConditionConstantCount = apTransitionConstant->m_ConditionConstantCount; + + return transitionWorkspace; + } + + void DestroyTransitionWorkspace(TransitionWorkspace* apTransitionWorkspace, memory::Allocator& arAlloc) + { + if(apTransitionWorkspace) + { + arAlloc.Deallocate(apTransitionWorkspace); + } + } + + + ConditionConstant *CreateConditionConstant( uint32_t aConditionMode, uint32_t aEventID, float aEventThreshold, float aExitTime, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(ConditionConstant); + ConditionConstant* conditionConstant = arAlloc.Construct<ConditionConstant>(); + + conditionConstant->m_ConditionMode = aConditionMode; + + if(aConditionMode == kConditionModeIf || aConditionMode == kConditionModeIfNot || aConditionMode == kConditionModeGreater || aConditionMode == kConditionModeLess || aConditionMode == kConditionModeEquals || aConditionMode == kConditionModeNotEqual) + { + conditionConstant->m_EventID = aEventID; + if(aConditionMode == kConditionModeGreater || aConditionMode == kConditionModeLess || aConditionMode == kConditionModeEquals || aConditionMode == kConditionModeNotEqual) + { + conditionConstant->m_EventThreshold = aEventThreshold; + } + } + else if(aConditionMode == kConditionModeExitTime) + { + conditionConstant->m_ExitTime = aExitTime; + } + + return conditionConstant; + } + void DestroyConditionConstant(ConditionConstant *apConditionConstant, memory::Allocator& arAlloc) + { + if(apConditionConstant) + { + arAlloc.Deallocate(apConditionConstant); + } + } + + static int GetBlendTreeIndex(const StateConstant& arStateConstant, mecanim::int32_t aMotionSetIndex) + { + return arStateConstant.m_BlendTreeConstantIndexArray[aMotionSetIndex]; + } + + animation::BlendTreeConstant const* GetBlendTreeConstant(const StateConstant& arStateConstant, mecanim::int32_t aMotionSetIndex) + { + int blendTreeIndex = GetBlendTreeIndex(arStateConstant,aMotionSetIndex); + return blendTreeIndex != -1 ? arStateConstant.m_BlendTreeConstantArray[blendTreeIndex].Get() : 0; + } + + animation::BlendTreeMemory *GetBlendTreeMemory(const StateConstant& arStateConstant,StateMemory& arStateMemory, mecanim::int32_t aMotionSetIndex) + { + int blendTreeIndex = GetBlendTreeIndex(arStateConstant,aMotionSetIndex); + return blendTreeIndex != -1 ? arStateMemory.m_BlendTreeMemoryArray[blendTreeIndex].Get() : 0; + } + + StateConstant* CreateStateConstant(TransitionConstant** apTransitionConstantArray, uint32_t aTransitionConstantCount, + float aSpeed, bool aIKOnFeet, bool aMirror, float aCycleOffset, animation::BlendTreeConstant** apBlendTreeConstantArray, + uint32_t aMotionSetCount, uint32_t nameID, uint32_t pathID, uint32_t aTagID, bool aLoop, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(StateConstant); + StateConstant* stateConstant = arAlloc.Construct<StateConstant>(); + + stateConstant->m_TransitionConstantCount = aTransitionConstantCount; + stateConstant->m_Speed = aSpeed; + stateConstant->m_IKOnFeet = aIKOnFeet; + stateConstant->m_Mirror = aMirror; + stateConstant->m_CycleOffset = aCycleOffset; + stateConstant->m_PathID = pathID; + stateConstant->m_NameID = nameID; + stateConstant->m_TagID = aTagID; + stateConstant->m_MotionSetCount = aMotionSetCount; + stateConstant->m_BlendTreeCount = 0 ; + stateConstant->m_Loop = aLoop; + + stateConstant->m_BlendTreeConstantIndexArray = arAlloc.ConstructArray<int32_t>(aMotionSetCount); + stateConstant->m_LeafInfoArray = arAlloc.ConstructArray<LeafInfoConstant>(aMotionSetCount); + + stateConstant->m_TransitionConstantArray = arAlloc.ConstructArray< OffsetPtr<TransitionConstant> >(aTransitionConstantCount); + + uint32_t i; + for(i = 0 ; i < aTransitionConstantCount; i++) + stateConstant->m_TransitionConstantArray[i] = apTransitionConstantArray[i]; + + for(i = 0 ; i < aMotionSetCount; i++) + { + if(apBlendTreeConstantArray[i] != 0) + { + stateConstant->m_BlendTreeConstantIndexArray[i] = stateConstant->m_BlendTreeCount ; + stateConstant->m_LeafInfoArray[i].m_Count = animation::GetLeafCount(*apBlendTreeConstantArray[i]); + stateConstant->m_LeafInfoArray[i].m_IDArray = arAlloc.ConstructArray<uint32_t>(stateConstant->m_LeafInfoArray[i].m_Count); + animation::FillLeafIDArray(*apBlendTreeConstantArray[i], stateConstant->m_LeafInfoArray[i].m_IDArray.Get()); + stateConstant->m_BlendTreeCount++; + } + else + { + stateConstant->m_BlendTreeConstantIndexArray[i] = -1; + stateConstant->m_LeafInfoArray[i].m_Count = 0; + stateConstant->m_LeafInfoArray[i].m_IDArray = 0 ; + } + + } + + stateConstant->m_BlendTreeConstantArray = arAlloc.ConstructArray< OffsetPtr<animation::BlendTreeConstant> >(stateConstant->m_BlendTreeCount); + uint32_t currentTreeCount = 0; + for(i = 0 ; i < aMotionSetCount ; i++) + { + if(apBlendTreeConstantArray[i] != 0) + { + stateConstant->m_BlendTreeConstantArray[currentTreeCount] = apBlendTreeConstantArray[i]; + currentTreeCount++; + } + } + + return stateConstant; + } + + void DestroyStateConstant(StateConstant* apStateConstant, memory::Allocator& arAlloc) + { + if(apStateConstant) + { + for(uint32_t i = 0 ; i < apStateConstant->m_MotionSetCount; i++) + { + arAlloc.Deallocate(apStateConstant->m_LeafInfoArray[i].m_IDArray); + } + arAlloc.Deallocate(apStateConstant->m_LeafInfoArray); + arAlloc.Deallocate(apStateConstant->m_BlendTreeConstantArray); + arAlloc.Deallocate(apStateConstant->m_BlendTreeConstantIndexArray); + + arAlloc.Deallocate(apStateConstant->m_TransitionConstantArray); + arAlloc.Deallocate(apStateConstant); + } + } + + StateMemory* CreateStateMemory(StateConstant const* apStateConstant, StateMachineConstant const* apParentStateMachineConstant, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(StateMemory); + StateMemory *stateMemory = arAlloc.Construct<StateMemory>(); + + stateMemory->m_BlendTreeCount = apStateConstant->m_BlendTreeCount; + stateMemory->m_BlendTreeMemoryArray = arAlloc.ConstructArray< OffsetPtr<animation::BlendTreeMemory> >(stateMemory->m_BlendTreeCount); + + for(int blendTreeIter = 0 ; blendTreeIter < stateMemory->m_BlendTreeCount; blendTreeIter++) + { + stateMemory->m_BlendTreeMemoryArray[blendTreeIter] = animation::CreateBlendTreeMemory(apStateConstant->m_BlendTreeConstantArray[blendTreeIter].Get(), arAlloc); + } + + return stateMemory; + } + + void DestroyStateMemory(StateMemory* apStateMemory, memory::Allocator& arAlloc) + { + if(apStateMemory) + { + for(int i = 0 ; i < apStateMemory->m_BlendTreeCount; i++) + { + arAlloc.Deallocate(apStateMemory->m_BlendTreeMemoryArray[i]); + } + + arAlloc.Deallocate(apStateMemory->m_BlendTreeMemoryArray); + arAlloc.Deallocate(apStateMemory); + } + } + + StateWorkspace* CreateStateWorkspace(StateConstant const* apStateConstant, uint32_t maxBlendedClip, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(StateWorkspace); + StateWorkspace* stateWorkspace = arAlloc.Construct<StateWorkspace>(); + + stateWorkspace->m_TransitionWorkspaceCount = apStateConstant->m_TransitionConstantCount; + stateWorkspace->m_TransitionWorkspaceArray = arAlloc.ConstructArray<TransitionWorkspace*>(stateWorkspace->m_TransitionWorkspaceCount); + stateWorkspace->m_BlendTreeInputArray = arAlloc.ConstructArray<animation::BlendTreeInput*>(apStateConstant->m_MotionSetCount); + stateWorkspace->m_BlendTreeOutputArray = arAlloc.ConstructArray<animation::BlendTreeOutput*>(apStateConstant->m_MotionSetCount); + stateWorkspace->m_BlendTreeWorkspaceArray = arAlloc.ConstructArray<animation::BlendTreeWorkspace*>(apStateConstant->m_MotionSetCount); + stateWorkspace->m_MotionSetCount = apStateConstant->m_MotionSetCount; + + for(uint32_t i = 0 ; i < stateWorkspace->m_TransitionWorkspaceCount ; i++) + { + stateWorkspace->m_TransitionWorkspaceArray[i] = CreateTransitionWorkspace(apStateConstant->m_TransitionConstantArray[i].Get(), arAlloc); + } + + for(uint32_t i = 0 ; i < stateWorkspace->m_MotionSetCount ; i++) + { + animation::BlendTreeConstant const *blendTreeConstant = statemachine::GetBlendTreeConstant(*apStateConstant,i); + if(blendTreeConstant != 0) + { + stateWorkspace->m_BlendTreeInputArray[i] = animation::CreateBlendTreeInput(blendTreeConstant, arAlloc); + stateWorkspace->m_BlendTreeOutputArray[i] = animation::CreateBlendTreeOutput(blendTreeConstant, maxBlendedClip, arAlloc); + stateWorkspace->m_BlendTreeWorkspaceArray[i] = animation::CreateBlendTreeWorkspace(blendTreeConstant,arAlloc); + } + else + { + stateWorkspace->m_BlendTreeInputArray[i] = 0; + stateWorkspace->m_BlendTreeOutputArray[i] = 0; + stateWorkspace->m_BlendTreeWorkspaceArray[i] = 0; + } + } + + return stateWorkspace; + } + + void DestroyStateWorkspace(StateWorkspace* apStateWorkspace, memory::Allocator& arAlloc) + { + if(apStateWorkspace) + { + + for(uint32_t i = 0 ; i < apStateWorkspace->m_TransitionWorkspaceCount; i++) + { + DestroyTransitionWorkspace(apStateWorkspace->m_TransitionWorkspaceArray[i], arAlloc); + } + for(uint32_t i = 0 ; i < apStateWorkspace->m_MotionSetCount ; i++) + { + animation::DestroyBlendTreeInput(apStateWorkspace->m_BlendTreeInputArray[i], arAlloc); + animation::DestroyBlendTreeOutput(apStateWorkspace->m_BlendTreeOutputArray[i], arAlloc); + animation::DestroyBlendTreeWorkspace(apStateWorkspace->m_BlendTreeWorkspaceArray[i], arAlloc); + } + + arAlloc.Deallocate(apStateWorkspace->m_BlendTreeInputArray); + arAlloc.Deallocate(apStateWorkspace->m_BlendTreeOutputArray); + arAlloc.Deallocate(apStateWorkspace->m_BlendTreeWorkspaceArray); + + arAlloc.Deallocate(apStateWorkspace->m_TransitionWorkspaceArray); + arAlloc.Deallocate(apStateWorkspace); + } + } + + + StateOutput* CreateStateOutput(StateConstant const* apStateConstant, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(StateOutput); + return arAlloc.Construct<StateOutput>(); + } + + void DestroyStateOutput(StateOutput* apStateOutput, memory::Allocator& arAlloc) + { + if(apStateOutput) + { + arAlloc.Deallocate(apStateOutput); + } + } + + StateMachineConstant* CreateStateMachineConstant(StateConstant** apStateConstantArray, uint32_t aStateConstantCount, uint32_t aDefaultState, + TransitionConstant** apAnyStateTransitionConstantArray, uint32_t aAnyStateTransitionConstantCount, uint32_t aMotionSetCount, + memory::Allocator& arAlloc) + { + SETPROFILERLABEL(StateMachineConstant); + StateMachineConstant* stateMachineConstant = arAlloc.Construct<StateMachineConstant>(); + stateMachineConstant->m_StateConstantArray = arAlloc.ConstructArray< OffsetPtr<StateConstant> >(aStateConstantCount); + stateMachineConstant->m_AnyStateTransitionConstantArray = arAlloc.ConstructArray< OffsetPtr<TransitionConstant> >(aAnyStateTransitionConstantCount); + + stateMachineConstant->m_StateConstantCount = aStateConstantCount; + stateMachineConstant->m_DefaultState = aDefaultState; + stateMachineConstant->m_AnyStateTransitionConstantCount = aAnyStateTransitionConstantCount; + stateMachineConstant->m_MotionSetCount = aMotionSetCount; + + uint32_t i; + for(i=0;i<aStateConstantCount;i++) + stateMachineConstant->m_StateConstantArray[i] = apStateConstantArray[i]; + + for(i=0;i<aAnyStateTransitionConstantCount;i++) + stateMachineConstant->m_AnyStateTransitionConstantArray[i] = apAnyStateTransitionConstantArray[i]; + + ///////////////////////////////////////////////////////// + // + uint32_t j; + for(j = 0 ; j < stateMachineConstant->m_MotionSetCount; j++) + { + uint32_t clipOffset = 0 ; + for( i = 0 ; i < aStateConstantCount; i++) + { + stateMachineConstant->m_StateConstantArray[i]->m_LeafInfoArray[j].m_IndexOffset = clipOffset; + clipOffset += stateMachineConstant->m_StateConstantArray[i]->m_LeafInfoArray[j].m_Count; + } + } + + return stateMachineConstant; + } + + void DestroyStateMachineConstant(StateMachineConstant* apStateMachineConstant, memory::Allocator& arAlloc) + { + if(apStateMachineConstant) + { + arAlloc.Deallocate(apStateMachineConstant->m_AnyStateTransitionConstantArray); + arAlloc.Deallocate(apStateMachineConstant->m_StateConstantArray); + + arAlloc.Deallocate(apStateMachineConstant); + } + } + + StateMachineInput* CreateStateMachineInput(StateMachineConstant const* apStateMachineConstant, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(StateMachineInput); + StateMachineInput* stateMachineInput = arAlloc.Construct<StateMachineInput>(); + return stateMachineInput; + } + + void DestroyStateMachineInput(StateMachineInput* apStateMachineInput, memory::Allocator& arAlloc) + { + if(apStateMachineInput) + { + arAlloc.Deallocate(apStateMachineInput); + } + } + + StateMachineMemory* CreateStateMachineMemory(StateMachineConstant const* apStateMachineConstant, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(StateMachineMemory); + StateMachineMemory* stateMachineMemory = arAlloc.Construct<StateMachineMemory>(); + stateMachineMemory->m_CurrentStateIndex = apStateMachineConstant->m_DefaultState; + stateMachineMemory->m_StateMemoryCount = apStateMachineConstant->m_StateConstantCount; + stateMachineMemory->m_StateMemoryArray = arAlloc.ConstructArray< OffsetPtr<StateMemory> >(stateMachineMemory->m_StateMemoryCount); + + stateMachineMemory->m_MotionSetAutoWeightArray = arAlloc.ConstructArray<float>(apStateMachineConstant->m_MotionSetCount); + + stateMachineMemory->m_MotionSetCount = apStateMachineConstant->m_MotionSetCount; + + for( uint32_t i = 0 ; i < stateMachineMemory->m_StateMemoryCount; i++) + { + stateMachineMemory->m_StateMemoryArray[i] = CreateStateMemory(apStateMachineConstant->m_StateConstantArray[i].Get(), apStateMachineConstant, arAlloc); + + if(stateMachineMemory->m_StateMemoryArray[i].IsNull()) + { + DestroyStateMachineMemory(stateMachineMemory, arAlloc); + return 0; + } + } + return stateMachineMemory; + } + + void DestroyStateMachineMemory(StateMachineMemory* apStateMachineMemory, memory::Allocator& arAlloc) + { + if(apStateMachineMemory) + { + for(uint32_t i = 0 ; i < apStateMachineMemory->m_StateMemoryCount ; i++) + { + DestroyStateMemory(apStateMachineMemory->m_StateMemoryArray[i].Get(), arAlloc); + } + + arAlloc.Deallocate(apStateMachineMemory->m_MotionSetAutoWeightArray); + arAlloc.Deallocate(apStateMachineMemory->m_StateMemoryArray); + arAlloc.Deallocate(apStateMachineMemory); + } + } + + + StateMachineWorkspace* CreateStateMachineWorkspace(StateMachineConstant const* apStateMachineConstant, uint32_t maxBlendState, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(StateMachineWorkspace); + StateMachineWorkspace* stateMachineWorkspace = arAlloc.Construct<StateMachineWorkspace>(); + + stateMachineWorkspace->m_StateWorkspaceCount = apStateMachineConstant->m_StateConstantCount; + stateMachineWorkspace->m_StateWorkspaceArray = arAlloc.ConstructArray<StateWorkspace*>(stateMachineWorkspace->m_StateWorkspaceCount); + memset(&stateMachineWorkspace->m_StateWorkspaceArray[0], 0, sizeof(StateWorkspace*)*stateMachineWorkspace->m_StateWorkspaceCount); + + for( uint32_t i = 0 ; i < stateMachineWorkspace->m_StateWorkspaceCount; i++) + { + stateMachineWorkspace->m_StateWorkspaceArray[i] = CreateStateWorkspace(apStateMachineConstant->m_StateConstantArray[i].Get(), maxBlendState, arAlloc); + if(stateMachineWorkspace->m_StateWorkspaceArray[i] == 0) + { + DestroyStateMachineWorkspace(stateMachineWorkspace, arAlloc); + return 0; + } + } + + + stateMachineWorkspace->m_AnyStateTransitionWorkspaceCount = apStateMachineConstant->m_AnyStateTransitionConstantCount; + stateMachineWorkspace->m_AnyStateTransitionWorkspaceArray = arAlloc.ConstructArray<TransitionWorkspace*>(stateMachineWorkspace->m_AnyStateTransitionWorkspaceCount); + for( uint32_t i = 0 ; i < stateMachineWorkspace->m_AnyStateTransitionWorkspaceCount; i++) + { + stateMachineWorkspace->m_AnyStateTransitionWorkspaceArray[i] = CreateTransitionWorkspace(apStateMachineConstant->m_AnyStateTransitionConstantArray[i].Get(), arAlloc); + } + + return stateMachineWorkspace; + + } + + void DestroyStateMachineWorkspace(StateMachineWorkspace* apStateMachineWorkspace, memory::Allocator& arAlloc) + { + if(apStateMachineWorkspace) + { + for(uint32_t i = 0 ; i < apStateMachineWorkspace->m_StateWorkspaceCount; i++) + { + DestroyStateWorkspace(apStateMachineWorkspace->m_StateWorkspaceArray[i], arAlloc); + } + + for(uint32_t i = 0 ; i < apStateMachineWorkspace->m_AnyStateTransitionWorkspaceCount ; i++) + { + DestroyTransitionWorkspace(apStateMachineWorkspace->m_AnyStateTransitionWorkspaceArray[i], arAlloc); + } + + arAlloc.Deallocate(apStateMachineWorkspace->m_StateWorkspaceArray); + arAlloc.Deallocate(apStateMachineWorkspace->m_AnyStateTransitionWorkspaceArray); + arAlloc.Deallocate(apStateMachineWorkspace); + } + } + + StateMachineOutput* CreateStateMachineOutput(StateMachineConstant const* apStateMachineConstant, uint32_t maxBlendedClip, memory::Allocator& arAlloc) + { + SETPROFILERLABEL(StateMachineOutput); + StateMachineOutput* stateMachineOutput = arAlloc.Construct<StateMachineOutput>(); + stateMachineOutput->m_MotionSetCount = apStateMachineConstant->m_MotionSetCount; + stateMachineOutput->m_Left.m_BlendNodeLayer = arAlloc.ConstructArray<BlendNodeLayer>(apStateMachineConstant->m_MotionSetCount); + stateMachineOutput->m_Right.m_BlendNodeLayer = arAlloc.ConstructArray<BlendNodeLayer>(apStateMachineConstant->m_MotionSetCount); + + for(int i=0;i<apStateMachineConstant->m_MotionSetCount;i++) + { + stateMachineOutput->m_Left.m_BlendNodeLayer[i].m_OutputBlendArray = arAlloc.ConstructArray<float>(maxBlendedClip); + stateMachineOutput->m_Left.m_BlendNodeLayer[i].m_OutputIndexArray = arAlloc.ConstructArray<uint32_t>(maxBlendedClip); + stateMachineOutput->m_Left.m_BlendNodeLayer[i].m_ReverseArray = arAlloc.ConstructArray<bool>(maxBlendedClip); + stateMachineOutput->m_Left.m_BlendNodeLayer[i].m_MirrorArray = arAlloc.ConstructArray<bool>(maxBlendedClip); + stateMachineOutput->m_Left.m_BlendNodeLayer[i].m_CycleOffsetArray = arAlloc.ConstructArray<float>(maxBlendedClip); + + stateMachineOutput->m_Right.m_BlendNodeLayer[i].m_OutputBlendArray = arAlloc.ConstructArray<float>(maxBlendedClip); + stateMachineOutput->m_Right.m_BlendNodeLayer[i].m_OutputIndexArray = arAlloc.ConstructArray<uint32_t>(maxBlendedClip); + stateMachineOutput->m_Right.m_BlendNodeLayer[i].m_ReverseArray = arAlloc.ConstructArray<bool>(maxBlendedClip); + stateMachineOutput->m_Right.m_BlendNodeLayer[i].m_MirrorArray = arAlloc.ConstructArray<bool>(maxBlendedClip); + stateMachineOutput->m_Right.m_BlendNodeLayer[i].m_CycleOffsetArray = arAlloc.ConstructArray<float>(maxBlendedClip); + } + + return stateMachineOutput; + } + + void DestroyStateMachineOutput(StateMachineOutput* apStateMachineOutput, memory::Allocator& arAlloc) + { + if(apStateMachineOutput) + { + for(int i=0;i<apStateMachineOutput->m_MotionSetCount;i++) + { + arAlloc.Deallocate(apStateMachineOutput->m_Right.m_BlendNodeLayer[i].m_OutputBlendArray); + arAlloc.Deallocate(apStateMachineOutput->m_Right.m_BlendNodeLayer[i].m_OutputIndexArray); + arAlloc.Deallocate(apStateMachineOutput->m_Right.m_BlendNodeLayer[i].m_ReverseArray); + arAlloc.Deallocate(apStateMachineOutput->m_Right.m_BlendNodeLayer[i].m_MirrorArray); + arAlloc.Deallocate(apStateMachineOutput->m_Right.m_BlendNodeLayer[i].m_CycleOffsetArray); + + arAlloc.Deallocate(apStateMachineOutput->m_Left.m_BlendNodeLayer[i].m_OutputBlendArray); + arAlloc.Deallocate(apStateMachineOutput->m_Left.m_BlendNodeLayer[i].m_OutputIndexArray); + arAlloc.Deallocate(apStateMachineOutput->m_Left.m_BlendNodeLayer[i].m_ReverseArray); + arAlloc.Deallocate(apStateMachineOutput->m_Left.m_BlendNodeLayer[i].m_MirrorArray); + arAlloc.Deallocate(apStateMachineOutput->m_Left.m_BlendNodeLayer[i].m_CycleOffsetArray); + } + + arAlloc.Deallocate(apStateMachineOutput->m_Left.m_BlendNodeLayer); + arAlloc.Deallocate(apStateMachineOutput->m_Right.m_BlendNodeLayer); + arAlloc.Deallocate(apStateMachineOutput); + } + } + + float DoBlendTreeEvaluation( const StateConstant& arStateConstant, StateOutput& arStateOutput, StateMemory& arStateMemory, StateWorkspace& arStateWorkspace, const ValueArrayConstant& arValues, const StateMachineInput& arStateMachineInput, int blendTreeIndex, float weight) + { + float duration = 0 ; + for(uint32_t i = 0 ; i < arStateConstant.m_MotionSetCount ; i++) + { + animation::BlendTreeConstant const* treeConstant = statemachine::GetBlendTreeConstant(arStateConstant, i); + animation::BlendTreeMemory const *treeMemory = statemachine::GetBlendTreeMemory(arStateConstant,arStateMemory, i); + + if(treeConstant) + { + if(!treeConstant->m_BlendEventArrayConstant.IsNull()) + { + for(uint32_t k = 0 ; k < treeConstant->m_BlendEventArrayConstant->m_Count ; k++) + { + float blendValue = 0.0f; + int32_t index = FindValueIndex(&arValues, treeConstant->m_BlendEventArrayConstant->m_ValueArray[k].m_ID); + if(index >=0) + { + arStateMachineInput.m_Values->ReadData(blendValue, arValues.m_ValueArray[index].m_Index); + } + arStateWorkspace.m_BlendTreeInputArray[i]->m_BlendValueArray->WriteData(blendValue, treeConstant->m_BlendEventArrayConstant->m_ValueArray[k].m_Index); + } + } + + animation::EvaluateBlendTree(*treeConstant, *arStateWorkspace.m_BlendTreeInputArray[i], *treeMemory, *arStateWorkspace.m_BlendTreeOutputArray[i], *arStateWorkspace.m_BlendTreeWorkspaceArray[i]); + + uint32_t index = 0 ; + uint32_t leafIndex = 0; + while(index < arStateWorkspace.m_BlendTreeOutputArray[i]->m_MaxBlendedClip && arStateWorkspace.m_BlendTreeOutputArray[i]->m_OutputBlendArray[index].m_ID != -1) + { + arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_OutputBlendArray[arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_OutputCount] = weight*arStateWorkspace.m_BlendTreeOutputArray[i]->m_OutputBlendArray[index].m_BlendValue; + + arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_ReverseArray[arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_OutputCount] = ( arStateWorkspace.m_BlendTreeOutputArray[i]->m_OutputBlendArray[index].m_Reverse && arStateConstant.m_Speed >= 0) || + ( !arStateWorkspace.m_BlendTreeOutputArray[i]->m_OutputBlendArray[index].m_Reverse && arStateConstant.m_Speed < 0); + + arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_MirrorArray[arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_OutputCount] = ( arStateWorkspace.m_BlendTreeOutputArray[i]->m_OutputBlendArray[index].m_Mirror || arStateConstant.m_Mirror) && + !(arStateWorkspace.m_BlendTreeOutputArray[i]->m_OutputBlendArray[index].m_Mirror && arStateConstant.m_Mirror); + + arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_CycleOffsetArray[arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_OutputCount] = arStateWorkspace.m_BlendTreeOutputArray[i]->m_OutputBlendArray[index].m_CycleOffset + arStateConstant.m_CycleOffset; + + for( ; leafIndex< arStateConstant.m_LeafInfoArray[i].m_Count ; leafIndex++) + { + /// match leaf ID + if(arStateWorkspace.m_BlendTreeOutputArray[i]->m_OutputBlendArray[index].m_ID == arStateConstant.m_LeafInfoArray[i].m_IDArray[leafIndex]) + { + arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_OutputIndexArray[arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_OutputCount] = leafIndex + arStateConstant.m_LeafInfoArray[i].m_IndexOffset; + arStateOutput.m_BlendNode->m_BlendNodeLayer[i].m_OutputCount++; + leafIndex++; + break; + } + } + + index++; + } + + // sync layers affect timing + float effectiveDurationWeight = 1 ; + for(int index = arStateConstant.m_MotionSetCount - 1 ; index >= (int)i+1; --index) + { + if(statemachine::GetBlendTreeConstant(arStateConstant, index)) // if has motion + effectiveDurationWeight -= (effectiveDurationWeight*arStateMachineInput.m_MotionSetTimingWeightArray[index]); + } + + duration += arStateWorkspace.m_BlendTreeOutputArray[i]->m_Duration*arStateMachineInput.m_MotionSetTimingWeightArray[i]*effectiveDurationWeight; + + + } + } + + return duration; + } + + void EvaluateState( ValueArrayConstant const* apValues, + StateConstant const *apStateConstant, + StateMachineInput const* apStateMachineInput, + StateMachineMemory* apStateMachineMemory, + StateOutput *apStateOutput, + StateMemory *apStateMemory, + StateWorkspace *apStateWorkspace) + { + for(int i = 0 ; i < apStateConstant->m_MotionSetCount ; i++) + { + apStateOutput->m_BlendNode->m_BlendNodeLayer[i].m_OutputCount = 0; + } + + apStateOutput->m_BlendNode->m_IKOnFeet = apStateConstant->m_IKOnFeet; + + float deltaTime = apStateMachineInput->m_DeltaTime; + + float speed = IS_CONTENT_NEWER_OR_SAME(kUnityVersion4_2_a1) ? math::abs(apStateConstant->m_Speed) : 1.f; + + apStateOutput->m_StateDuration = DoBlendTreeEvaluation( *apStateConstant, *apStateOutput, *apStateMemory, *apStateWorkspace, *apValues, *apStateMachineInput, 0, 1.f) / speed; + + if( apStateOutput->m_StateDuration != 0) + { + deltaTime /= apStateOutput->m_StateDuration; + } + + if(apStateMachineMemory->m_ActiveGotoState && apStateMachineInput->m_GotoStateInfo->m_StateID == 0) + { + apStateOutput->m_BlendNode->m_CurrentTime = apStateMachineInput->m_GotoStateInfo->m_NormalizedTime + (apStateMachineInput->m_GotoStateInfo->m_DenormalizedTimeOffset/apStateOutput->m_StateDuration); + apStateOutput->m_BlendNode->m_PreviousTime = apStateMachineInput->m_GotoStateInfo->m_NormalizedTime - deltaTime; + apStateMachineMemory->m_ActiveGotoState = false; + apStateMachineInput->m_GotoStateInfo->m_DenormalizedTimeOffset = 0.0f; + } + else + { + apStateOutput->m_BlendNode->m_CurrentTime = apStateMemory->m_PreviousTime + deltaTime; + apStateOutput->m_BlendNode->m_PreviousTime = apStateMemory->m_PreviousTime ; + } + + apStateMemory->m_PreviousTime = apStateOutput->m_BlendNode->m_CurrentTime; + apStateMemory->m_Duration = apStateOutput->m_StateDuration; + } + + void EvaluateTransition ( TransitionConstant const* apTransitionConstant, TransitionInput const *apTransitionInput, TransitionOutput * apTransitionOutput, TransitionMemory const* apTransitionMemory, TransitionWorkspace const* apTransitionWorkspace) + { + apTransitionOutput->m_DoTransition = apTransitionConstant->m_ConditionConstantCount > 0; + apTransitionOutput->m_TransitionDuration = apTransitionConstant->m_TransitionDuration; + apTransitionOutput->m_TransitionOffset = apTransitionConstant->m_TransitionOffset; + apTransitionOutput->m_TransitionStartTime = 0.0f; + apTransitionOutput->m_NextStateStartTime = apTransitionConstant->m_TransitionOffset; + + + for( uint32_t conditionIndex = 0 ; apTransitionOutput->m_DoTransition && conditionIndex < apTransitionConstant->m_ConditionConstantCount ; conditionIndex++) + { + apTransitionOutput->m_DoTransition = false; + const ConditionConstant* currentCondition = apTransitionConstant->m_ConditionConstantArray[conditionIndex].Get(); + + if( currentCondition->m_ConditionMode == kConditionModeIf || + currentCondition->m_ConditionMode == kConditionModeIfNot || + currentCondition->m_ConditionMode == kConditionModeGreater || + currentCondition->m_ConditionMode == kConditionModeLess || + currentCondition->m_ConditionMode == kConditionModeEquals || + currentCondition->m_ConditionMode == kConditionModeNotEqual) + { + int32_t index = FindValueIndex(apTransitionMemory->m_ValuesConstant, currentCondition->m_EventID); + + if(index > -1) + { + ValueConstant const& valueConstant = apTransitionMemory->m_ValuesConstant->m_ValueArray[index]; + if(currentCondition->m_ConditionMode == kConditionModeIf || currentCondition->m_ConditionMode == kConditionModeIfNot) + { + bool booleanEvent; + apTransitionInput->m_Values->ReadData(booleanEvent, valueConstant.m_Index); + apTransitionOutput->m_DoTransition = currentCondition->m_ConditionMode == kConditionModeIf ? booleanEvent : !booleanEvent; + } + else if (currentCondition->m_ConditionMode == kConditionModeEquals || currentCondition->m_ConditionMode == kConditionModeNotEqual) + { + mecanim::int32_t intEvent; + apTransitionInput->m_Values->ReadData(intEvent, valueConstant.m_Index); + apTransitionOutput->m_DoTransition = currentCondition->m_ConditionMode == kConditionModeEquals ? intEvent == currentCondition->m_EventThreshold : intEvent != currentCondition->m_EventThreshold; + } + else + { + if(valueConstant.m_Type == mecanim::kFloatType) + { + float floatEvent; + apTransitionInput->m_Values->ReadData(floatEvent, valueConstant.m_Index); + apTransitionOutput->m_DoTransition = currentCondition->m_ConditionMode == kConditionModeGreater ? floatEvent > currentCondition->m_EventThreshold : floatEvent < currentCondition->m_EventThreshold; + } + else if(valueConstant.m_Type == mecanim::kInt32Type) + { + mecanim::int32_t intEvent; + apTransitionInput->m_Values->ReadData(intEvent, valueConstant.m_Index); + apTransitionOutput->m_DoTransition = currentCondition->m_ConditionMode == kConditionModeGreater ? intEvent > currentCondition->m_EventThreshold : intEvent < currentCondition->m_EventThreshold; + } + + } + } + } + else if(currentCondition->m_ConditionMode == kConditionModeExitTime) + { + float relativeTimeError = 0; + if(currentCondition->m_ExitTime <= 1) + { + float previousTimeLow = math::fmod(apTransitionInput->m_PreviousTime,1); + float currentTimeLow = math::fmod(apTransitionInput->m_CurrentTime,1); + + float previousTimeHigh = previousTimeLow; + float currentTimeHigh = currentTimeLow; + + if(previousTimeLow > currentTimeLow) + { + previousTimeLow -= 1; + currentTimeHigh += 1; + } + + if(previousTimeLow < currentCondition->m_ExitTime && currentTimeLow >= currentCondition->m_ExitTime) + { + apTransitionOutput->m_DoTransition = true; + relativeTimeError = (currentTimeLow - currentCondition->m_ExitTime); + } + else if(previousTimeHigh < currentCondition->m_ExitTime && currentTimeHigh >= currentCondition->m_ExitTime) + { + apTransitionOutput->m_DoTransition = true; + relativeTimeError = (currentTimeHigh - currentCondition->m_ExitTime); + } + } + else if ( apTransitionInput->m_PreviousTime < currentCondition->m_ExitTime && apTransitionInput->m_CurrentTime >= currentCondition->m_ExitTime) + { + apTransitionOutput->m_DoTransition = true; + relativeTimeError = (apTransitionInput->m_CurrentTime - currentCondition->m_ExitTime); + } + + if(apTransitionOutput->m_DoTransition) + { + apTransitionOutput->m_TransitionStartTime = apTransitionOutput->m_TransitionDuration == 0 ? 1 : (relativeTimeError / apTransitionOutput->m_TransitionDuration); + apTransitionOutput->m_NextStateStartTime = apTransitionOutput->m_TransitionOffset; + apTransitionOutput->m_NextStateStartInitialDeltaTime = relativeTimeError; // this is in source state normalized time + } + } + } + + } + + void EvaluateStateMachine( StateMachineConstant const* apStateMachineConstant, + StateMachineInput const* apStateMachineInput, + StateMachineOutput * apStateMachineOutput, + StateMachineMemory * apStateMachineMemory, + StateMachineWorkspace * apStateMachineWorkspace) + { + + /// Initialize workspace values + apStateMachineOutput->m_BlendFactor = 0; + for(uint32_t i = 0 ; i < apStateMachineConstant->m_MotionSetCount; i++) + { + apStateMachineOutput->m_Right.m_BlendNodeLayer[i].m_OutputCount = 0; + apStateMachineOutput->m_Left.m_BlendNodeLayer[i].m_OutputCount = 0; + } + + if(apStateMachineConstant->m_StateConstantCount == 0) + { + for(int i = 0 ; i < apStateMachineConstant->m_MotionSetCount; i++) + apStateMachineMemory->m_MotionSetAutoWeightArray[i] = 0; + return ; + } + + float deltaTime = apStateMachineInput->m_DeltaTime; + + const uint32_t currentStateIndex = apStateMachineMemory->m_CurrentStateIndex; + const StateConstant* currentState = apStateMachineConstant->m_StateConstantArray[currentStateIndex].Get(); + StateMemory* currentStateMemory = apStateMachineMemory->m_StateMemoryArray[currentStateIndex].Get(); + StateWorkspace* currentStateWS = apStateMachineWorkspace->m_StateWorkspaceArray[currentStateIndex]; + + StateOutput outputSrc; + outputSrc.m_BlendNode = &apStateMachineOutput->m_Left; + EvaluateState(apStateMachineWorkspace->m_ValuesConstant, currentState, apStateMachineInput, apStateMachineMemory, &outputSrc, currentStateMemory, currentStateWS); + + TransitionInput transitionInput; + transitionInput.m_Values = apStateMachineInput->m_Values; + transitionInput.m_CurrentStatePreviousTime = currentStateMemory->m_PreviousTime; + transitionInput.m_CurrentStateDuration = outputSrc.m_StateDuration; + transitionInput.m_CurrentTime = outputSrc.m_BlendNode->m_CurrentTime; + transitionInput.m_PreviousTime = outputSrc.m_BlendNode->m_PreviousTime; + + TransitionOutput transitionOutput; + + TransitionMemory transitionMemory; + transitionMemory.m_ValuesConstant = apStateMachineWorkspace->m_ValuesConstant; + + bool startTransition = false; + bool wasInTransition = apStateMachineMemory->m_InTransition ; + + const TransitionConstant* currentTransition = wasInTransition ? GetTransitionConstant(apStateMachineConstant, apStateMachineConstant->m_StateConstantArray[apStateMachineMemory->m_CurrentStateIndex].Get(), apStateMachineMemory->m_TransitionId) : 0; + bool isCurrentTransitionAnyState = apStateMachineMemory->m_TransitionId >= s_AnyTransitionEncodeKey; + + for(int i = 0 ; i < apStateMachineConstant->m_MotionSetCount; i++) + apStateMachineMemory->m_MotionSetAutoWeightArray[i] = outputSrc.m_BlendNode->m_BlendNodeLayer[i].m_OutputCount ? 1 : 0; + + //////////////////////////////////////////////////////// + // Validate if we need to transition + if(apStateMachineMemory->m_ActiveGotoState) + { + int32_t nextState = GetStateIndex(apStateMachineConstant, apStateMachineInput->m_GotoStateInfo->m_StateID); + if(nextState != -1) + { + startTransition = true; + + apStateMachineMemory->m_ActiveGotoState = false; + + apStateMachineMemory->m_InTransition = true; + apStateMachineMemory->m_NextStateIndex = nextState; + apStateMachineMemory->m_TransitionId = s_DynamicTransitionEncodeKey; + apStateMachineMemory->m_TransitionDuration = apStateMachineInput->m_GotoStateInfo->m_TransitionDuration; + apStateMachineMemory->m_TransitionOffset = apStateMachineInput->m_GotoStateInfo->m_NormalizedTime; + apStateMachineMemory->m_TransitionTime = apStateMachineInput->m_GotoStateInfo->m_TransitionTime; + + apStateMachineMemory->m_StateMemoryArray[apStateMachineMemory->m_NextStateIndex]->m_PreviousTime = apStateMachineInput->m_GotoStateInfo->m_NormalizedTime + apStateMachineMemory->m_TransitionTime; + apStateMachineMemory->m_InInterruptedTransition = wasInTransition; + } + } + + // Dynamic transition cannot be interrupted by any transition + for(uint32_t run = 0 ; apStateMachineMemory->m_TransitionId != s_DynamicTransitionEncodeKey && !startTransition && run < 2 ; run++) // run 0 for AnyState, run 1 for transition in current state + { + ///////////////////// + int transitionCount = 0 ; + + if(apStateMachineMemory->m_InTransition) + { + if(!currentTransition->m_Atomic) + { + if ( run == 0 ) + transitionCount = isCurrentTransitionAnyState ? apStateMachineMemory->m_TransitionId - s_AnyTransitionEncodeKey : apStateMachineConstant->m_AnyStateTransitionConstantCount; + else + transitionCount = !isCurrentTransitionAnyState? apStateMachineMemory->m_TransitionId : 0 ; // anyState has higher priority + } + // else -> the transition is atomic, cannot be interrupted, transitionCount = 0 + } + else + { + transitionCount = run == 0 ? apStateMachineConstant->m_AnyStateTransitionConstantCount : currentState->m_TransitionConstantCount; + } + + for(uint32_t i = 0 ; !startTransition && i < transitionCount; i++) + { + const TransitionConstant* transitionConstant = run == 0 ? apStateMachineConstant->m_AnyStateTransitionConstantArray[i].Get() : currentState->m_TransitionConstantArray[i].Get(); + TransitionWorkspace* transitionWorkspace = run == 0 ? apStateMachineWorkspace->m_AnyStateTransitionWorkspaceArray[i] : currentStateWS->m_TransitionWorkspaceArray[i]; + + transitionMemory.m_InTransition = apStateMachineMemory->m_InTransition; + + EvaluateTransition(transitionConstant, &transitionInput, &transitionOutput, &transitionMemory, transitionWorkspace); + + if(transitionOutput.m_DoTransition) + { + startTransition = true; + + apStateMachineMemory->m_InTransition = true; + apStateMachineMemory->m_NextStateIndex = transitionConstant->m_DestinationState; + apStateMachineMemory->m_TransitionId = run == 0 ? (i + s_AnyTransitionEncodeKey) : i; + apStateMachineMemory->m_TransitionDuration = transitionOutput.m_TransitionDuration; + apStateMachineMemory->m_TransitionOffset = transitionOutput.m_TransitionOffset; + apStateMachineMemory->m_TransitionTime = transitionOutput.m_TransitionStartTime; + apStateMachineMemory->m_InInterruptedTransition = wasInTransition; + + apStateMachineMemory->m_ActiveGotoState = true; + apStateMachineInput->m_GotoStateInfo->m_StateID = 0; + apStateMachineInput->m_GotoStateInfo->m_NormalizedTime = transitionOutput.m_NextStateStartTime; + apStateMachineInput->m_GotoStateInfo->m_DenormalizedTimeOffset = transitionOutput.m_NextStateStartInitialDeltaTime * currentStateMemory->m_Duration; // denormalize source state time to denormalizedTime + + } + } + } + + + if(apStateMachineMemory->m_InTransition) + { + const int32_t nextStateIndex = apStateMachineMemory->m_NextStateIndex; + const StateConstant* nextState = apStateMachineConstant->m_StateConstantArray[nextStateIndex].Get(); + StateMemory* nextStateMemory = apStateMachineMemory->m_StateMemoryArray[nextStateIndex].Get(); + StateWorkspace* nextStateWS = apStateMachineWorkspace->m_StateWorkspaceArray[nextStateIndex]; + + if(wasInTransition) // delta already applied in m_TransitionStartTime when transition was just triggered, so dont add twice. + apStateMachineMemory->m_TransitionTime += apStateMachineMemory->m_TransitionDuration == 0 ? 1 : (deltaTime / (apStateMachineMemory->m_TransitionDuration * (outputSrc.m_StateDuration != 0 ? outputSrc.m_StateDuration : 1.0f))); + apStateMachineOutput->m_BlendFactor = math::saturate(apStateMachineMemory->m_TransitionTime); + + + StateOutput outputDst; + outputDst.m_BlendNode = &apStateMachineOutput->m_Right; + EvaluateState(apStateMachineWorkspace->m_ValuesConstant, nextState, apStateMachineInput, apStateMachineMemory, &outputDst, nextStateMemory, nextStateWS); + + for(int i = 0 ; i < apStateMachineConstant->m_MotionSetCount; i++) + { + if(apStateMachineOutput->m_Left.m_BlendNodeLayer[i].m_OutputCount == 0 && apStateMachineOutput->m_Right.m_BlendNodeLayer[i].m_OutputCount > 0) + apStateMachineMemory->m_MotionSetAutoWeightArray[i] = apStateMachineOutput->m_BlendFactor; + else if(apStateMachineOutput->m_Right.m_BlendNodeLayer[i].m_OutputCount == 0 && apStateMachineOutput->m_Left.m_BlendNodeLayer[i].m_OutputCount > 0) + apStateMachineMemory->m_MotionSetAutoWeightArray[i] = 1 - apStateMachineOutput->m_BlendFactor; + + } + + ///////////////////////////////////////// + // Transition is finished + if(apStateMachineMemory->m_TransitionTime >= 1) + { + for( uint32_t conditionIndex = 0 ; currentTransition != 0 && conditionIndex < currentTransition->m_ConditionConstantCount ; conditionIndex++) + { + const ConditionConstant* currentCondition = currentTransition->m_ConditionConstantArray[conditionIndex].Get(); + + if( currentCondition->m_ConditionMode == kConditionModeIf) + { + int32_t index = FindValueIndex(apStateMachineWorkspace->m_ValuesConstant, currentCondition->m_EventID); + + if(index > -1) + { + ValueConstant const& valueConstant = apStateMachineWorkspace->m_ValuesConstant->m_ValueArray[index]; + if(valueConstant.m_Type == mecanim::kTriggerType) + apStateMachineInput->m_Values->WriteData(false, valueConstant.m_Index); + } + } + } + + apStateMachineMemory->m_InTransition = false; + apStateMachineMemory->m_TransitionTime = 0.f; + apStateMachineMemory->m_TransitionId = 0; + apStateMachineMemory->m_TransitionDuration = 0.f; + apStateMachineMemory->m_TransitionOffset = 0.f; + apStateMachineMemory->m_CurrentStateIndex = apStateMachineMemory->m_NextStateIndex; + apStateMachineMemory->m_InInterruptedTransition = false; + + + BlendNodeLayer *nodeSwap = apStateMachineOutput->m_Left.m_BlendNodeLayer; + apStateMachineOutput->m_Left = apStateMachineOutput->m_Right; + apStateMachineOutput->m_Right.m_BlendNodeLayer = nodeSwap; + apStateMachineOutput->m_BlendFactor = 0.f; + apStateMachineOutput->m_Right.m_BlendNodeLayer[0].m_OutputCount = 0; + + } + } + } + + void StateConstant::InitializeClass() + { + RegisterAllowNameConversion("StateConstant", "m_ID", "m_PathID"); + } + + +}//namespace statemachine + +}//namespace mecanim diff --git a/Runtime/mecanim/statemachine/statemachine.h b/Runtime/mecanim/statemachine/statemachine.h new file mode 100644 index 0000000..6de46a9 --- /dev/null +++ b/Runtime/mecanim/statemachine/statemachine.h @@ -0,0 +1,547 @@ +#pragma once + +#include "Runtime/mecanim/animation/blendtree.h" +#include "Runtime/mecanim/generic/valuearray.h" + +#include "Runtime/Serialize/Blobification/offsetptr.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Animation/MecanimArraySerialization.h" + +namespace mecanim +{ + +namespace statemachine +{ + static const mecanim::int32_t s_AnyTransitionEncodeKey = 20000; + static const mecanim::int32_t s_DynamicTransitionEncodeKey = 30000; + + enum ConditionMode + { + //kConditionModeGraph = 0, + kConditionModeIf = 1, // backward compat + kConditionModeIfNot, + kConditionModeGreater, + kConditionModeLess, + kConditionModeExitTime, + kConditionModeEquals, + kConditionModeNotEqual, + kConditionModeCount + }; + + struct GotoStateInfo + { + GotoStateInfo():m_StateID(0),m_NormalizedTime(0),m_TransitionDuration(0),m_TransitionTime(0), m_DenormalizedTimeOffset(0){} + + uint32_t m_StateID; + float m_NormalizedTime; + float m_TransitionDuration; + float m_TransitionTime; + float m_DenormalizedTimeOffset; // used to offset by seconds, internally used when transitions are taken + }; + + + struct BlendNodeLayer + { + BlendNodeLayer() : m_OutputCount(0), m_OutputIndexArray(0), m_OutputBlendArray(0), m_ReverseArray(0), m_MirrorArray(0), m_CycleOffsetArray(0) {} + uint32_t m_OutputCount; + uint32_t* m_OutputIndexArray; + float* m_OutputBlendArray; + bool* m_ReverseArray; + bool* m_MirrorArray; + float* m_CycleOffsetArray; + }; + + struct BlendNode + { + BlendNode(): m_BlendNodeLayer(0), m_CurrentTime(0),m_PreviousTime(0), m_IKOnFeet(false){} + + BlendNodeLayer* m_BlendNodeLayer; + + float m_CurrentTime; + float m_PreviousTime; + bool m_IKOnFeet; + }; + + + struct ConditionConstant + { + DEFINE_GET_TYPESTRING(ConditionConstant) + + ConditionConstant() : m_ConditionMode(kConditionModeIf), + m_EventID(0), + m_EventThreshold(0.f), + m_ExitTime(0) + { + } + + uint32_t m_ConditionMode; + uint32_t m_EventID; + + float m_EventThreshold; + float m_ExitTime; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_ConditionMode); + TRANSFER(m_EventID); + TRANSFER(m_EventThreshold); + TRANSFER(m_ExitTime); + } + }; + + struct TransitionConstant + { + DEFINE_GET_TYPESTRING(TransitionConstant) + + TransitionConstant() : m_ConditionConstantCount(0), + m_DestinationState(0), + m_ID(0), + m_UserID(0), + m_TransitionDuration(0), + m_TransitionOffset(0), + m_Atomic(true) + + {} + + + uint32_t m_ConditionConstantCount; + OffsetPtr< OffsetPtr<ConditionConstant> > m_ConditionConstantArray; + + uint32_t m_DestinationState; + uint32_t m_ID; + uint32_t m_UserID; + + float m_TransitionDuration; + float m_TransitionOffset; + + bool m_Atomic; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_ConditionConstantCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<mecanim::statemachine::ConditionConstant>, m_ConditionConstantArray, m_ConditionConstantCount); + + TRANSFER(m_DestinationState); + TRANSFER(m_ID); + TRANSFER(m_UserID); + + TRANSFER(m_TransitionDuration); + TRANSFER(m_TransitionOffset); + + TRANSFER(m_Atomic); + transfer.Align(); + } + }; + + struct TransitionWorkspace + { + TransitionWorkspace(): m_ConditionConstantCount(0) + { + } + + uint32_t m_ConditionConstantCount; + }; + + + struct TransitionInput + { + TransitionInput(): m_Values(0), + m_CurrentStatePreviousTime(0), + m_CurrentStateDuration(0), + m_CurrentTime(0), + m_PreviousTime() + {} + ValueArray* m_Values; + float m_CurrentStatePreviousTime; + float m_CurrentStateDuration; + float m_CurrentTime; + float m_PreviousTime; + }; + + + struct TransitionOutput + { + TransitionOutput(): m_DoTransition(false), + m_TransitionStartTime(0), + m_NextStateStartTime(0), + m_NextStateStartInitialDeltaTime(0), + m_TransitionDuration(0), + m_TransitionOffset(0) + {} + + bool m_DoTransition; + float m_TransitionStartTime; + float m_NextStateStartTime; + float m_NextStateStartInitialDeltaTime; + float m_TransitionDuration; + float m_TransitionOffset; + + }; + + struct TransitionMemory + { + TransitionMemory(): m_ValuesConstant(0), + m_InTransition(false) + {} + + ValueArrayConstant const* m_ValuesConstant; + bool m_InTransition; + }; + + + struct LeafInfoConstant + { + DEFINE_GET_TYPESTRING(LeafInfoConstant) + + LeafInfoConstant() : m_Count(0), m_IndexOffset(0) + { + + } + + uint32_t m_Count; + OffsetPtr<uint32_t> m_IDArray; + uint32_t m_IndexOffset; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_Count); + MANUAL_ARRAY_TRANSFER2(mecanim::uint32_t, m_IDArray, m_Count); + TRANSFER(m_IndexOffset); + } + }; + + struct StateConstant + { + DEFINE_GET_TYPESTRING(StateConstant) + + StateConstant() : m_TransitionConstantCount(0), + m_MotionSetCount(0), + m_BlendTreeCount(0), + m_NameID(0), + m_PathID(0), + m_TagID(0), + m_Speed(1), + m_CycleOffset(0), + m_IKOnFeet(true), + m_Loop(false), + m_Mirror(false) + { + } + + uint32_t m_TransitionConstantCount; + OffsetPtr< OffsetPtr<TransitionConstant> > m_TransitionConstantArray; + + uint32_t m_MotionSetCount; + OffsetPtr<int32_t> m_BlendTreeConstantIndexArray; + OffsetPtr<LeafInfoConstant> m_LeafInfoArray; + + uint32_t m_BlendTreeCount; + OffsetPtr< OffsetPtr<animation::BlendTreeConstant> > m_BlendTreeConstantArray; + + uint32_t m_NameID; + uint32_t m_PathID; + uint32_t m_TagID; + + float m_Speed; + float m_CycleOffset; + bool m_IKOnFeet; + bool m_Loop; + bool m_Mirror; + + static void InitializeClass(); + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_TransitionConstantCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<mecanim::statemachine::TransitionConstant>, m_TransitionConstantArray, m_TransitionConstantCount); + + TRANSFER_BLOB_ONLY(m_MotionSetCount); + MANUAL_ARRAY_TRANSFER2(int32_t, m_BlendTreeConstantIndexArray, m_MotionSetCount); + MANUAL_ARRAY_TRANSFER2(LeafInfoConstant, m_LeafInfoArray, m_MotionSetCount); + + TRANSFER_BLOB_ONLY(m_BlendTreeCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<mecanim::animation::BlendTreeConstant>, m_BlendTreeConstantArray, m_BlendTreeCount); + + TRANSFER(m_NameID); + TRANSFER(m_PathID); + TRANSFER(m_TagID); + + TRANSFER(m_Speed); + TRANSFER(m_CycleOffset); + + TRANSFER(m_IKOnFeet); + TRANSFER(m_Loop); + TRANSFER(m_Mirror); + + transfer.Align(); + } + }; + + struct StateMemory + { + DEFINE_GET_TYPESTRING(StateMemory) + + StateMemory(): m_PreviousTime(0), m_Duration(0) + {} + uint32_t m_BlendTreeCount; + OffsetPtr< OffsetPtr<animation::BlendTreeMemory> > m_BlendTreeMemoryArray; + + float m_PreviousTime; + float m_Duration; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_BlendTreeCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<mecanim::animation::BlendTreeMemory>, m_BlendTreeMemoryArray, m_BlendTreeCount); + + TRANSFER(m_PreviousTime); + TRANSFER(m_Duration); + + transfer.Align(); + } + + }; + + struct StateWorkspace + { + StateWorkspace():m_TransitionWorkspaceArray(0), + m_BlendTreeInputArray(0), + m_BlendTreeOutputArray(0), + m_BlendTreeWorkspaceArray(0), + m_TransitionWorkspaceCount(0) + {} + + TransitionWorkspace** m_TransitionWorkspaceArray; + + + animation::BlendTreeInput** m_BlendTreeInputArray; + animation::BlendTreeOutput** m_BlendTreeOutputArray; + animation::BlendTreeWorkspace** m_BlendTreeWorkspaceArray; + + uint32_t m_TransitionWorkspaceCount; + uint32_t m_MotionSetCount; + }; + + + + struct StateOutput + { + StateOutput() : m_BlendNode(0), m_StateDuration(0) {} + + float m_StateDuration; + + BlendNode* m_BlendNode; + }; + + + struct StateMachineConstant + { + DEFINE_GET_TYPESTRING(StateMachineConstant) + + StateMachineConstant() : m_StateConstantCount(0), + m_AnyStateTransitionConstantCount(0), + m_DefaultState(0), + m_MotionSetCount(0) + {} + + uint32_t m_StateConstantCount; + OffsetPtr< OffsetPtr<StateConstant> > m_StateConstantArray; + + uint32_t m_AnyStateTransitionConstantCount; + OffsetPtr< OffsetPtr<TransitionConstant> > m_AnyStateTransitionConstantArray; + + uint32_t m_DefaultState; + uint32_t m_MotionSetCount; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_StateConstantCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<mecanim::statemachine::StateConstant> , m_StateConstantArray, m_StateConstantCount); + + TRANSFER_BLOB_ONLY(m_AnyStateTransitionConstantCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<mecanim::statemachine::TransitionConstant>, m_AnyStateTransitionConstantArray, m_AnyStateTransitionConstantCount); + + TRANSFER(m_DefaultState); + TRANSFER(m_MotionSetCount); + } + }; + + struct StateMachineInput + { + StateMachineInput() : m_Values(0) + {} + + float m_DeltaTime; + ValueArray* m_Values; + + float* m_MotionSetTimingWeightArray; // allocated externally + + GotoStateInfo* m_GotoStateInfo; + }; + + struct StateMachineMemory + { + DEFINE_GET_TYPESTRING(StateMachineMemory) + + StateMachineMemory() : m_MotionSetCount(0), + m_StateMemoryCount(0), + m_CurrentStateIndex(0), + m_NextStateIndex(0), + m_TransitionId(0), + m_TransitionTime(0), + m_TransitionDuration(0), + m_TransitionOffset(0), + m_InInterruptedTransition(false), + m_InTransition(false), + m_ActiveGotoState(false) + {} + + uint32_t m_MotionSetCount; + OffsetPtr<float> m_MotionSetAutoWeightArray; + + uint32_t m_StateMemoryCount; + OffsetPtr<OffsetPtr<StateMemory> > m_StateMemoryArray; + + uint32_t m_CurrentStateIndex; + uint32_t m_NextStateIndex; + uint32_t m_TransitionId; + + float m_TransitionTime; + float m_TransitionDuration; + float m_TransitionOffset; + + bool m_InInterruptedTransition; + bool m_InTransition; + bool m_ActiveGotoState; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER_BLOB_ONLY(m_MotionSetCount); + MANUAL_ARRAY_TRANSFER2( float, m_MotionSetAutoWeightArray, m_MotionSetCount); + + TRANSFER_BLOB_ONLY(m_StateMemoryCount); + MANUAL_ARRAY_TRANSFER2( OffsetPtr<StateMemory>, m_StateMemoryArray, m_StateMemoryCount); + + TRANSFER(m_CurrentStateIndex); + TRANSFER(m_NextStateIndex); + TRANSFER(m_TransitionId); + + TRANSFER(m_TransitionTime); + TRANSFER(m_TransitionDuration); + TRANSFER(m_TransitionOffset); + + TRANSFER(m_InInterruptedTransition); + TRANSFER(m_InTransition); + TRANSFER(m_ActiveGotoState); + transfer.Align(); + } + }; + + struct StateMachineWorkspace + { + StateMachineWorkspace() : m_StateWorkspaceArray(0), + m_StateWorkspaceCount(0), + m_AnyStateTransitionWorkspaceArray(0), + m_AnyStateTransitionWorkspaceCount(0) + {} + + StateWorkspace** m_StateWorkspaceArray; + uint32_t m_StateWorkspaceCount; + + TransitionWorkspace** m_AnyStateTransitionWorkspaceArray; + uint32_t m_AnyStateTransitionWorkspaceCount; + + ValueArrayConstant* m_ValuesConstant; + + }; + + struct StateMachineOutput + { + StateMachineOutput(): m_BlendFactor(0),m_MotionSetCount(0) + {} + + float m_BlendFactor; + + BlendNode m_Left; + BlendNode m_Right; + + uint32_t m_MotionSetCount; + + + }; + + + + TransitionConstant const* GetTransitionConstant(StateMachineConstant const* apStateMachineConstant, StateConstant const* apStateConstant, uint32_t id); + + bool IsCurrentTransitionAtomic(StateMachineConstant const* apStateMachineConstant, StateMachineMemory *apStateMachineMemory); + + ConditionConstant *CreateConditionConstant( uint32_t aConditionMode, uint32_t aEventID, float aEventThreshold, float aExitTime, memory::Allocator& arAlloc); + void DestroyConditionConstant(ConditionConstant *apConditionConstant, memory::Allocator& arAlloc); + + TransitionConstant *CreateTransitionConstant(ConditionConstant** apConditionsConstantArray, uint32_t aConditionConstantCount, uint32_t aDestinationState, + float aTransitionDuration, float aTransitionOffset, bool aAtomic, uint32_t aID, uint32_t aUserID, memory::Allocator& arAlloc); + + void DestroyTransitionConstant(TransitionConstant *apTransitionConstant, memory::Allocator& arAlloc); + + + TransitionWorkspace* CreateTransitionWorkspace(TransitionConstant const* apTransitionConstant, memory::Allocator& arAlloc); + void DestroyTransitionWorkspace(TransitionWorkspace* apTransitionWorkspace, memory::Allocator& arAlloc); + + + StateConstant* CreateStateConstant(TransitionConstant** apTransitionConstantArray, uint32_t aTransitionConstantCount, + float aSpeed, bool aIKOnFeet, bool aMirror, float aCycleOffset, animation::BlendTreeConstant** apBlendTreeConstantArray, + uint32_t aMotionSetCount, uint32_t nameID, uint32_t pathID, uint32_t aTagID, bool aLoop, memory::Allocator& arAlloc); + + void DestroyStateConstant(StateConstant* apStateConstant, memory::Allocator& arAlloc); + + void DestroyStateMemory(StateMemory* apStateMemory, memory::Allocator& arAlloc); + + StateWorkspace* CreateStateWorkspace(StateConstant const* apStateConstant, memory::Allocator& arAlloc); + void DestroyStateWorkspace(StateWorkspace* apStateWorkspace, memory::Allocator& arAlloc); + + StateOutput* CreateStateOutput(StateConstant const* apStateConstant, memory::Allocator& arAlloc); + void DestroyStateOutput(StateOutput* apStateOutput, memory::Allocator& arAlloc); + + StateMachineConstant* CreateStateMachineConstant(StateConstant** apStateConstantArray, uint32_t aStateConstantCount, uint32_t aDefaultState, + TransitionConstant** apAnyStateTransitionConstantArray, uint32_t aAnyStateTransitionConstantCount, uint32_t aMotionSetCount, + memory::Allocator& arAlloc); + void DestroyStateMachineConstant(StateMachineConstant* apStateMachineConstant, memory::Allocator& arAlloc); + + void ClearStateMachineConstant(StateMachineConstant* apStateMachineConstant, memory::Allocator& arAlloc); + + StateMachineInput* CreateStateMachineInput(StateMachineConstant const* apStateMachineConstant, memory::Allocator& arAlloc); + void DestroyStateMachineInput(StateMachineInput* apStateMachineInput, memory::Allocator& arAlloc); + + StateMachineMemory* CreateStateMachineMemory(StateMachineConstant const* apStateMachineConstant, memory::Allocator& arAlloc); + void DestroyStateMachineMemory(StateMachineMemory* apStateMachineMemory, memory::Allocator& arAlloc); + + + StateMachineWorkspace* CreateStateMachineWorkspace(StateMachineConstant const* apStateMachineConstant, uint32_t maxBlendedClip, memory::Allocator& arAlloc); + void DestroyStateMachineWorkspace(StateMachineWorkspace* apStateMachineWorkspace, memory::Allocator& arAlloc); + + StateMachineOutput* CreateStateMachineOutput(StateMachineConstant const* apStateMachineConstant, uint32_t maxBlendedClip, memory::Allocator& arAlloc); + void DestroyStateMachineOutput(StateMachineOutput* apStateMachineOutput, memory::Allocator& arAlloc); + + void EvaluateStateMachine(StateMachineConstant const* apStateMachineConstant, StateMachineInput const* apStateMachineInput, + StateMachineOutput * apStateMachineOutput, StateMachineMemory * apStateMachineMemory, + StateMachineWorkspace * apStateMachineWorkspace); + + animation::BlendTreeConstant const* GetBlendTreeConstant(const StateConstant& arStateConstant, mecanim::int32_t aMotionSetCount); + animation::BlendTreeMemory *GetBlendTreeMemory(const StateConstant& arStateConstant,StateMemory& arStateMemory, mecanim::int32_t aMotionSetIndex); + + int32_t GetStateIndex(StateMachineConstant const* apStateMachineConstant, uint32_t id); + + inline int32_t CompareStateID (StateConstant const* state, uint32_t id) { return state->m_PathID == id || state->m_NameID == id; } + } +} + + + diff --git a/Runtime/mecanim/string.h b/Runtime/mecanim/string.h new file mode 100644 index 0000000..8a1d772 --- /dev/null +++ b/Runtime/mecanim/string.h @@ -0,0 +1,96 @@ +#pragma once + + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/types.h" + +#include "Runtime/Serialize/SerializeTraits.h" + +#include <string.h> + +namespace mecanim +{ + template<int CAPACITY = 128> class basic_string + { + protected: + char eos(){return '\0';} + char mStr[CAPACITY]; + void terminatestr(){mStr[CAPACITY-1] = eos();} + public: + + typedef char value_type; + typedef char* iterator; + typedef char const * const_iterator; + + static const int npos = CAPACITY; + static const int capacity = CAPACITY; + + basic_string(){ mStr[0] = eos(); } + basic_string( const basic_string& str ){ strncpy(mStr, str.mStr, max_size()); terminatestr(); } + explicit basic_string( const char * s, std::size_t n = CAPACITY){ strncpy(mStr, s, n); terminatestr(); } + + char const* c_str()const{return mStr; } + + const char& operator[] ( std::size_t pos ) const{ return mStr[pos]; } + char& operator[] ( std::size_t pos ){ return mStr[pos]; } + + iterator begin(){return &mStr[0];} + const_iterator begin() const{return &mStr[0];} + + iterator end(){return &mStr[size()];} + const_iterator end() const{return &mStr[size()];} + + basic_string& operator= ( const basic_string& str ){ strncpy(mStr, str.mStr, max_size()); terminatestr(); return *this; } + basic_string& operator= ( const char* s ){ strncpy(mStr, s, max_size()); terminatestr(); return *this; } + + basic_string& operator+= ( const basic_string& str ){ strncat(mStr, str.mStr, max_size()-size() ); terminatestr(); return *this; } + basic_string& operator+= ( const char* s ){ strncat(mStr, s, max_size()-size()); terminatestr(); return *this; } + + int compare ( const basic_string& str ) const { return strcmp(mStr, str.mStr); } + int compare ( const char* s ) const { return strcmp(mStr, s); } + int compare ( size_t pos1, size_t n1, const basic_string& str ) const { return strncmp(mStr+pos1, str.mStr, n1); } + int compare ( size_t pos1, size_t n1, const char* s) const{ return strncmp(mStr+pos1, s, n1); } + + basic_string substr ( size_t pos = 0, size_t n = npos ) const{ return basic_string( &mStr[pos], n-pos ); } + + size_t find ( const basic_string& str, size_t pos = 0 ) const + { + return find(str.mStr, pos); + } + size_t find ( const char* s, size_t pos = 0 ) const + { + size_t i = npos; + char const* p = strstr(&mStr[pos], s); + if(p) + { + i = reinterpret_cast<size_t>(p) - reinterpret_cast<size_t>(mStr); + } + return i; + } + + void resize(size_t size) + { + for(int i = 0; i < size && i < max_size();++i) + mStr[i] = ' '; + mStr[ size < max_size()-1 ? size : max_size()-1 ] = eos(); + } + + size_t size() const { return strlen(mStr); } + bool empty() const { return size() == 0; } + void clear() { mStr[0] = eos(); } + static size_t max_size(){ return CAPACITY; } + }; + + template<int CAPACITY1, int CAPACITY2> bool operator==(basic_string<CAPACITY1> const& l, basic_string<CAPACITY2> const& r){ return l.compare(r.c_str()) == 0; } + template<int CAPACITY> bool operator==(basic_string<CAPACITY> const& l, const char* r){ return l.compare(r) == 0; } + + template<int CAPACITY1, int CAPACITY2> bool operator!=(basic_string<CAPACITY1> const& l, basic_string<CAPACITY2> const& r){ return l.compare(r.c_str()) != 0; } + template<int CAPACITY> bool operator!=(basic_string<CAPACITY> const& l, const char* r){ return l.compare(r) != 0; } + + template<int CAPACITY1, int CAPACITY2> bool operator<(basic_string<CAPACITY1> const& l, basic_string<CAPACITY2> const& r){ return l.compare(r.c_str()) < 0; } + template<int CAPACITY> bool operator<(basic_string<CAPACITY> const& l, const char* r){ return l.compare(r) < 0; } + + + typedef basic_string<> String; +} + diff --git a/Runtime/mecanim/types.h b/Runtime/mecanim/types.h new file mode 100644 index 0000000..088e0cb --- /dev/null +++ b/Runtime/mecanim/types.h @@ -0,0 +1,221 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" + +// Microsoft is not supporting standard integer type define in C99 <cstdint> +// +#if defined(_MSC_VER) || defined(__GNUC__) + +#include <cfloat> +#include <climits> + +namespace mecanim +{ +// 8-bit types -----------------------------------------------------------// +# if UCHAR_MAX == 0xff + typedef signed char int8_t; + typedef unsigned char uint8_t; + + #define INT8_T_MIN CHAR_MIN + #define INT8_T_MAX CHAR_MAX + #define UINT8_T_MIN 0 + #define UINT8_T_MAX UCHAR_MAX +# else +# error defaults not correct; you must hand modify types.h +# endif + +// 16-bit types -----------------------------------------------------------// +# if USHRT_MAX == 0xffff + typedef short int16_t; + typedef unsigned short uint16_t; + + #define INT16_T_MIN SHRT_MIN + #define INT16_T_MAX SHRT_MAX + #define UINT16_T_MIN 0 + #define UINT16_T_MAX USHRT_MAX +# else +# error defaults not correct; you must hand modify types.h +# endif + +// 32-bit types -----------------------------------------------------------// +#if UINT_MAX == 0xffffffff + typedef int int32_t; + typedef unsigned int uint32_t; + + #define INT32_T_MIN INT_MIN + #define INT32_T_MAX INT_MAX + #define UINT32_T_MIN 0 + #define UINT32_T_MAX UINT_MAX +#elif ULONG_MAX == 0xffffffff + typedef long int32_t; + typedef unsigned long uint32_t; + + #define INT32_T_MIN LONG_MIN + #define INT32_T_MAX LONG_MAX + #define UINT32_T_MIN 0 + #define UINT32_T_MAX ULONG_MAX +# else +# error defaults not correct; you must hand modify types.h +# endif + +// 64-bit types -----------------------------------------------------------// +# if ULLONG_MAX == 0xffffffffffffffff + typedef long long int64_t; + typedef unsigned long long uint64_t; + + #define INT64_T_MIN LLONG_MIN + #define INT64_T_MAX LLONG_MAX + #define UINT64_T_MIN 0 + #define UINT64_T_MAX ULLONG_MAX +# else +//# error defaults not correct; you must hand modify types.h + typedef long long int64_t; + typedef unsigned long long uint64_t; +# endif + //typedef char String[20]; +}; +#else + +#include <cfloat> +#include <climits> +#include <cstdint.h> +namespace mecanim +{ + using std::int8_t; + using std::uint8_t; + using std::int16_t; + using std::uint16_t; + using std::int32_t; + using std::uint32_t; + using std::int64_t; + using std::uint64_t; + + #define INT8_T_MIN CHAR_MIN + #define INT8_T_MAX CHAR_MAX + #define UINT8_T_MIN 0 + #define UINT8_T_MAX UCHAR_MAX + + #define INT16_T_MIN SHRT_MIN + #define INT16_T_MAX SHRT_MAX + #define UINT16_T_MIN 0 + #define UINT16_T_MAX USHRT_MAX + +# if ULONG_MAX == 0xffffffff + #define INT32_T_MIN LONG_MIN + #define INT32_T_MAX LONG_MAX + #define UINT32_T_MIN 0 + #define UINT32_T_MAX ULONG_MAX +# elif UINT_MAX == 0xffffffff + #define INT32_T_MIN INT_MIN + #define INT32_T_MAX INT_MAX + #define UINT32_T_MIN 0 + #define UINT32_T_MAX UINT_MAX +# else +# error defaults not correct; you must hand modify types.h +# endif + #define INT64_T_MIN LLONG_MIN + #define INT64_T_MAX LLONG_MAX + #define UINT64_T_MIN 0 + #define UINT64_T_MAX ULLONG_MAX +}; + +#endif + +namespace mecanim +{ + template <typename _Ty> struct BindValue + { + typedef _Ty value_type; + + value_type mValue; + uint32_t mID; + }; + + template <typename _Ty> struct numeric_limits + { + typedef _Ty Type; + + // C++ allow us to initialize only a static constant data member if + // it has an integral or enumeration Type + + // All other specialization cannot have these member, consequently we do provide + // min/max trait has inline member function for those case. + static const Type min_value = Type(0); + static const Type max_value = Type(0); + + static Type min() { return min_value; } + static Type max() { return max_value; } + }; + + template<> struct numeric_limits<int32_t> + { + typedef int32_t Type; + + static const Type min_value = Type(INT32_T_MIN); + static const Type max_value = Type(INT32_T_MAX); + + static Type min() { return min_value; } + static Type max() { return max_value; } + }; + + template<> struct numeric_limits<uint32_t> + { + typedef uint32_t Type; + + static const Type min_value = Type(UINT32_T_MIN); + static const Type max_value = Type(UINT32_T_MAX); + + static Type min() { return min_value; } + static Type max() { return max_value; } + }; + + template<> struct numeric_limits<float> + { + typedef float Type; + + static Type min() { return -FLT_MAX; } + static Type max() { return FLT_MAX; } + }; + + template <std::size_t> struct uint_t; + template <> struct uint_t<8> { typedef uint8_t value_type; }; + template <> struct uint_t<16> { typedef uint16_t value_type; }; + template <> struct uint_t<32> { typedef uint32_t value_type; }; + template <> struct uint_t<64> { typedef uint64_t value_type; }; + + template <std::size_t> struct int_t; + template <> struct int_t<8> { typedef int8_t value_type; }; + template <> struct int_t<16> { typedef int16_t value_type; }; + template <> struct int_t<32> { typedef int32_t value_type; }; + template <> struct int_t<64> { typedef int64_t value_type; }; + + + template <bool C, typename Ta, typename Tb> class IfThenElse; + template< typename Ta, typename Tb> class IfThenElse<true, Ta, Tb> + { + public: + typedef Ta result_type; + }; + + template< typename Ta, typename Tb> class IfThenElse<false, Ta, Tb> + { + public: + typedef Tb result_type; + }; + + template<typename T> class is_pointer + { + public: + static const bool value = false; + }; + + template<typename T> class is_pointer<T*> + { + public: + static const bool value = true; + }; + + +} + + |