Hermite splines

HermiteSpline.hh

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

OrientationMapping.hh

#pragma once
#include "util/Numeric.h"

namespace math
{
	inline float4 SO3ToR4(Quat const& q)
	{
		float const mul = 1.0f / sqrt(2.0f * (1.0f - q.w));
		return float4(q.x, q.y, q.z, (1.0f - q.w)).Mul(mul);
	}

	inline Quat R4ToSO3(float4 const& v)
	{
		float4 const sq = v.Mul(v);
		float const denom = v.SumOfElements();
		float const s = sq.xyz().SumOfElements() - sq.w;
		return Quat(
			2.0f*v.x*v.w/denom,
			2.0f*v.y*v.w/denom,
			2.0f*v.z*v.w/denom,
			s/denom);
	}
}