collisiondetectors/CD_rangesensor/octree.h

Go to the documentation of this file.

#if !defined(octree_h__INCLUDED_)
#define octree_h__INCLUDED_

#include "geometry\geo_rangesensor\Range_Sensor.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include <set>
#include <algorithm>
#include <iterator>
#include <functional>

//using namespace std ;
//#define SPAN_ANGLE 30
//#define SIZE_OF_CONE 0.1
#define TRUE 1
#define FALSE 0
#define WHITE 0
#define BLACK 3
#define GRAY 1

//resolution notes:
//worldsize = 256/(2^7) = 2 units minimum l,w,h; # levels = 7+1 = 8
#define LIMIT 128
#define LIMITP 127
#define TEMP_EPSILON 0.00000001
#define DBL_LIMIT (128+TEMP_EPSILON)
#define DBL_LIMITP (127+TEMP_EPSILON)


#define INSIDE 0
#define OUTSIDE 1

#define MAXLINELENGTH 444
#define MAXBUFFSIZE (64*64)
#define NUMELEMENTS (256*256)


class PointClass
{
public:
  char p[3];
};

#define PointsList std::set<PointClass>;

struct vector_d {
        double x, y ,z;
};

struct vector_i {
        int x, y, z;
};


struct free_space {
        struct vector_d apex; // apex of the V-cone, where the sensor is
  //struct vector_d heading; // sensor direction
  //struct vector_d position[256][256]; // the data from your sensor
        short scaled_distance[6][256][256]; // internal use, don't fill
        struct vector_d bound_start, bound_end; // internal use, don't fill
};

struct OctreeNode;

struct child_pointer
{
        OctreeNode* pointer[8];
};

struct OctreeNode 
{
        struct child_pointer *son;
        OctreeNode* father;
        char color;
        //double info_value;
};

struct cubic {
        int x0, y0, z0;
        int x1, y1, z1;
};

struct cubic_d {
        double x0, y0, z0;
        double x1, y1, z1;
};
// Octree Contruction Components


void mapping(struct free_space *a, 
             vector_d *point, 
             int proj_point[3]);
OctreeNode *construct_octree(struct free_space *a, 
                                                 struct cubic *scope, 
                                                 int level, int maxlevel);
OctreeNode * assign_white_node();
OctreeNode * assign_black_node();
OctreeNode * assign_gray_node(struct free_space *a, 
                                                 struct cubic *c, int level,int maxlevel);
inline void scale_distance_assign(int [3], int, int, 
             struct free_space *, Range_Sensor* jc);
void calculate_scaled_distance(struct free_space *a, Range_Sensor* jc);


#endif

---