prioritybuffer.h

Particle filtering implementation and application to people tracking.

#ifndef _buffer_h_
#define _buffer_h_

#include <vector>

template<typename T, class Numeric=double>
class prioritybuffer
{
	public:
		prioritybuffer()
		{
			m_size = 0;
			m_maxindex = 0;
		}

		prioritybuffer(unsigned int m)
		{
			m_size = 0;
			m_maxindex = m;
			m_data.resize(1);
		}
		
		typedef typename std::vector<T>::const_iterator const_iterator;

		const_iterator begin() const
		{
			return m_data.begin();
		}

		const_iterator end() const
		{
			return m_data.end();
		}

		bool push(const T& t)
		{
			if (m_size < m_maxindex)
			{
				for (typename std::vector<T>::iterator it=m_data.begin(); it!=m_data.end(); ++it)
				{
					if ((Numeric)(t) >= (Numeric)(*it))
					{
						m_data.insert(it,t);
						++m_size;
						return true;
					}
				}
				return false;
			}
			else
			{
				for (typename std::vector<T>::iterator it=m_data.begin(); it!=m_data.end(); ++it)
				{
					if ((Numeric)(t) >= (Numeric)(*it))
					{
						m_data.insert(it,t);
						m_data.pop_back();
						return true;
					}
				}
				return false;
			}
		}

		T& operator[] (unsigned int i)
		{
			return m_data[i];
		}

		const unsigned int size()
		{
			return m_size;
		}

	private:
		std::vector<T> m_data;
		unsigned int m_size;
		unsigned int m_maxindex;
};

#endif