Skeleton pose evaluation with support for blending and layers

Source/BansheeCore/CMakeSources.cmake

@@ -502,7 +502,7 @@ set(BS_BANSHEECORE_INC_ANIMATION
 	"Include/BsAnimationClip.h"
 	"Include/BsSkeleton.h"
 	"Include/BsAnimation.h"
-	"Include/BsAnimationInstance.h"
+	"Include/BsCurveEvaluator.h"
 )
 
 set(BS_BANSHEECORE_SRC_ANIMATION

Source/BansheeCore/Include/BsAnimationClip.h

@@ -14,6 +14,8 @@ namespace BansheeEngine
 	 *  @{
 	 */
 
+	struct AnimationCurveMapping;
+
 	/** A set of animation curves representing translation/rotation/scale and generic animation. */
 	struct AnimationCurves
 	{
@@ -23,14 +25,6 @@ namespace BansheeEngine
 		Vector<TNamedAnimationCurve<float>> generic;
 	};
 
-	/** Contains indices for position/rotation/scale animation curves. */
-	struct AnimationCurveMapping
-	{
-		UINT32 position;
-		UINT32 rotation;
-		UINT32 scale;
-	};
-
 	/** Types of curves in an AnimationClip. */
 	enum class CurveType
 	{
@@ -144,7 +138,7 @@ namespace BansheeEngine
 		 *							be large enough to store an index for every bone in the @p skeleton. Bones that have
 		 *							no related animation curves will be assigned value -1.
 		 */
-		void getBoneMapping(const SPtr<Skeleton>& skeleton, AnimationCurveMapping* mapping);
+		void getBoneMapping(const Skeleton& skeleton, AnimationCurveMapping* mapping) const;
 
 		/** 
 		 * Returns a version that can be used for detecting modifications on the clip by external systems. Whenever the clip

Source/BansheeCore/Include/BsAnimationCurve.h

@@ -3,7 +3,7 @@
 #pragma once
 
 #include "BsCorePrerequisites.h"
-#include "BsAnimationInstance.h"
+#include "BsCurveEvaluator.h"
 
 namespace BansheeEngine
 {
@@ -47,7 +47,7 @@ namespace BansheeEngine
 		 *								curve value will be clamped.
 		 * @return						Interpolated value from the curve at provided time.
 		 */
-		T evaluate(const AnimationInstanceData<T>& animInstance, bool loop = true);
+		T evaluate(const TCurveEvaluator<T>& animInstance, bool loop = true) const;
 
 		/**
 		 * Evaluate the animation curve at the specified time. If evaluating multiple values in a sequential order consider
@@ -58,7 +58,7 @@ namespace BansheeEngine
 		 *						value will be clamped.
 		 * @return				Interpolated value from the curve at provided time.
 		 */
-		T evaluate(float time, bool loop = true);
+		T evaluate(float time, bool loop = true) const;
 
 	private:
 		friend struct RTTIPlainType<TAnimationCurve<T>>;
@@ -75,7 +75,7 @@ namespace BansheeEngine
 		 * @param[out]	leftKey			Index of the key to interpolate from.
 		 * @param[out]	rightKey		Index of the key to interpolate to.
 		 */
-		void findKeys(float time, const AnimationInstanceData<T>& animInstance, UINT32& leftKey, UINT32& rightKey);
+		void findKeys(float time, const TCurveEvaluator<T>& animInstance, UINT32& leftKey, UINT32& rightKey) const;
 
 		/** 
 		 * Returns a pair of keys that can be used for interpolating to field the value at the provided time. 
@@ -85,7 +85,7 @@ namespace BansheeEngine
 		 * @param[out]	leftKey			Index of the key to interpolate from.
 		 * @param[out]	rightKey		Index of the key to interpolate to.
 		 */
-		void findKeys(float time, UINT32& leftKey, UINT32& rightKey);
+		void findKeys(float time, UINT32& leftKey, UINT32& rightKey) const;
 
 		/** 
 		 * Evaluates a value at the cached curve. Caller must ensure the request time falls within the cached curve range.
@@ -94,7 +94,7 @@ namespace BansheeEngine
 		 *								data from previous requests.
 		 * @return						Interpolated value from the curve at provided time.
 		 */
-		T evaluateCache(const AnimationInstanceData<T>& animInstance);
+		T evaluateCache(const TCurveEvaluator<T>& animInstance) const;
 
 		static const UINT32 CACHE_LOOKAHEAD;
 

Source/BansheeCore/Include/BsAnimationInstance.h → Source/BansheeCore/Include/BsCurveEvaluator.h

@@ -15,10 +15,10 @@ namespace BansheeEngine
 	 * You should not use the same instance of this object for evaluating multiple different animation curves.
 	 */
 	template <class T>
-	struct AnimationInstanceData
+	struct TCurveEvaluator
 	{
 	public:
-		AnimationInstanceData()
+		TCurveEvaluator()
 			: time(0.0f), cachedKey((UINT32)-1), cachedCurveStart(std::numeric_limits<float>::infinity())
 			, cachedCurveEnd(0.0f), cachedCubicCoefficients()
 		{ }

Source/BansheeCore/Include/BsSkeleton.h

@@ -5,6 +5,9 @@
 #include "BsCorePrerequisites.h"
 #include "BsIReflectable.h"
 #include "BsMatrix4.h"
+#include "BsVector3.h"
+#include "BsQuaternion.h"
+#include "BsCurveEvaluator.h"
 
 namespace BansheeEngine
 {
@@ -12,6 +15,14 @@ namespace BansheeEngine
 	 *  @{
 	 */
 
+	/** Contains indices for position/rotation/scale animation curves. */
+	struct AnimationCurveMapping
+	{
+		UINT32 position;
+		UINT32 rotation;
+		UINT32 scale;
+	};
+
 	struct BONE_DESC
 	{
 		String name;
@@ -22,7 +33,13 @@ namespace BansheeEngine
 
 	struct ANIM_BLEND_STATE_DESC
 	{
-		const AnimationClip* clip;
+		SPtr<AnimationCurves> curves;
+		Vector<AnimationCurveMapping> boneToCurveMapping;
+
+		TCurveEvaluator<Vector3> positionEval;
+		TCurveEvaluator<Quaternion> rotationEval;
+		TCurveEvaluator<Vector3> scaleEval;
+
 		float weight;
 		bool loop;
 		UINT8 layer;
@@ -30,16 +47,13 @@ namespace BansheeEngine
 
 	struct SkeletonPose
 	{
-		SkeletonPose(UINT32 numBones)
-			:numBones(numBones), bonePoses(bs_newN<Matrix4>(numBones))
-		{}
-
-		~SkeletonPose()
-		{
-			bs_deleteN(bonePoses, numBones);
-		}
+		SkeletonPose(UINT32 numBones);
+		~SkeletonPose();
 
-		Matrix4* bonePoses;
+		Matrix4* bonePoses; // Global bone poses
+		Vector3* positions; // Local positions
+		Quaternion* rotations; // Local rotations
+		Vector3* scales; // Local scales
 		UINT32 numBones;
 	};
 
@@ -55,11 +69,13 @@ namespace BansheeEngine
 		~Skeleton();
 
 		void getPose(SkeletonPose& pose, const AnimationClip& clip, float time, bool loop = true);
-		void getPose(SkeletonPose& pose, const ANIM_BLEND_STATE_DESC* states, UINT32 numStates, float time);
+
+		// Animations in same layer must be consecutive. Layers must be arranged in ascending order.
+		void getPose(SkeletonPose& pose, const ANIM_BLEND_STATE_DESC* states, UINT32 numStates);
 
 		UINT32 getNumBones() const { return mNumBones; }
 		const SkeletonBoneInfo& getBoneInfo(UINT32 idx) const { return mBoneInfo[idx]; }
-		const Matrix4& getBindPose(UINT32 idx) const { return mBindPoses[idx]; }
+		const Matrix4& getBindPose(UINT32 idx) const { return mInvBindPoses[idx]; }
 
 		static SPtr<Skeleton> create(BONE_DESC* bones, UINT32 numBones);
 
@@ -68,7 +84,7 @@ namespace BansheeEngine
 		Skeleton(BONE_DESC* bones, UINT32 numBones);
 
 		UINT32 mNumBones;
-		Matrix4* mBindPoses;
+		Matrix4* mInvBindPoses;
 		SkeletonBoneInfo* mBoneInfo;
 
 		/************************************************************************/

Source/BansheeCore/Include/BsSkeletonRTTI.h

@@ -16,15 +16,15 @@ namespace BansheeEngine
 	class BS_CORE_EXPORT SkeletonRTTI : public RTTIType <Skeleton, IReflectable, SkeletonRTTI>
 	{
 	private:
-		Matrix4& getBindPose(Skeleton* obj, UINT32 idx) { return obj->mBindPoses[idx]; }
-		void setBindPose(Skeleton* obj, UINT32 idx, Matrix4& value) { obj->mBindPoses[idx] = value; }
+		Matrix4& getBindPose(Skeleton* obj, UINT32 idx) { return obj->mInvBindPoses[idx]; }
+		void setBindPose(Skeleton* obj, UINT32 idx, Matrix4& value) { obj->mInvBindPoses[idx] = value; }
 
 		void setNumBindPoses(Skeleton* obj, UINT32 size)
 		{
 			obj->mNumBones = size;
 			
-			assert(obj->mBindPoses == nullptr);
-			obj->mBindPoses = bs_newN<Matrix4>(size);
+			assert(obj->mInvBindPoses == nullptr);
+			obj->mInvBindPoses = bs_newN<Matrix4>(size);
 		}
 
 		SkeletonBoneInfo& getBoneInfo(Skeleton* obj, UINT32 idx) { return obj->mBoneInfo[idx]; }

Source/BansheeCore/Source/BsAnimationClip.cpp

@@ -246,17 +246,17 @@ namespace BansheeEngine
 		Resource::initialize();
 	}
 
-	void AnimationClip::getBoneMapping(const SPtr<Skeleton>& skeleton, AnimationCurveMapping* mapping)
+	void AnimationClip::getBoneMapping(const Skeleton& skeleton, AnimationCurveMapping* mapping) const
 	{
-		UINT32 numBones = skeleton->getNumBones();
+		UINT32 numBones = skeleton.getNumBones();
 		for(UINT32 i = 0; i < numBones; i++)
 		{
-			const SkeletonBoneInfo& boneInfo = skeleton->getBoneInfo(i);
+			const SkeletonBoneInfo& boneInfo = skeleton.getBoneInfo(i);
 
 			auto iterFind = mNameMapping.find(boneInfo.name);
 			if(iterFind != mNameMapping.end())
 			{
-				UINT32* indices = iterFind->second;
+				const UINT32* indices = iterFind->second;
 
 				mapping[i].position = indices[(UINT32)CurveType::Position];
 				mapping[i].rotation = indices[(UINT32)CurveType::Rotation];

Source/BansheeCore/Source/BsAnimationCurve.cpp

@@ -50,7 +50,7 @@ namespace BansheeEngine
 	}
 
 	template <class T>
-	T TAnimationCurve<T>::evaluate(const AnimationInstanceData<T>& animInstance, bool loop)
+	T TAnimationCurve<T>::evaluate(const TCurveEvaluator<T>& animInstance, bool loop) const
 	{
 		if (mKeyframes.size() == 0)
 			return T();
@@ -123,7 +123,7 @@ namespace BansheeEngine
 	}
 
 	template <class T>
-	T TAnimationCurve<T>::evaluate(float time, bool loop)
+	T TAnimationCurve<T>::evaluate(float time, bool loop) const
 	{
 		if (mKeyframes.size() == 0)
 			return T();
@@ -177,7 +177,7 @@ namespace BansheeEngine
 	}
 
 	template <class T>
-	T TAnimationCurve<T>::evaluateCache(const AnimationInstanceData<T>& animInstance)
+	T TAnimationCurve<T>::evaluateCache(const TCurveEvaluator<T>& animInstance) const
 	{
 		float t = animInstance.time - animInstance.cachedCurveStart;
 
@@ -186,7 +186,7 @@ namespace BansheeEngine
 	}
 
 	template <class T>
-	void TAnimationCurve<T>::findKeys(float time, const AnimationInstanceData<T>& animInstance, UINT32& leftKey, UINT32& rightKey)
+	void TAnimationCurve<T>::findKeys(float time, const TCurveEvaluator<T>& animInstance, UINT32& leftKey, UINT32& rightKey) const
 	{
 		// Check nearby keys first if there is cached data
 		if (animInstance.cachedKey != (UINT32)-1)
@@ -234,7 +234,7 @@ namespace BansheeEngine
 	}
 
 	template <class T>
-	void TAnimationCurve<T>::findKeys(float time, UINT32& leftKey, UINT32& rightKey)
+	void TAnimationCurve<T>::findKeys(float time, UINT32& leftKey, UINT32& rightKey) const
 	{
 		INT32 start = 0;
 		INT32 searchLength = (INT32)mKeyframes.size();

Source/BansheeCore/Source/BsSkeleton.cpp

@@ -1,20 +1,52 @@
 //********************************** Banshee Engine (www.banshee3d.com) **************************************************//
 //**************** Copyright (c) 2016 Marko Pintera ([email protected]). All rights reserved. **********************//
 #include "BsSkeleton.h"
+#include "BsAnimationClip.h"
 #include "BsSkeletonRTTI.h"
 
 namespace BansheeEngine
 {
+	SkeletonPose::SkeletonPose(UINT32 numBones)
+		: numBones(numBones)
+	{
+		UINT32 elementSize = sizeof(Matrix4) + sizeof(Vector3) * 2 + sizeof(Quaternion);
+		UINT8* buffer = (UINT8*)bs_alloc(elementSize * sizeof(numBones));
+
+		bonePoses = (Matrix4*)buffer;
+		buffer += sizeof(Matrix4) * numBones;
+
+		positions = (Vector3*)buffer;
+		buffer += sizeof(Vector3) * numBones;
+
+		rotations = (Quaternion*)buffer;
+		buffer += sizeof(Quaternion) * numBones;
+
+		scales = (Vector3*)buffer;
+
+		for (UINT32 i = 0; i < numBones; i++)
+		{
+			bonePoses[i] = Matrix4::IDENTITY;
+			positions[i] = Vector3::ZERO;
+			rotations[i] = Quaternion::IDENTITY;
+			scales[i] = Vector3::ONE;
+		}
+	}
+
+	SkeletonPose::~SkeletonPose()
+	{
+		bs_free(bonePoses);
+	}
+
 	Skeleton::Skeleton()
-		:mBindPoses(nullptr), mBoneInfo(nullptr), mNumBones(0)
+		:mInvBindPoses(nullptr), mBoneInfo(nullptr), mNumBones(0)
 	{ }
 
 	Skeleton::Skeleton(BONE_DESC* bones, UINT32 numBones)
-		:mBindPoses(bs_newN<Matrix4>(numBones)), mBoneInfo(bs_newN<SkeletonBoneInfo>(numBones)), mNumBones(numBones)
+		:mInvBindPoses(bs_newN<Matrix4>(numBones)), mBoneInfo(bs_newN<SkeletonBoneInfo>(numBones)), mNumBones(numBones)
 	{
 		for(UINT32 i = 0; i < numBones; i++)
 		{
-			mBindPoses[i] = bones[i].invBindPose;
+			mInvBindPoses[i] = bones[i].invBindPose;
 			mBoneInfo[i].name = bones[i].name;
 			mBoneInfo[i].parent = bones[i].parent;
 		}
@@ -22,8 +54,8 @@ namespace BansheeEngine
 
 	Skeleton::~Skeleton()
 	{
-		if(mBindPoses != nullptr)
-			bs_deleteN(mBindPoses, mNumBones);
+		if(mInvBindPoses != nullptr)
+			bs_deleteN(mInvBindPoses, mNumBones);
 
 		if (mBoneInfo != nullptr)
 			bs_deleteN(mBoneInfo, mNumBones);
@@ -39,19 +71,129 @@ namespace BansheeEngine
 	void Skeleton::getPose(SkeletonPose& pose, const AnimationClip& clip, float time, bool loop)
 	{
 		ANIM_BLEND_STATE_DESC state;
-		state.clip = &clip;
+		state.curves = clip.getCurves();
 		state.layer = 0;
 		state.loop = loop;
 		state.weight = 1.0f;
+		state.positionEval.time = time;
+		state.rotationEval.time = time;
+		state.scaleEval.time = time;
+
+		state.boneToCurveMapping.resize(mNumBones);
+		clip.getBoneMapping(*this, state.boneToCurveMapping.data());
 
-		getPose(pose, &state, 1, time);
+		getPose(pose, &state, 1);
 	}
 
-	void Skeleton::getPose(SkeletonPose& pose, const ANIM_BLEND_STATE_DESC* states, UINT32 numStates, float time)
+	void Skeleton::getPose(SkeletonPose& pose, const ANIM_BLEND_STATE_DESC* states, UINT32 numStates)
 	{
-		// TODO -Blend locally, normalize all weights to 1, layers for additive animations
+		assert(pose.numBones == mNumBones);
+
+		for(UINT32 i = 0; i < mNumBones; i++)
+		{
+			pose.positions[i] = Vector3::ZERO;
+			pose.rotations[i] = Quaternion::IDENTITY;
+			pose.scales[i] = Vector3::ONE;
+		}
+
+		UINT32 stateIdx = 0;
+		UINT32 currentLayer = 0;
+		UINT32 numStatesInLayer = 0;
+		float layerWeight = 0.0f;
+
+		while(true)
+		{
+			bool lastEntry = stateIdx == numStates;
+			if (lastEntry || currentLayer != states[stateIdx].layer)
+			{
+				if (!Math::approxEquals(layerWeight, 0.0f))
+				{
+					float invLayerWeight = 1.0f / layerWeight;
+					UINT32 start = stateIdx - numStatesInLayer;
+
+					for (UINT32 i = start; i < stateIdx; i++)
+					{
+						const ANIM_BLEND_STATE_DESC& state = states[i];
+
+						float normWeight = state.weight * invLayerWeight;
+						for (UINT32 j = 0; j < mNumBones; j++)
+						{
+							const AnimationCurveMapping& mapping = state.boneToCurveMapping[j];
+
+							if (mapping.position != (UINT32)-1)
+							{
+								const TAnimationCurve<Vector3>& curve = state.curves->position[mapping.position].curve;
+								pose.positions[j] += curve.evaluate(state.positionEval, state.loop) * normWeight;
+							}
+
+							if (mapping.rotation != (UINT32)-1)
+							{
+								const TAnimationCurve<Quaternion>& curve = state.curves->rotation[mapping.rotation].curve;
+								pose.rotations[j] += curve.evaluate(state.rotationEval, state.loop) * normWeight;
+							}
+
+							if (mapping.scale != (UINT32)-1)
+							{
+								const TAnimationCurve<Vector3>& curve = state.curves->scale[mapping.scale].curve;
+								pose.scales[j] += curve.evaluate(state.scaleEval, state.loop) * normWeight;
+							}
+						}
+					}
+				}
+
+				numStatesInLayer = 0;
+				layerWeight = 0.0f;
+
+				if (lastEntry)
+					break;
+			}
+
+			{
+				const ANIM_BLEND_STATE_DESC& state = states[stateIdx];
+				currentLayer = state.layer;
+				layerWeight += state.weight;
+				numStatesInLayer++;
+				stateIdx++;
+			}
+		}
+
+		// Calculate local pose matrices
+		for(UINT32 i = 0; i < mNumBones; i++)
+		{
+			pose.rotations[i].normalize();
+
+			pose.bonePoses[i] = Matrix4::TRS(pose.positions[i], pose.rotations[i], pose.scales[i]);
+			pose.bonePoses[i] = pose.bonePoses[i] * mInvBindPoses[i];
+		}
+
+		// Calculate global poses
+		UINT32 isGlobalBytes = sizeof(bool) * mNumBones;
+		bool* isGlobal = (bool*)bs_stack_alloc(isGlobalBytes);
+		memset(isGlobal, 0, isGlobalBytes);
+
+		std::function<void(UINT32)> calcGlobal = [&](UINT32 boneIdx)
+		{
+			UINT32 parentBoneIdx = mBoneInfo[boneIdx].parent;
+			if (parentBoneIdx == (UINT32)-1)
+			{
+				isGlobal[boneIdx] = true;
+				return;
+			}
+
+			if (!isGlobal[parentBoneIdx])
+				calcGlobal(parentBoneIdx);
+
+			pose.bonePoses[boneIdx] = pose.bonePoses[parentBoneIdx] * pose.bonePoses[boneIdx];
+			isGlobal[boneIdx] = true;
+		};
+
+		for (UINT32 i = 0; i < mNumBones; i++)
+		{
+			if (!isGlobal[i])
+				calcGlobal(i);
+		}
 
-		// TODO
+		bs_stack_free(isGlobal);
 	}
 
 	SPtr<Skeleton> Skeleton::createEmpty()

Source/BansheeUtility/Include/BsQuaternion.h

@@ -225,6 +225,26 @@ namespace BansheeEngine
 			return !operator==(rhs);
 		}
 
+		Quaternion& operator+= (const Quaternion& rhs)
+		{
+			w += rhs.w;
+			x += rhs.x;
+			y += rhs.y;
+			z += rhs.z;
+
+			return *this;
+		}
+
+		Quaternion& operator-= (const Quaternion& rhs)
+		{
+			w -= rhs.w;
+			x -= rhs.x;
+			y -= rhs.y;
+			z -= rhs.z;
+
+			return *this;
+		}
+
 		friend Quaternion operator* (float lhs, const Quaternion& rhs)
 		{
 			return Quaternion(lhs * rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z);