point3d.h

Particle filtering implementation and application to people tracking.

#ifndef _POINT3D_H_
#define _POINT3D_H_
#include <assert.h>
#include <math.h>
#include <iostream>


template <class T>
struct point3d
{
	inline point3d(){}
	inline point3d(T _x, T _y, T _z):x(_x),y(_y),z(_z){}
	T x, y, z;
};

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

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

template <class T>
inline point3d<T> operator * (const point3d<T>& p, const T& v)
{
	return point3d<T>(p.x*v, p.y*v, p.z*v);
}

template <class T>
inline point3d<T> operator * (const T& v, const point3d<T>& p)
{
	return point3d<T>(p.x*v, p.y*v, p.z*v);
}

template <class T>
inline point3d<T> operator / (const point3d<T>& p, const T& v)
{
	return point3d<T>(p.x/v, p.y/v, p.z/v);
}

template <class T>
inline point3d<T> operator / (const T& v, const point3d<T>& p)
{
	return point3d<T>(p.x/v, p.y/v, p.z/v);
}

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

template <class T>
inline bool operator < (const point3d<T>& p, const T& v)
{
	return p.x < v && p.y < v && p.z < v;
}

template <class T>
inline bool operator > (const point3d<T>& p, const T& v)
{
	return p.x > v && p.y > v && p.z > v;
}

template <class T, class A>
struct orientedpoint3d: public point3d<T>
{
	inline orientedpoint3d(){}
	//inline orientedpoint3d(const point3d<T>& p);
	inline orientedpoint3d(T x, T y, T z, A _roll, A _pitch, A _yaw):
		point3d<T>(x,y,z),
		roll(_roll),
		pitch(_pitch),
		yaw(_yaw)
	{
	}

	A roll; // rotation around X-axis
	A pitch; // rotation around Y-axis
	A yaw; // rotation around Z-axis
};

//template <class T, class A>
//orientedpoint3d<T,A>::orientedpoint3d(const point3d<T>& p)
//{
//  std::cerr << "operator f" << std::endl;
//  this->x=p.x;
//  this->y=p.y;
//  this->z=p.z;
//  this->roll=0.;
//  this->pitch=0.;
//  this->yaw=0.;
//}

template <class T, class A>
orientedpoint3d<T,A> rotateRoll(const orientedpoint3d<T,A>& p, const T& a)
{
	double c=cos(a), s=sin(a);
	return orientedpoint3d<T,A>(
							p.x*c-p.y*s,
							p.y*c+p.x*s,
							p.z,
							norm(a+p.roll),
							p.pitch,
							p.yaw);
}

template <class T, class A>
orientedpoint3d<T,A> rotatePitch(const orientedpoint3d<T,A>& p, const T& a)
{
	double c=cos(a), s=sin(a);
	return orientedpoint3d<T,A>(
							p.x*c+p.z*s,
							p.y,
							p.z*c-p.x*s,
							p.roll,
							norm(a+p.pitch),
							p.yaw);
}

template <class T, class A>
orientedpoint3d<T,A> rotateYaw(const orientedpoint3d<T,A>& p, const T& a)
{
	double c=cos(a), s=sin(a);
	return orientedpoint3d<T,A>(
							p.x,
							p.y*c-p.z*s,
							p.z*c+p.y*s,
							p.roll,
							p.pitch,
							norm(a+p.yaw));
}

template <class T, class A>
orientedpoint3d<T,A> operator+(const orientedpoint3d<T,A>& p1, const orientedpoint3d<T,A>& p2)
{
	return orientedpoint3d<T,A>(
			p1.x+p2.x, p1.y+p2.y, p1.z+p2.z,
			norm(p1.roll+p2.roll), norm(p1.pitch+p2.pitch), norm(p1.yaw+p2.yaw)
			);
}

//template <class T, class A>
//orientedpoint3d<T,A> operator - (const orientedpoint3d<T,A> & p1, const orientedpoint3d<T,A> & p2)
//{
//  return orientedpoint3d<T,A>(
//      p1.x-p2.x, p1.y-p2.y, p1.z-p2.z,
//      norm(p1.roll-p2.roll), norm(p1.pitch-p2.pitch), norm(p1.yaw-p2.yaw)
//      );
//}
//
//template <class T, class A>
//orientedpoint3d<T,A> operator * (const orientedpoint3d<T,A>& p, const T& v)
//{
//  return orientedpoint3d<T,A>(
//      p.x*v, p.y*v, p.z*v,
//      p.roll*v, p.pitch*v
//      );
//}
//
//template <class T, class A>
//orientedpoint3d<T,A> operator * (const T& v, const orientedpoint3d<T,A>& p){
//  return orientedpoint3d<T,A>(
//      p.x*v, p.y*v, p.z*v,
//      p.roll*v, p.pitch*v
//      );
//}

