collisiondetectors/CD_rangesensor/octree.cpp

Go to the documentation of this file.

#pragma warning ( disable : 4786 )

#include "octree.h"

// Octree Contruction Components

//Most of the code in this module written by Yong Yu

#define GET_REP(a, point, Side)\
  \
{\
  switch(Side) \
    { \
    case 0: \
    rep = point.z - a->apex.z; \
    break; \
    case 1: \
    rep = -point.z + a->apex.z; \
    break; \
   case 2: \
   rep = point.y - a->apex.y; \
   break; \
   case 3:   \
   rep = -point.y + a->apex.y; \
   break; \
   case 4: \
   rep = point.x - a->apex.x; \
   break; \
   case 5: \
   rep = -point.x + a->apex.x; \
   break; \
   default: \
   exit(51); \
   break; \
   } \
};

int last_face = -1;


OctreeNode * assign_white_node()
{
  OctreeNode *p;
  
  p = (OctreeNode *) malloc(sizeof(OctreeNode));
  
  if(p == NULL) 
  {
    exit (53);
    //printf("space max reached\n");
  }
  
  p->son = NULL;
  p->color = WHITE;
  //p->info_value = 0.0;
  return(p);
}


OctreeNode * assign_black_node()
{
  OctreeNode *p;
  
  p = (OctreeNode *) malloc(sizeof(OctreeNode));
  
  
  if(p == NULL) 
  {
    exit (53);
    //printf("space max reached\n");
  }
  
  p->son = NULL;
  p->color = BLACK;
  //p->info_value = 0.0;
  return(p);
}


OctreeNode * assign_gray_node(struct free_space *a, 
                               struct cubic *c, int level,int maxlevel)
{
  //int i;
  struct cubic sub_c;
  int x_mid, y_mid, z_mid;
  OctreeNode *p;
  int next_level;
  next_level = level +1;
  p = (OctreeNode *) malloc(sizeof(OctreeNode));
  
  p->son = (struct child_pointer*) malloc(sizeof(struct child_pointer));
  if(p == NULL) 
  {
    exit (53);
    //printf("space max reached\n");
  }
  p->color= GRAY;
  x_mid = (c->x1 + c->x0)/2;
  y_mid = (c->y1 + c->y0)/2;
  z_mid = (c->z1 + c->z0)/2;
  sub_c.x0= c->x0;
  sub_c.y0= c->y0;
  sub_c.z0= c->z0;
  sub_c.x1= x_mid;
  sub_c.y1= y_mid;
  sub_c.z1= z_mid;
  p->son->pointer[0] = construct_octree(a, &sub_c, next_level, maxlevel);
  p->son->pointer[0]->father= p;
  sub_c.x0= c->x0;
  sub_c.y0= c->y0;
  sub_c.z0= z_mid;
  sub_c.x1= x_mid;
  sub_c.y1= y_mid;
  sub_c.z1= c->z1;
  p->son->pointer[1] = construct_octree(a, &sub_c, next_level, maxlevel);
  p->son->pointer[1]->father= p;
  sub_c.x0= c->x0;
  sub_c.y0= y_mid;
  sub_c.z0= c->z0;
  sub_c.x1= x_mid;
  sub_c.y1= c->y1;
  sub_c.z1= z_mid;
  p->son->pointer[2] = construct_octree(a, &sub_c, next_level, maxlevel);
  p->son->pointer[2]->father= p;
  sub_c.x0= c->x0;
  sub_c.y0= y_mid;
  sub_c.z0= z_mid;
  sub_c.x1= x_mid;
  sub_c.y1= c->y1;
  sub_c.z1= c->z1;
  p->son->pointer[3] = construct_octree(a, &sub_c, next_level, maxlevel);
  p->son->pointer[3]->father= p;
  sub_c.x0= x_mid;
  sub_c.y0= c->y0;
  sub_c.z0= c->z0;
  sub_c.x1= c->x1;
  sub_c.y1= y_mid;
  sub_c.z1= z_mid;
  p->son->pointer[4] = construct_octree(a, &sub_c, next_level, maxlevel);
  p->son->pointer[4]->father= p;
  sub_c.x0= x_mid;
  sub_c.y0= c->y0;
  sub_c.z0= z_mid;
  sub_c.x1= c->x1;
  sub_c.y1= y_mid;
  sub_c.z1= c->z1;
  p->son->pointer[5] = construct_octree(a, &sub_c, next_level, maxlevel);
  p->son->pointer[5]->father= p;
  sub_c.x0= x_mid;
  sub_c.y0= y_mid;
  sub_c.z0= c->z0;
  sub_c.x1= c->x1;
  sub_c.y1= c->y1;
  sub_c.z1= z_mid;
  p->son->pointer[6] = construct_octree(a, &sub_c, next_level, maxlevel);
  p->son->pointer[6]->father= p;
  sub_c.x0= x_mid;
  sub_c.y0= y_mid;
  sub_c.z0= z_mid;
  sub_c.x1= c->x1;
  sub_c.y1= c->y1;
  sub_c.z1= c->z1;
  p->son->pointer[7] = construct_octree(a, &sub_c, next_level, maxlevel);
  p->son->pointer[7]->father= p;
  return(p);
}

