Webcam/include/engine/motion/quaternion_channel.cpp

/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include "motion/quaternion_channel.h"

		using namespace GS;


//------------------------------------------------------------------------------
static	float	range(float v, float lo, float hi, int *i)
{
	float r = hi - lo;

	if	(!r)
    {
        if	(i)
			*i = 0;
        return lo;
    }

	float v2 = v - lo;

	if	(v2 >= 0)
			v2 = lo + v2 - r * Math::Floor(v2 / r);
	else	v2 = hi + v2 - r * Math::Ceil(v2 / r);

	if	(i)
		*i = - (int)((v2 - v) / r + (v2 > v ? 0.5f : -0.5f));
	return Types::Clamp(v2, lo, hi);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void	QuaternionChannel::Evaluate(Time t, Quaternion &p, Curve::LoopMode loop, Time loop_start, Time loop_end) const
{
	if	(keys.GetCount() == 0)
		return;

	QuaternionKey *skey = keys[0], *ekey = keys[keys.GetCount() - 1];

	loop_start = (loop_start == Time::Inf) ? skey->t : Types::Clamp(loop_start, skey->t, ekey->t);
	loop_end = (loop_end == Time::Inf) ? ekey->t : Types::Clamp(loop_end, skey->t, ekey->t);

	int noff = 0;
	if	(t < loop_start)
	{
		switch (loop)
		{
			default:
			case Curve::Constant:
				Evaluate(loop_start, p, loop, loop_start, loop_end);
				return;
			case Curve::Reset:
				p.Set(0, 0, 0, 1);
				return;

			case Curve::Repeat:
			case Curve::OffsetAndRepeat:
				t.setSec(range(t.toSec(), loop_start.toSec(), loop_end.toSec(), NULL));
				break;
			case Curve::Oscillate:
				t.setSec(range(t.toSec(), loop_start.toSec(), loop_end.toSec(), &noff));
				if	(noff % 2)
					t = loop_end + loop_start - t;
				break;
		}
	}
    else if (t > loop_end)
	{
		switch (loop)
		{
			default:
			case Curve::Constant:
				Evaluate(loop_end, p, loop, loop_start, loop_end);
				return;
			case Curve::Reset:
				p.Set(0, 0, 0, 1);
				return;

			case Curve::Repeat:
			case Curve::OffsetAndRepeat:
				t.setSec(range(t.toSec(), loop_start.toSec(), loop_end.toSec(), NULL));
				break;
			case Curve::Oscillate:
				t.setSec(range(t.toSec(), loop_start.toSec(), loop_end.toSec(), &noff));
				if	(noff % 2)
					t = loop_end + loop_start - t;
				break;
		}
	}

	// Evaluate (t is guaranteed to be in range).
	int ikey0;
#if	1
	{
		uint lo = 0, hi = keys.GetCount() - 1;

		forever
		{
			uint mid = (lo + hi) / 2;

			if	(keys[mid]->t > t)
				hi = mid;
			else
			{
				if	(lo == mid)
				{
					ikey0 = lo;
					break;
				}
				else
					lo = mid;
			}
		}
	}
#else
	for	(ikey0 = 1; ikey0 < int(keys.GetCount()); ikey0++)
		if	(keys[ikey0]->t > t)
			break;
	--ikey0;
#endif

	// Slerp.
	if	(ikey0 == (int(keys.GetCount()) - 1))
		p = keys[ikey0]->q;
	else
	{
		const float k = (t - keys[ikey0]->t).toSec() / (keys[ikey0 + 1]->t - keys[ikey0]->t).toSec();
		p = Quaternion::Slerp(k, keys[ikey0]->q, keys[ikey0 + 1]->q).Normalize();
	}
}
uint	QuaternionChannel::Optimize(float threshold)
{
	return 0;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
TimeRange	QuaternionChannel::GetTimeRange() const
{
	Time min, max;
	if	(keys.GetCount() > 0)
	{
		min = max = keys[0]->t;
		for (uint n = 1; n < keys.GetCount(); ++n)
		{
			min = Types::Min(min, keys[n]->t);
			max = Types::Max(max, keys[n]->t);
		}
	}
	return TimeRange(min, max);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool	QuaternionChannel::Insert(const QuaternionKey &key)
{
	uint n = 0;
	for (; n < keys.GetCount(); ++n)
		if	(keys[n]->t > key.t)
			break;

	return keys.Insert(new QuaternionKey(key), n);
}
void	QuaternionChannel::Clear()
{
	ArrayListDeleteAllPtr(QuaternionKey *, keys)
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
QuaternionChannel::QuaternionChannel()
{}
QuaternionChannel::~QuaternionChannel()
{	Clear();	}
//------------------------------------------------------------------------------