Hermite spline application

HermiteSpline.hpp

#pragma once

#include "math/OrientationMapping.h"
#include "util/Numeric.h"
#include "util/PaddedVector.h"

namespace math
{
    template <size_t A, size_t B>
    float H(float t);

	template <>
	inline float H<0,0>(float t)
	{
		return 2*t*t*t - 3*t*t + 1;
	}

	template <>
	inline float H<1,0>(float t)
	{
		return t*t*t - 2*t*t + t;
	}

	template <>
	inline float H<0,1>(float t)
	{
		return -2*t*t*t + 3*t*t;
	}

	template <>
	inline float H<1,1>(float t)
	{
		return t*t*t - t*t;
	}
	
	template <typename V>
	struct HermiteSpline
	{
		static size_t const componentCount = V::Size;
		util::PaddedVector<V> p;
		util::PaddedVector<V> m;

		explicit HermiteSpline(int n = 0)
			: p(n, 4, 4)
			, m(n, 4, 4)
		{}

		HermiteSpline(util::PaddedVector<V> const& p, util::PaddedVector<V> const& m)
			: p(p)
			, m(m)
		{}

		V Evaluate(float x) const
		{
			x = Clamp(x, 0.0f, (float)(p.n+1));
			V ret;
			float k;
			float const t = std::modf(x, &k);
			float const xDiff = 1.0f;
			return
				H<0,0>(t) * p[k  ] +
				H<0,1>(t) * p[k+1] +
				H<1,0>(t) * m[k  ] * xDiff +
				H<1,1>(t) * m[k+1] * xDiff;
		}
	};

	template <>
	struct HermiteSpline<Quat>
	{
		HermiteSpline<float4> baseSpline;

		explicit HermiteSpline(int n = 0)
			: baseSpline(n)
		{}

		HermiteSpline(util::PaddedVector<float4> const& p, util::PaddedVector<float4> const& m)
			: baseSpline(p, m)
		{}

		Quat Evaluate(float x) const
		{
			float4 v = baseSpline.Evaluate(x);
			return math::R4ToSO3(v);
		}
	};
}