math.h

Particle filtering implementation and application to people tracking.

#ifndef UTILS_H
#define UTILS_H

#include <cmath>
#include "point3d.h"
#include "point.h"
#define M_2PI   (2*M_PI)
#define RAND(x) x*(2*drand48()-1)

inline double deg2rad(double d ) { return d * M_PI / 180; }
inline double rad2deg(double d ) { return d * 180 / M_PI; }
inline double    m2mm(double m ) { return      m *  1000; }
inline double    mm2m(double mm) { return     mm * 0.001; }
inline double    norm(double a ) { return atan2(sin(a),cos(a)); }

inline double randrange(double r) { return 2*r*(drand48() - .5); }
inline double randrange(double rmin, double rmax) { return (rmax-rmin)*drand48() + rmin; }

template <class X> inline X sgn(X d) { return d<0?-1:1; }
/** Squares a value. */
template<class X> inline X square(const X& x) { return x*x; }


template <class T>
inline point3d<T> sgn (const point3d<T>& p)
{
	return point3d<T>(p.x<0?-p.x:p.x,p.y<0?-p.y:p.y,p.z<0?-p.z:p.z);
}

template <class T>
inline point3d<T> crossprod (const point3d<T>& p1, const point3d<T>& p2)
{
	return point3d<T>(p1.y*p2.z - p1.z*p2.y,
										p1.z*p2.x - p1.x*p2.z,
										p1.x*p2.y - p1.y*p2.x);
}

template <class T>
inline point<T> getIntersection (const point<T>& p1, const point<T>& p2, const point<T>& p3, const point<T>& p4)
{
	T ua =  ((p4.x-p3.x)*(p1.y-p3.y) - (p4.y-p3.y)*(p1.x-p3.x))
				 /((p4.y-p3.y)*(p2.x-p1.x) - (p4.x-p3.x)*(p2.y-p1.y));
	
	return point<T>(p1.x + ua*(p2.x-p1.x),p1.y + ua*(p2.y-p1.y));
}

template <class T>
inline bool clip(point<T>& pa, point<T>& pb, const point<T>& p1, const point<T>& p2, const point<T>& p3, const point<T>& p4)
{
	point<T> dtoclip = p1 - p2; // Vector2<double> dq=lineToClip.p1-lineToClip.p0;
	point<T> dcut = p3 - p4; // Vector2<double> dp=cut.p1-cut.p0;
	point<T> n(-dcut.y, dcut.x); //Vector2<double> n(-dp.y, dp.x);
	bool firstInside = (n * (p1 - p3)) > 0; //bool firstInside=(n*(lineToClip.p0-cut.p0))>0.;
	bool secondInside = (n * (p2 - p3)) > 0; //bool secondInside=(n*(lineToClip.p1-cut.p0))>0.;
	pa = p1;
	pb = p2;
	//SimpleLine l = lineToClip;
	//r=l;
	if (firstInside&&secondInside)
		return true;
	if (!firstInside && !secondInside) 
		return false;
	
	double det = n*dtoclip; //double det=n*dq;
	if (det==0)
		return false;
	//compute the intersection;
	point<T> delta = p3 - p1; //Vector2<double> delta=cut.p0-lineToClip.p0;
	det=1./det;
	double t = det*(point<T>(-dtoclip.y,dtoclip.x)*delta); //double t = det*(Vector2<double>(-dq.y,dq.x)*delta);
	point<T> replaced = p3 + dcut*t; //Vector2<double> replaced=cut.p0+dp*t;
	if (firstInside)
		pb = replaced; //  l.p1=replaced;
	else
		pa = replaced;//  l.p0=replaced;
	//r=l;	
	return true;
}

template <class T>
inline void calculateplane(T& A, T& B, T& C, T& D,
		const point3d<T>& p1,
		const point3d<T>& p2,
		const point3d<T>& p3)
{
	A = p1.y*(p2.z - p3.z) + p2.y*(p3.z - p1.z) + p3.y*(p1.z - p2.z);
	B = p1.z*(p2.x - p3.x) + p2.z*(p3.x - p1.x) + p3.z*(p1.x - p2.x);
	C = p1.x*(p2.y - p3.y) + p2.x*(p3.y - p1.y) + p3.x*(p1.y - p2.y);
	D = -(p1.x*(p2.y*p3.z - p3.y*p2.z) + p2.x*(p3.y*p1.z - p1.y*p3.z) + p3.x*(p1.y*p2.z - p2.y*p1.z));
}

#endif