void mapping(struct free_space *a, 
             struct vector_d *point, 
             int proj_point[3])
             
{//proj_point[0] face number, the other two are coordinates
  double tempi_x, tempi_y, tempi_z; 
  double t;
  double d_x, d_y, d_z;
  
  d_x = point->x - a->apex.x;
  d_y = point->y - a->apex.y;
  d_z = point->z - a->apex.z;
  if((d_x == 0) && ( d_y == 0) && (d_z ==0))  
  {
    d_x = TEMP_EPSILON;
    d_y = TEMP_EPSILON;
    d_z = TEMP_EPSILON;
  } 
  switch(last_face)
  {  // try the last face, which probably is the result of this time 
  case 0:
    t = (LIMITP - a->apex.z)/d_z;
    if(t > 0)
    {
      tempi_x = t*d_x + a->apex.x;
      if((tempi_x > (-DBL_LIMIT)) && (tempi_x < (DBL_LIMITP)))
      {
        tempi_y = t*d_y + a->apex.y;
        if((tempi_y > (-DBL_LIMIT)) && (tempi_y < (DBL_LIMITP)))
        {
          last_face = proj_point[0] = 0; 
          proj_point[1] = (int)tempi_x; 
          proj_point[2] = (int) tempi_y;
          return;
        }
      }
    }
    break;
  case 1:
    // face 1, bottom 
    t = (-LIMIT - a->apex.z)/d_z;
    if(t > 0)
    {
      tempi_x = t*d_x + a->apex.x;
      if((tempi_x > (-DBL_LIMIT)) && (tempi_x < (DBL_LIMITP)))
      {
        tempi_y = t*d_y + a->apex.y;
        if((tempi_y > (-DBL_LIMIT)) && (tempi_y < (DBL_LIMITP)))
        {
          last_face = proj_point[0] = 1; proj_point[1] = (int)tempi_x; 
          proj_point[2] = (int)tempi_y;
          return;
        }
      }
    }
    break;
  case 2:
    // face 2, front 
    t = (LIMITP - a->apex.y)/d_y;
    if(t > 0)
    {
      tempi_x = t*d_x + a->apex.x; 
      if((tempi_x > (-DBL_LIMIT)) && (tempi_x < (DBL_LIMITP)))
      {
        tempi_z = t*d_z + a->apex.z;
        if((tempi_z > (-DBL_LIMIT)) && (tempi_z < (DBL_LIMITP)))
        {
          last_face = proj_point[0] = 2; proj_point[1] = (int)tempi_x; 
          proj_point[2] = (int)tempi_z;
          return;
        }
      }
    }
    break;
  case 3:
    // face 3, back  
    t = (-LIMIT - a->apex.y)/d_y;
    if(t > 0)
    {
      tempi_x = t*d_x + a->apex.x;
      if((tempi_x > (-DBL_LIMIT)) && (tempi_x < (DBL_LIMITP)))
      {
        tempi_z = t*d_z + a->apex.z;
        if((tempi_z > (-DBL_LIMIT)) && (tempi_z < (DBL_LIMITP)))
        {
          last_face = proj_point[0] = 3; proj_point[1] = (int)tempi_x; 
          proj_point[2] = (int)tempi_z;
          return;
        }
      }
    }
    break;
  case 4:
    // face 4, right 
    t = (LIMITP - a->apex.x)/d_x;
    if(t > 0)
    {
      tempi_z = t*d_z + a->apex.z; 
      if((tempi_z > (-DBL_LIMIT)) && (tempi_z < (DBL_LIMITP)))
      {
        tempi_y = t*d_y + a->apex.y;
        if((tempi_y > (-DBL_LIMIT)) && (tempi_y < (DBL_LIMITP)))
        {
          last_face = proj_point[0] = 4; proj_point[1] = (int)tempi_y; 
          proj_point[2] = (int)tempi_z;
          return;
        }
      }
    }
    break;
  case 5:
    // face 5, left 
    t = (-LIMIT - a->apex.x)/d_x;
    if(t > 0)
    {
      tempi_z = t*d_z + a->apex.z;
      if((tempi_z > (-DBL_LIMIT)) && (tempi_z < (DBL_LIMITP)))
      {
        tempi_y = t*d_y + a->apex.y;
        if((tempi_y > (-DBL_LIMIT)) && (tempi_y < (DBL_LIMITP)))
        {
          last_face = proj_point[0] = 5; proj_point[1] = (int)tempi_y;
          proj_point[2] = (int)tempi_z;
          return;
        }
      }
    }
    break;
  default:
    break;
}

// face 0, top 
t = (LIMITP - a->apex.z)/d_z;
if(t > 0)
{
  tempi_x = t*d_x + a->apex.x;
  if((tempi_x > (-DBL_LIMIT)) && (tempi_x < (DBL_LIMITP)))
  {
    tempi_y = t*d_y + a->apex.y;
    if((tempi_y > (-DBL_LIMIT)) && (tempi_y < (DBL_LIMITP)))
    {
      last_face = proj_point[0] = 0; proj_point[1] = (int)tempi_x; 
      proj_point[2] = (int) tempi_y;
      return;
    }
  }
}
// face 1, bottom 
t = (-LIMIT - a->apex.z)/d_z;
if(t > 0)
{
  tempi_x = t*d_x + a->apex.x;
  if((tempi_x > (-DBL_LIMIT)) && (tempi_x < (DBL_LIMITP)))
  {
    tempi_y = t*d_y + a->apex.y;
    if((tempi_y > (-DBL_LIMIT)) && (tempi_y < (DBL_LIMITP)))
    {
      last_face = proj_point[0] = 1; proj_point[1] = (int)tempi_x; 
      proj_point[2] = (int)tempi_y;
      return;
    }
  }
}
// face 2, front 
t = (LIMITP - a->apex.y)/d_y;
if(t > 0)
{
  tempi_x = t*d_x + a->apex.x; 
  if((tempi_x > (-DBL_LIMIT)) && (tempi_x < (DBL_LIMITP)))
  {
    tempi_z = t*d_z + a->apex.z;
    if((tempi_z > (-DBL_LIMIT)) && (tempi_z < (DBL_LIMITP)))
    {
      last_face = proj_point[0] = 2; proj_point[1] = (int)tempi_x; 
      proj_point[2] = (int)tempi_z;
      return;
    }
  }
}
// face 3, back  
t = (-LIMIT - a->apex.y)/d_y;
if(t > 0)
{
  tempi_x = t*d_x + a->apex.x;
  if((tempi_x > (-DBL_LIMIT)) && (tempi_x < (DBL_LIMITP)))
  {
    tempi_z = t*d_z + a->apex.z;
    if((tempi_z > (-DBL_LIMIT)) && (tempi_z < (DBL_LIMITP)))
    {
      last_face = proj_point[0] = 3; proj_point[1] = (int)tempi_x; 
      proj_point[2] = (int)tempi_z;
      return;
    }
  }
}
// face 4, right 
t = (LIMITP - a->apex.x)/d_x;
if(t > 0)
{
  tempi_z = t*d_z + a->apex.z; 
  if((tempi_z > (-DBL_LIMIT)) && (tempi_z < (DBL_LIMITP)))
  {
    tempi_y = t*d_y + a->apex.y;
    if((tempi_y > (-DBL_LIMIT)) && (tempi_y < (DBL_LIMITP)))
    {
      last_face = proj_point[0] = 4; proj_point[1] = (int)tempi_y; 
      proj_point[2] = (int)tempi_z;
      return;
    }
  }
}
// face 5, left 
t = (-LIMIT - a->apex.x)/d_x;
if(t > 0)
{
  tempi_z = t*d_z + a->apex.z;
  if((tempi_z > (-DBL_LIMIT)) && (tempi_z < (DBL_LIMITP)))
  {
    tempi_y = t*d_y + a->apex.y;
    if((tempi_y > (-DBL_LIMIT)) && (tempi_y < (DBL_LIMITP)))
    {
      last_face = proj_point[0] = 5; proj_point[1] = (int)tempi_y;
      proj_point[2] = (int)tempi_z;
      return;
    }
  }
}

//if ((proj_point[0] < 0) || (proj_point[0] > 5))
//{
//  printf ("\ndx %f dy %f dz %f",d_x,d_y,d_z);
//  printf ("\napex.x %f %f %f",a->apex.x,a->apex.y,a->apex.z);
//  printf ("\n proj_point %d %d %d",proj_point[0],proj_point[1],proj_point[2]);
//  
//}

//printf(".");
if( (point->x > LIMITP ) || (point->x < -LIMIT) ||
   (point->y > LIMITP ) || (point->y < -LIMIT) ||
   (point->z > LIMITP ) || (point->z < -LIMIT) )
   exit (52);
//printf("out of limit\n");
}