//template <class T, class A>
//orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){
//  orientedpoint<T,A> delta=p1-p2;
//  delta.roll=atan2(sin(delta.roll), cos(delta.roll));
//  double s=sin(p2.roll), c=cos(p2.roll);
//  return orientedpoint<T,A>(c*delta.x+s*delta.y, 
//                           -s*delta.x+c*delta.y, delta.roll);
//}

template <class T, class A>
orientedpoint3d<T,A> absoluteSum(const orientedpoint3d<T,A>& p1,const orientedpoint3d<T,A>& p2)
{
	double cR = cos(p1.roll),
				 sR = sin(p1.roll),
				 cP = cos(p1.pitch),
				 sP = sin(p1.pitch),
				 cY = cos(p1.yaw),
				 sY = sin(p1.yaw);
	return orientedpoint3d<T,A>(
			p1.x + p2.x * (   cY*cP ) + p2.y * ( - sY*cR + cY*sP*sR ) + p2.z * (   sY*sR + cY*sP*cR ) ,
			p1.y + p2.x * (   sY*cP ) + p2.y * (   cY*cR + sY*sP*sR ) + p2.z * ( - cY*sR + sY*sP*cR ) ,
			p1.z + p2.x * ( - sP )    + p2.y * (   cP*sR )            + p2.z * (   cP*cR ) ,
			norm(p1.roll  + p2.roll),
			norm(p1.pitch + p2.pitch),
			norm(p1.yaw   + p2.yaw)
			);
}

//template <class T, class A>
//point<T> absoluteSum(const orientedpoint<T,A>& p1,const point<T>& p2){
//  double s=sin(p1.roll), c=cos(p1.roll);
//  return point<T>(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point<T>) p1;
//}

//template <class T>
//struct pointcomparator{
//  bool operator ()(const point<T>& a, const point<T>& b) const {
//    return a.x<b.x || (a.x==b.x && a.y<b.y);
//  }	
//};

//template <class T>
//struct pointradialcomparator{
//  point<T> origin;
//  bool operator ()(const point<T>& a, const point<T>& b) const {
//    point<T> delta1=a-origin;
//    point<T> delta2=b-origin;
//    return (atan2(delta1.y,delta1.x)<atan2(delta2.y,delta2.x));
//  }	
//};

template <class T>
inline point3d<T> max(const point3d<T>& p1, const point3d<T>& p2, const point3d<T>& p3)
{
	point3d<T> p=p1;
	p.x=p.x>p2.x?p.x:p2.x;
	p.y=p.y>p2.y?p.y:p2.y;
	p.z=p.z>p2.z?p.z:p2.z;
	
	p.x=p.x>p3.x?p.x:p3.x;
	p.y=p.y>p3.y?p.y:p3.y;
	p.z=p.z>p3.z?p.z:p3.z;
	return p;
}

template <class T>
inline point3d<T> min(const point3d<T>& p1, const point3d<T>& p2, const point3d<T>& p3){
	point3d<T> p=p1;
	p.x=p.x<p2.x?p.x:p2.x;
	p.y=p.y<p2.y?p.y:p2.y;
	p.z=p.z<p2.z?p.z:p2.z;
	
	p.x=p.x<p3.x?p.x:p3.x;
	p.y=p.y<p3.y?p.y:p3.y;
	p.z=p.z<p3.z?p.z:p3.z;
	return p;
}

//template <class T, class F>
//inline point<T> interpolate(const point<T>& p1,  const F& t1, const point<T>& p2, const F& t2, const F& t3){
//  F gain=(t3-t1)/(t2-t1);
//  point<T> p=p1+(p2-p1)*gain;
//  return p;
//}

//template <class T, class A, class F>
//inline orientedpoint<T,A> 
//interpolate(const orientedpoint<T,A>& p1,  const F& t1, const orientedpoint<T,A>& p2, const F& t2, const F& t3){
//  F gain=(t3-t1)/(t2-t1);
//  orientedpoint<T,A> p;
//  p.x=p1.x+(p2.x-p1.x)*gain;
//  p.y=p1.y+(p2.y-p1.y)*gain;
//  double  s=sin(p1.roll)+sin(p2.roll)*gain,
//    c=cos(p1.roll)+cos(p2.roll)*gain;
//  p.roll=atan2(s,c);
//  return p;
//}

typedef point3d<int> IntPoint3d;
typedef point3d<double> Point3d;
typedef orientedpoint3d<double, double> OrientedPoint3d;

#endif