+Unity Runtime codeHEADmaster

1 files changed, 351 insertions, 0 deletions

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);
+			}
+		}
+	}
+}
+}