OctreeNode *construct_octree(struct free_space *a, 
                              struct cubic *scope, 
                              int level, int maxlevel)
{
  int i, j;
  int x_start, y_start, z_start, x_end, y_end, z_end;
  int status;
  int p_p[3];
  struct vector_d point;
  double rep;
  double side1, side2;
  
  x_start = scope->x0 - LIMIT;
  x_end   = scope->x1 - LIMIT;
  
  y_start = scope->y0 - LIMIT;
  y_end   = scope->y1 - LIMIT;
  
  z_start = scope->z0 - LIMIT;
  z_end   = scope->z1 - LIMIT;
  
  if ( (a->bound_start.x > scope->x1) || 
    (a->bound_start.y > scope->y1) ||
    (a->bound_start.z > scope->z1) ||
    (a->bound_end.x < scope->x0) || 
    (a->bound_end.y < scope->y0) ||
    (a->bound_end.z < scope->z0) )
  {
    return(assign_black_node()); 
  }
  point.x = x_end;
  point.y = y_end;
  point.z = z_end;
  
  mapping(a, &point, p_p);
  
  GET_REP(a, point, p_p[0])
    if(a->scaled_distance[p_p[0]][p_p[1]+128][p_p[2]+128] >= rep) 
    {
      //printf ("\nrep:%d,%d,%d",p_p[0],p_p[1]+128,p_p[2]+128);
      status = INSIDE; 
      if(level >= maxlevel)
      {
        return(assign_white_node());
      }
    }
    else
    {
      status = OUTSIDE;
      if(level >= maxlevel) 
      {
        return(assign_black_node());
      }
    }
    side1 = z_start;
    point.z = side1;
    if( ( (status == INSIDE ) && ( a->apex.z - side1 > 0) ) ||
      ( (status == OUTSIDE) && ( a->apex.z - side1 < 0) ) )
    {
      // bottom 
      for(i = x_start; i < x_end; i++)
      {
        point.x = i;
        for(j = y_start; j < y_end; j++)
        {
          point.y = j;
          mapping(a, &point, p_p);
          GET_REP(a, point, p_p[0]);
          if(status^(a->scaled_distance[p_p[0]][p_p[1]+128][p_p[2]+128]<= rep))
          { // gray node 
            return(assign_gray_node(a, scope, level, maxlevel));
          }
        }
      }
    }
    side2 = z_end;
    point.z = z_end;
    if( ( (status == OUTSIDE) && ( a->apex.z - side2 >0) ) ||
      ( (status == INSIDE) && ( a->apex.z - side2 < 0) ) )
    {
      //top 
      for(i = x_start; i < x_end; i++)
      {
        point.x = i;
        for(j = y_start; j < y_end; j++)
        {
          point.y = j;
          mapping(a, &point, p_p);
          GET_REP(a, point, p_p[0]);
          if(status ^(a->scaled_distance[p_p[0]][p_p[1]+128][p_p[2]+128] <= rep))
          {  // gray node 
            return(assign_gray_node(a, scope, level, maxlevel));
          }
        }
      }
    }
    point.y = side1 = y_start;
    if( ( (status == INSIDE) && ( a->apex.y - side1 > 0) ) ||
      ( (status == OUTSIDE) && ( a->apex.y - side1 <0) ) )
    {
      // front
      for(i = x_start; i < x_end; i++)
      {
        point.x = i;
        for(j = z_start; j < z_end; j++)
        {
          point.z = j;
          
          mapping(a, &point, p_p);
          
          GET_REP(a, point, p_p[0]);
          if(status ^ (a->scaled_distance[p_p[0]][p_p[1]+128][p_p[2]+128]<=rep))
          {
            return(assign_gray_node(a, scope, level, maxlevel));
          }
        }
      }
    }
    point.y = side2 = y_end;
    if( ( (status == OUTSIDE) && ( a->apex.y - side2 > 0) ) ||
      ( (status == INSIDE) && ( a->apex.y - side2 <0) ) )
    {
      //back 
      for(i = x_start; i < x_end; i++)
      {
        point.x = i;
        for(j = z_start; j < z_end; j++)
        {
          point.z = j;
          
          mapping(a, &point, p_p);
          GET_REP(a, point, p_p[0]);
          if(status^(a->scaled_distance[p_p[0]][p_p[1]+128][p_p[2]+128]<=rep))
          {
            return(assign_gray_node(a, scope, level, maxlevel));
          }
        }
      }
    }
    point.x = side1 = x_start;
    if( ( (status == INSIDE) && ( a->apex.x - side1 > 0) ) ||
      ( (status == OUTSIDE) && ( a->apex.x - side1 <0) ) )
    {
      // left 
      for(i = y_start; i < y_end; i++)
      {
        point.y = i;
        for(j = z_start; j < z_end; j++)
        {
          point.z = j;
          
          mapping(a, &point, p_p);
          GET_REP(a, point, p_p[0]);
          if(status^(a->scaled_distance[p_p[0]][p_p[1]+128][p_p[2]+128]<=rep))
          {
            return(assign_gray_node(a, scope, level, maxlevel));
          }
        }
      }
    }
    point.x = side2 = x_end;
    
    if( ( (status == OUTSIDE) && ( a->apex.x - side2 > 0) ) ||
      ( (status == INSIDE) && ( a->apex.x - side2 <0) ) )
    {
      //right 
      for(i = y_start; i < y_end; i++)
      {
        point.y = i;
        for(j = z_start; j < z_end; j++)
        {
          point.z = j;
          mapping(a, &point, p_p);
          GET_REP(a, point, p_p[0]);
          if(status^(a->scaled_distance[p_p[0]][p_p[1]+128][p_p[2]+128]<=rep))
          {
            return(assign_gray_node(a, scope, level, maxlevel));
          }
        }
      }
    }
    if(status == INSIDE) 
    {
      return(assign_white_node());
    }
    else
    {
      if( ((a->apex.x + LIMIT)<=scope->x1) && ((a->apex.x + LIMIT)>=scope->x0) &&
        ((a->apex.y + LIMIT)<=scope->y1) && ((a->apex.y + LIMIT)>=scope->y0) &&
        ((a->apex.z + LIMIT)<=scope->z1) && ((a->apex.z + LIMIT)>=scope->z0))
      {
        return(assign_gray_node(a,  scope, level, maxlevel));
      }
      else
      {
        return(assign_black_node());
      }
    }   
}


