Webcam/include/engine/motion/motion.cpp

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


		#include "motion/motion.h"
		#include "core/path_kdtree.h"
		#include "math/vector.h"
		#include "geometry/geometric_tools.h"
		#include "time/ntime_range.h"
		#include "memory/memory.h"
		#include "platform.h"
		#include "log/log.h"

		using namespace GS;
		using namespace GS::Core;


//------------------------------------------------------------------------------
void				Motion::Clone(const Motion &src, const TimeRange &range, bool enforce_loop)
{
	// Clone all channels.
	channel_list.Clear();
	ListForeachPtr(MotionChannel *, sc, src.GetChannelList())
	{
		MotionChannel *c = new MotionChannel;
		if	(c == NULL)
			continue;

		c->type = sc->type;

		// Transfer points.
		const ArrayList <CurvePoint *> &points = sc->GetPoints();
		for (uint n = 0; n < points.GetCount(); ++n)
		{
			CurvePoint *p = points[n];
			if	((p->t < range.start) || (p->t > range.end))
				continue;

			CurvePoint np(*p);
			np.t -= range.start;
			c->Append(np);
		}
		channel_list.Add(c);

		// Post-processing.
		const ArrayList <CurvePoint *> &c_points = c->GetPoints();
		uint count = c_points.GetCount();

		if	(count == 0)
			continue;

		if	(enforce_loop)
		{
			c_points[0]->t.setSec(0);	// snap key start

			if	(count > 1)
			{
				c_points[count - 1]->t = range.valueRange();
				c_points[count - 1]->v = c_points[0]->v;
			}
		}
	}

	// Clone quaternion channel.
	quaternion.Clear();

	if	((use_quaternion = src.GetUseQuaternion()) != false)
	{
		const ArrayList <QuaternionKey *> &keys = src.GetQuaternion().GetKeys();

		for (uint n = 0; n < keys.GetCount(); ++n)
		{
			QuaternionKey *k = keys[n];
			if	((k->t < range.start) || (k->t > range.end))
				continue;

			QuaternionKey nk(*k);
			nk.t -= range.start;
			quaternion.Insert(nk);
		}

		// Post-processing.
		const ArrayList <QuaternionKey *> &c_keys = quaternion.GetKeys();
		uint count = c_keys.GetCount();

		if	(enforce_loop)
		{
			c_keys[0]->t.setSec(0);	// snap key start

			if	(count > 1)
			{
				c_keys[count - 1]->t = range.valueRange();
				c_keys[count - 1]->q = c_keys[0]->q;
			}
		}
	}
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool				Motion::HasKey(const TimeRange &t) const
{
	ListForeachPtr(MotionChannel *, c, GetChannelList())
		for (uint n = 0; n < c->GetPointCount(); ++n)
		{
			const CurvePoint *p = c->GetPoints()[n];
			if	(t.inRange(p->t))
				return true;
		}

	return false;
}
void				Motion::MoveKey(const TimeRange &t, const Time &offset) const
{
	ListForeachPtr(MotionChannel *, c, GetChannelList())
	{
		for (uint n = 0; n < c->GetPointCount(); ++n)
		{
			CurvePoint *p = c->GetPoints()[n];
			if	(t.inRange(p->t))
				p->t += offset;
		}
		c->Sort();
	}
}
void				Motion::DeleteKey(const TimeRange &t)
{
	ListForeachPtr(MotionChannel *, c, GetChannelList())
	{
		for (uint n = 0; n < c->GetPointCount(); ++n)
		{
			CurvePoint *p = c->GetPoints()[n];
			if	(t.inRange(p->t))
			{
				c->Delete(p);
				--n;
			}
		}
		c->Sort();
	}
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool				Motion::GetClosestPoint(const Vector4 &position, Vector4 &closest, float *closest_t)
{
	float best_d = -1.f;
	if	(closest_t)
		*closest_t = -1;	
	
	MotionChannel *c[3];
	GetTransformationChannels(c, NULL, NULL);
	
	// kdtree path
	if	(c[0] && c[1] && c[2])
	{		
		if(0 && quadtree.IsNull())	// create it, because we need it
		{
			quadtree = new PathKdtree;
			
			ArrayList<CurvePoint *> &array_x = c[0]->GetPoints();
			ArrayList<CurvePoint *> &array_y = c[1]->GetPoints();
			ArrayList<CurvePoint *> &array_z = c[2]->GetPoints();

			ArrayList <CurvePoint *> ::Iterator iterator_x(array_x); 
			ArrayList <CurvePoint *> ::Iterator iterator_y(array_y); 
			ArrayList <CurvePoint *> ::Iterator iterator_z(array_z); 

			CurvePoint * x = iterator_x.ObjectPtr();
			CurvePoint * y = iterator_y.ObjectPtr();
			CurvePoint * z = iterator_z.ObjectPtr();
		
			Vector4 a_node(x->v,y->v, z->v);
			float a_node_t = x->t.toSec();
		
			// get the second node
				++iterator_x;
				++iterator_y;
				++iterator_z;
			
				x = iterator_x.ObjectPtr();
				y = iterator_y.ObjectPtr();
				z = iterator_z.ObjectPtr();

			while(x && y && z)
			{
				Vector4 b_node(x->v,y->v, z->v);
				float b_node_t = x->t.toSec();
				
				nMSegment * segment = new nMSegment();
				segment->a = a_node;
				segment->b = b_node;
				segment->a_t = a_node_t;
				segment->b_t = b_node_t;

				quadtree->AddSegment(segment);

				a_node = b_node;	
				a_node_t = b_node_t;			
			
				++iterator_x;
				++iterator_y;
				++iterator_z;
			
				x = iterator_x.ObjectPtr();
				y = iterator_y.ObjectPtr();
				z = iterator_z.ObjectPtr();
			}

			quadtree->BuildQuadtree();
		}
		
		// check first if the point is inside the kdtree, else brute force
		if(0 && quadtree->InsideKdTree(position))
		{
			SharedArrayList <nMSegment*> list_segment;
			quadtree->NearestQuadtreeTreeNode(position, list_segment);

			for	(uint i = 0; i < list_segment.GetCount(); ++i)
			{
				Vector4 p;
				float t = GS::Geometric::SegmentClosestPoint(list_segment[i]->a, list_segment[i]->b, position, &p);			

				t = t < 0.0f? 0.0f: (t>1.0f? 1.0f:t);

				float d = Vector4::Dist2(position, p);
				if	((best_d < 0.f) || (d < best_d))
				{
					best_d = d;
					closest = p;
					if	(closest_t)
						*closest_t = t * (list_segment[i]->b_t - list_segment[i]->a_t) + list_segment[i]->a_t;
				}
			}
		}
		else
		//brute force
		{
			ArrayList<CurvePoint *> &array_x = c[0]->GetPoints();
			ArrayList<CurvePoint *> &array_y = c[1]->GetPoints();
			ArrayList<CurvePoint *> &array_z = c[2]->GetPoints();
			
			ArrayList <CurvePoint *> ::Iterator iterator_x(array_x); 
			ArrayList <CurvePoint *> ::Iterator iterator_y(array_y); 
			ArrayList <CurvePoint *> ::Iterator iterator_z(array_z); 

			CurvePoint * x = iterator_x.ObjectPtr();
			CurvePoint * y = iterator_y.ObjectPtr();
			CurvePoint * z = iterator_z.ObjectPtr();
			
			if	(x && y && z)	// [EJ] empty channels would crash on the next line
			{
				Vector4 a_node(x->v,y->v, z->v);
				float a_node_t = x->t.toSec();
		
				// get the second node
					++iterator_x;
					++iterator_y;
					++iterator_z;
			
					x = iterator_x.ObjectPtr();
					y = iterator_y.ObjectPtr();
					z = iterator_z.ObjectPtr();

				while(x && y && z)
				{
					Vector4 b_node(x->v,y->v, z->v);
					float b_node_t = x->t.toSec();
			
					Vector4 p;
					float t = Geometric::SegmentClosestPoint(a_node, b_node, position, &p);			

					t = t < 0.0f? 0.0f: (t>1.0f? 1.0f:t);

					float d = Vector4::Dist2(position, p);
					if	((best_d < 0.f) || (d < best_d))
					{
						best_d = d;
						closest = p;
						if	(closest_t)
							*closest_t = t * (b_node_t - a_node_t) + a_node_t;
					}

					a_node = b_node;	
					a_node_t = b_node_t;			
			
					++iterator_x;
					++iterator_y;
					++iterator_z;
			
					x = iterator_x.ObjectPtr();
					y = iterator_y.ObjectPtr();
					z = iterator_z.ObjectPtr();
				}
			}
		}
	}
	return best_d != -1.f;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				Motion::EvaluateData(const Time &t, Variant &sample)
{
	ArrayListForeachPtr(SDataPoint *, data, data_point)
	{
		if	(data->t <= t)
			sample = data->data;
		else
			return;		// the t in data is more than the t asked so return because the t in data is ordered
	}
}
void				Motion::EvaluatePosition(const Time &t, Vector4 &sample, Curve::LoopMode _loop_mode)
{
	MotionChannel *c[3];
	GetTransformationChannels(c, NULL, NULL);
	for	(int n = 0; n < 3; ++n)
		if	(c[n])
			c[n]->Evaluate(t, &sample[n], _loop_mode);
}
void				Motion::EvaluateRotation(const Time &t, Vector4 &sample, Curve::LoopMode _loop_mode)
{
	MotionChannel *c[3];
	GetTransformationChannels(NULL, c, NULL);
	for	(int n = 0; n < 3; ++n)
		if	(c[n])
			c[n]->Evaluate(t, &sample[n], _loop_mode);
}

void Motion::EvaluateDirection(const Time &t, Vector4 &sample, Curve::LoopMode loop_mode)
{
	const float dt = 0.01f;

	Vector4 p1(0,0,0), p2(0,0,0);

	EvaluatePosition(t, p1, loop_mode);

	Time t2 = Time::fromSec(t.toSec() + dt);
	EvaluatePosition(t2, p2, loop_mode);

	sample = p2 - p1;

	float len = sample.Len();
	if (len > 1e-4f)
		sample /= len;
	else
		sample = Vector4(0,0,0);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				Motion::GetTransformationChannels(MotionChannel *t[3], MotionChannel *r[3], MotionChannel *s[3])
{
	if	(t)	{ t[0] = t[1] = t[2] = NULL; }
	if	(r)	{ r[0] = r[1] = r[2] = NULL; }
	if	(s)	{ s[0] = s[1] = s[2] = NULL; }

	ListForeachPtr(MotionChannel *, channel, channel_list)
		switch (channel->type)
		{
			case MotionChannel::XPos:	if (t) t[0] = channel;	break;
			case MotionChannel::YPos:	if (t) t[1] = channel;	break;
			case MotionChannel::ZPos:	if (t) t[2] = channel;	break;
			case MotionChannel::XRot:	if (r) r[0] = channel;	break;
			case MotionChannel::YRot:	if (r) r[1] = channel;	break;
			case MotionChannel::ZRot:	if (r) r[2] = channel;	break;
			case MotionChannel::XScl:	if (s) s[0] = channel;	break;
			case MotionChannel::YScl:	if (s) s[1] = channel;	break;
			case MotionChannel::ZScl:	if (s) s[2] = channel;	break;

			default: break;
		}
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
TimeRange		Motion::GetTimeRange() const
{
	if	(channel_list.GetCount() == 0)
		return TimeRange();

	List <MotionChannel *> ::Item *i = channel_list.GetRoot();

	TimeRange range = i->Object()->GetTimeRange();
	for	(i = i->Next(); i; i = i->Next())
		range = TimeRange::Union(range, i->Object()->GetTimeRange());

	range = TimeRange::Union(range, quaternion.GetTimeRange());
	return range;
}
Time			Motion::GetDuration() const
{
	Time duration;
	ListForeachPtr(MotionChannel *, channel, channel_list)
		duration = Types::Max(duration, channel->GetDuration());
	return duration;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
MotionChannel		*Motion::AddChannel(MotionChannel::Type type)
{
	MotionChannel *nc = new MotionChannel;
	if	(!nc)
		__ERR__(__LOG_E__ << "Could not allocate channel.\n", NULL)
	nc->type = type;
	channel_list.Add(nc);
	return nc;
}
void				Motion::AddChannels(uint nc)
{
	while (nc--)
		AddChannel(MotionChannel::Undf);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool				Motion::GetUseQuaternion() const
{	return use_quaternion && quaternion.GetKeys().GetCount();	}
MotionChannel		*Motion::GetChannel(uint index) const
{	return GetChannelList().ObjectAt(index);	}
MotionChannel		*Motion::GetChannel(MotionChannel::Type type) const
{
	ListForeachPtr(MotionChannel *, channel, channel_list)
		if	(channel && channel->type == type)
			return channel;
	return NULL;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
size_t				Motion::MemoryFootPrint() const
{
	size_t footprint = 0;
	ListForeachPtr(MotionChannel *, channel, channel_list)
		footprint += channel->MemoryFootPrint();
	footprint += quaternion.MemoryFootPrint();
	footprint += sizeof(Motion);
	return footprint;
}
uint				Motion::Optimize(float threshold)
{
	uint wiped = 0;
	ListForeachPtr(MotionChannel *, channel, channel_list)
		forever
		{
			uint pass_wiped = channel->Optimize(threshold);
			if	(!pass_wiped)
				break;
			wiped += pass_wiped;
		}

	return wiped;
}
//------------------------------------------------------------------------------