polarpoint.h

Particle filtering implementation and application to people tracking.

#ifndef __polarpoint_h__
#define __polarpoint_h__

#include "point.h"

struct PolarPoint
{
	double theta, rho;

	PolarPoint(double theta=0, double rho=0) : theta(theta), rho(rho) {}
	PolarPoint(const Point& p)
	{
		theta = p.theta();
		rho   = p.abs();
	}

	Point getCartesian() const
	{
		return vers(theta) * rho;
	}

	Point vers(double theta) const
	{
		return Point(cos(theta), sin(theta));
	}

	bool operator < (const PolarPoint&m) const {
		return theta < m.theta;
	}

	PolarPoint operator+ (const PolarPoint&  p) const {
		double x = rho*cos(theta) + p.rho*cos(p.theta);
		double y = rho*sin(theta) + p.rho*sin(p.theta);

		if(x==0 && y==0)
		return PolarPoint(0,0);
		else
		return PolarPoint( atan2(y,x), sqrt(x*x+y*y));
	}

};

#endif