inline void scale_distance_assign(int proj_p[3], int i, int j, struct free_space *a, Range_Sensor* camera)
{
  switch (proj_p[0])
  {
  case 0:
    a->scaled_distance[proj_p[0]][proj_p[1]+128][proj_p[2]+128] = (short)
      (camera->ray_coord_map[i][j][2] - a->apex.z);
    break;
  case 1:
    a->scaled_distance[proj_p[0]][proj_p[1]+128][proj_p[2]+128] = (short)
      -(camera->ray_coord_map[i][j][2] - a->apex.z);
    break;
  case 2:
    a->scaled_distance[proj_p[0]][proj_p[1]+128][proj_p[2]+128] = (short)
      (camera->ray_coord_map[i][j][1] - a->apex.y);
    break;
  case 3:
    a->scaled_distance[proj_p[0]][proj_p[1]+128][proj_p[2]+128] = (short)
      -(camera->ray_coord_map[i][j][1] - a->apex.y);
    break;
  case 4:
    a->scaled_distance[proj_p[0]][proj_p[1]+128][proj_p[2]+128] = (short)
      (camera->ray_coord_map[i][j][0] - a->apex.x);
    break;
  case 5:
    a->scaled_distance[proj_p[0]][proj_p[1]+128][proj_p[2]+128] = (short)
      -(camera->ray_coord_map[i][j][0] - a->apex.x);
    break;
  default: 
    exit(3);
    //printf("wrong.\n");
  }
}


