diff options
Diffstat (limited to 'Runtime/mecanim/skeleton/skeleton.cpp')
| -rw-r--r-- | Runtime/mecanim/skeleton/skeleton.cpp | 515 |
1 files changed, 515 insertions, 0 deletions
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; + } +} +} + |