3dpcp/.svn/pristine/c4/c4f86eb39b78b5eeca27bfdde5060c25e146abe7.svn-base

339 lines
8.6 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
#ifndef __SHAPE_H__
#define __SHAPE_H__
#include <vector>
using std::vector;
#include "slam6d/globals.icc"
#include "newmat/newmatio.h"
#include "newmat/newmatap.h"
using namespace NEWMAT;
/**
* The Shape class is for efficient collision detection in the Octree.
*/
template <class T=double>
class CollisionShape {
public:
/**
* This is the main function for speeding up the search for points on the shape.
*
* @param cx, cy, cz, size The center and size of the octrees bucket. Buckets are axis aligned bounding cubes
*
* @return returns wether this shape is within the cube (true even if only partially)
* if unsure err on the side of caution, i.e. return true
*/
virtual bool isInCube(T cx, T cy, T cz, T size) = 0;
virtual void refine(vector<T *> *points) = 0;
virtual bool containsPoint(T* p) = 0;
virtual bool hypothesize(vector<T *> &points) = 0;
virtual unsigned char getNrPoints() = 0;
virtual CollisionShape<T> *copy() = 0;
virtual CollisionShape<T>& operator=(const CollisionShape<T> &other) {return *this;};
// virtual bool valid() = 0;
};
template <class T=double>
class CollisionPlane : public CollisionShape<T> {
public:
// CollisionPlane (T *_plane, T _maxDist) {
CollisionPlane (T _maxDist) {
maxDist = _maxDist;
// TODO make nicer
/* nx = _plane[0];
ny = _plane[1];
nz = _plane[2];
d = _plane[3];*/
}
CollisionPlane(T _maxDist, T x, T y, T z, T _d) {
maxDist = _maxDist;
nx = x;
ny = y;
nz = z;
d = _d;
}
virtual bool isInCube(T x, T y, T z, T size) {
T xm, xp, ym, yp, zm, zp;
T Fxm, Fxp, Fym, Fyp, Fzm, Fzp;
xm = x - size;
xp = x + size;
ym = y - size;
yp = y + size;
zm = z - size;
zp = z + size;
Fxm = nx * xm;
Fym = ny * ym;
Fzm = nz * zm;
bool positive = (Fxm + Fym + Fzm + d > 0);
Fxp = nx * xp;
if( (Fxp + Fym + Fzm + d < 0) == positive )
return true;
Fyp = ny * yp;
if( (Fxm + Fyp + Fzm + d < 0) == positive )
return true;
if( (Fxp + Fyp + Fzm + d < 0) == positive )
return true;
Fzp = nz * zp;
if( (Fxm + Fym + Fzp + d < 0) == positive )
return true;
if( (Fxp + Fym + Fzp + d < 0) == positive )
return true;
if( (Fxm + Fyp + Fzp + d < 0) == positive )
return true;
if( (Fxp + Fyp + Fzp + d < 0) == positive )
return true;
return false;
}
virtual bool containsPoint(T* p) {
return fabs(planeDist(p, nx, ny, nz, d)) < maxDist;
}
virtual void refine(vector<T *> *points) {
cout << nx << " " << ny << " " << nz << " " << d << endl;
T plane[4] = {0,0,0,0};
T centroid[3];
fitPlane((*points), plane, centroid);
nx = plane[0];
ny = plane[1];
nz = plane[2];
d = plane[3];
cout << nx << " " << ny << " " << nz << " " << d << endl;
}
virtual bool hypothesize(vector<T *> &points) {
if (points.size() < getNrPoints()) return false;
T a[3], b[3], f[3], plane[4];
for (int j = 0; j < 3;j++) { // compute plane
a[j] = points[0][j] - points[1][j];
b[j] = points[0][j] - points[2][j];
f[j] = points[0][j] + points[1][j] + points[2][j];
f[j] /= 3.0;
}
Cross(a,b, plane);
if (fabs(Len2(plane)) < 0.0001 ) {
// points are collinear
return false;
}
Normalize3(plane);
plane[3] = -1.0 * planeDist(f, plane[0], plane[1], plane[2], 0.0); // compute distance from origin
if (plane[3] < 0.0) { // flip normal if necessary
for (int j = 0; j < 4;j++) {
plane[j] = -plane[j];
}
}
nx = plane[0];
ny = plane[1];
nz = plane[2];
d = plane[3];
return true;
}
virtual unsigned char getNrPoints() {
return 3;
}
virtual CollisionShape<T>* copy() {
return new CollisionPlane<T>(maxDist, nx, ny, nz, d);
}
virtual CollisionPlane<T>& operator=(const CollisionShape<T> &_other) {
CollisionPlane<T> &other = (CollisionPlane<T> &)_other;
if (this != &other) {
this->maxDist = other.maxDist;
this->nx = other.nx ;
this->ny = other.ny ;
this->nz = other.nz ;
this->d = other.d ;
}
return *this;
}
void getPlane(double &x, double &y, double &z, double &_d) {
x = nx;
y = ny;
z = nz;
_d = d;
}
protected:
T maxDist;
T nx, ny, nz, d; // plane equation TODO make nicer
};
template <class T=double>
class LightBulbPlane : public CollisionPlane<T> {
public:
LightBulbPlane (T _maxDist, T _maxSize) : CollisionPlane<T>(_maxDist) {
maxSize = _maxSize;
c[0] = 0;
c[1] = 0;
c[2] = 0;
}
LightBulbPlane (T _maxDist, T _maxSize, T x, T y, T z, T _d, T* center) : CollisionPlane<T>(_maxDist) {
maxSize = _maxSize;
CollisionPlane<T>::nx = x;
CollisionPlane<T>::ny = y;
CollisionPlane<T>::nz = z;
CollisionPlane<T>::d = _d;
for(int i = 0; i < 3; i++) {
c[i] = center[i];
}
}
void refine(vector<T *> *points) {
cout << "LightBulbPlane" << endl;
cout << this->nx << " " << this->ny << " " << this->nz << " " << this->d << endl;
T plane[4];
fitPlane((*points), plane, c);
this->nx = plane[0];
this->ny = plane[1];
this->nz = plane[2];
this->d = plane[3];
cout << this->nx << " " << this->ny << " " << this->nz << " " << this->d << endl;
}
bool isInCube(T cx, T cy, T cz, T size) {
double radius = sqrt(3*size*size);
T c_dist = (cx - c[0])*(cx - c[0]) + (cy - c[1])*(cy - c[1]) + (cz - c[2])*(cz - c[2]);
return c_dist <= ((radius + maxSize)*(radius + maxSize));
}
virtual bool containsPoint(T* p) {
if(fabs(p[0]*CollisionPlane<T>::nx + p[1]*CollisionPlane<T>::ny + p[2]*CollisionPlane<T>::nz + CollisionPlane<T>::d) < CollisionPlane<T>::maxDist) {
return (Dist2(p, c) < (maxSize*maxSize));
}
return false;
}
virtual bool hypothesize(vector<T *> &points) {
if(!CollisionPlane<T>::hypothesize(points)) return false;
double maxSize2 = maxSize*maxSize;
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++) {
if(Dist2(points[i], points[j]) > maxSize2) return false;
}
}
for (int j = 0; j < 3;j++) { // compute plane
c[j] = points[0][j] + points[1][j] + points[2][j];
c[j] /= 3.0;
}
return true;
}
virtual CollisionShape<T> * copy() {
return new LightBulbPlane<T>(CollisionPlane<T>::maxDist, maxSize, CollisionPlane<T>::nx, CollisionPlane<T>::ny, CollisionPlane<T>::nz, CollisionPlane<T>::d, c);
}
virtual LightBulbPlane<T>& operator=(const CollisionShape<T> &_other) {
LightBulbPlane<T> &other = (LightBulbPlane<T> &)_other;
if (this != &other) {
this->maxDist = other.maxDist;
this->nx = other.nx ;
this->ny = other.ny ;
this->nz = other.nz ;
this->d = other.d ;
this->maxSize = other.maxSize;
for(int i = 0; i < 3; i++) {
this->c[i] = other.c[i];
}
}
return *this;
}
void getCenter(T &x, T &y, T &z) {
x = this->c[0];
y = this->c[1];
z = this->c[2];
}
/*
bool validate(vector<T *> pts) {
// create array which will not be used
bool plane[125][125];
for(int i = 0; i < 125; i++) {
for(int j = 0; j < 125; j++) {
plane[j][i] = false;
}
}
double t[3];
double alignxf[16];
double aa[4];
aa[0] = -1.0 * acos(this.ny);
aa[1] = this.nz / sqrt( this.nz*this.nz + this.nx*this.nx );
aa[2] = 0;
aa[3] = -this.nx / sqrt( this.nx*this.nz + this.nx*this.nx );
AAToMatrix(aa, t, alignxf);
// compute 2d projection of the points, and scale reflectivity
for (unsigned int i = 0; i < points.size(); i++) {
double *p = points[i];
npoints[i] = new double[4];
wykobi::point2d<double> point;
double x, y;
x = -(p[0] * alignxf[0] + p[1] * alignxf[4] + p[2] * alignxf[8]);
y = p[0] * alignxf[2] + p[1] * alignxf[6] + p[2] * alignxf[10];
if (x > maxx) maxx = x;
if (x < minx) minx = y;
if (y > maxz) maxz = y;
point = wykobi::make_point(x, y);
point_list.push_back(point);
}
vector< wykobi::point2d<double> > point_list;
wykobi::polygon<double,2> convex_hull;
wykobi::algorithm::convex_hull_jarvis_march< wykobi::point2d<double> >(point_list.begin(),point_list.end(),std::back_inserter(convex_hull));
return true;
}
*/
protected:
T maxSize;
T c[3];
};
#endif