void calculate_scaled_distance(struct free_space *a, Range_Sensor* camera)
{
  int i, j;
  int proj_point[3];
  struct vector_d point;
  struct vector_d max, min;
  
  //zero off the scaled_distance array
  memset (&a->scaled_distance[0][0][0],0,sizeof(a->scaled_distance));
  
  max.x = min.x = a->apex.x;
  max.y = min.y = a->apex.y;
  max.z = min.z = a->apex.z;
  
  for(i = 0; i < 256; i++)
  {
    for(j = 0; j < 256; j++)
    {
      point.x = camera->ray_coord_map[i][j][0];
      point.y = camera->ray_coord_map[i][j][1];
      point.z = camera->ray_coord_map[i][j][2];
      
      if( (point.x == 0) && (point.y == 0) && (point.z == 0)) 
        continue;
      if(point.x > max.x) max.x = point.x;
      if(point.y > max.y) max.y = point.y;
      if(point.z > max.z) max.z = point.z;
      if(point.x < min.x) min.x = point.x;
      if(point.y < min.y) min.y = point.y;
      if(point.z < min.z) min.z = point.z;
      
      mapping(a, &point, proj_point);
      scale_distance_assign(proj_point, i, j, a, camera);
      //      printf("\n%d %d (%d %d %d) (%f %f %f)\n", i, j, proj_point[0], proj_point[1], proj_point[2],
      //        point.x, point.y, point.z);
    }
  }
  a->bound_start.x = min.x + LIMIT;
  a->bound_start.y = min.y + LIMIT;
  a->bound_start.z = min.z + LIMIT;
  a->bound_end.x = max.x + LIMIT;
  a->bound_end.y = max.y + LIMIT;
  a->bound_end.z = max.z + LIMIT;
}

---