collisiondetectors/CD_rangesensor/logical.cpp

Go to the documentation of this file.

#pragma warning ( disable : 4786 )

#include "logical.h"

//  Octree Logic Components

inline int white_node( OctreeNode *node)
{
  return(node -> color == WHITE);
}

inline int black_node( OctreeNode *node) 
{ 
  return(node -> color == BLACK);
} 

void delete_node( OctreeNode *node)
{
  int i;
  OctreeNode *father;
  if(node == NULL ) return;
  father = node->father;
  if(father != NULL)
  {
    for(i =0; i< 8; i++)
    {
      if(father->son->pointer[i] == node) 
      {
        father->son->pointer[i] = NULL;
        break;
      }
    } 
  }
  
  if(node->son==NULL) 
  {
    free(node);
  }
  else
  {
    for(i =0; i< 8; i++) 
    {
      if(node->son != NULL)
        delete_node(node->son->pointer[i]);
    }
    free(node->son);
    //node->son= NULL;
    free(node);
  }
  if(father!= NULL)
    if( (father->son->pointer[0]==NULL) && (father->son->pointer[1]==NULL) &&
      (father->son->pointer[2]==NULL) && (father->son->pointer[3]==NULL) &&
      (father->son->pointer[4]==NULL) && (father->son->pointer[5]==NULL) &&
      (father->son->pointer[6]==NULL) && (father->son->pointer[7]==NULL) )
    {
      free(father->son);
      father->son = NULL;
    }
}

static char input_bitbucket_place = 0;
static char input_bitbucket_ch = (char)0xAA;

static char output_bitbucket_place = 0;
static char output_bitbucket_ch = 0;

void reset_bitbuckets ()
{
  input_bitbucket_place = 0;
  input_bitbucket_ch = (char)0xAA;
  
  output_bitbucket_place = 0;
  output_bitbucket_ch = 0;
}

void input_bitbucket (char invalue, bool flush, FILE *fp)
//Saves to disk the invalue of 0x00,0x01,0x03 in 2 byte chunks
//Rules: input (invalue = 0x02, flush = true) when finished with the input
{
  
  //eliminate everything above the 2 lsb of input data
  invalue = (invalue & 0x03); 
  
  //format is [4][3][2][1] in the 8 bit byte.
  //if there is no entry then the space will be occupied by 0x02.
  if (input_bitbucket_place == 0)
    input_bitbucket_ch = (input_bitbucket_ch & 0xFC) | invalue;
  else if (input_bitbucket_place == 2)
    input_bitbucket_ch = (input_bitbucket_ch & 0xF3) | (invalue << 2);
  else if (input_bitbucket_place == 4)
    input_bitbucket_ch = (input_bitbucket_ch & 0xCF) | (invalue << 4);
  else if (input_bitbucket_place == 6)
    input_bitbucket_ch = (input_bitbucket_ch & 0x3F) | (invalue << 6); 
  
  //if we have finished defining a character, or time to flush data, write it to disk
  //IMPROVE: could store more than a character sized buffer for faster disk access
  if ((input_bitbucket_place >= 6) || (flush == true))
  {
    putc(input_bitbucket_ch,fp);
    //printf ("\nbitchar:%x, place=%d",input_bitbucket_ch,input_bitbucket_place);
    //reset values to start processing next char
    input_bitbucket_place = -2;
    input_bitbucket_ch = (char)0xAA;
  }
  input_bitbucket_place++;
  input_bitbucket_place++;
}

char output_bitbucket (bool &eof_found, FILE *fp)
//Reads from disk the invalue of 0x00,0x01,0x03 in 2 byte chunks
//Rules: look for eof_found == true to find when to quit.
//       if quit, ignore the current character ( == 2 )
{
  
  char tempout;
  
  eof_found = false;
  //format is [4][3][2][1] in the 8 bit byte.
  //if there is no entry then the space will be occupied by 0x02.
  //note that on first pass when ch = 0, has no effect on data.
  output_bitbucket_ch = output_bitbucket_ch >> 2;
  if (output_bitbucket_place == 0)
  {
    //read target char from file
    output_bitbucket_ch = getc(fp);
    // Note: we may read in a char value with exactly the value of EOF from the
    // binary file, so forget about EOF and look for the 0x02 marker.
  } 
  output_bitbucket_place++;
  output_bitbucket_place++;
  //reset counter if reading is at end of the byte
  if (output_bitbucket_place >= 8)
    output_bitbucket_place = 0;
  //printf ("\nbitchar:%x, place=%d",output_bitbucket_ch,output_bitbucket_place);
  
  //the 2 lsb of the byte are the ones we are interested in now
  tempout = output_bitbucket_ch & 0x03;
  if (tempout == 0x02)
    eof_found=true;
  
  return (tempout);
}

OctreeNode* read_node(FILE *tree_structure)
{
  char color;
  bool temp= false;
  
  OctreeNode* p;
  color = output_bitbucket(temp,tree_structure);
  if ( temp == true )
    return(NULL);
  
  p = ( OctreeNode* ) malloc( sizeof( OctreeNode ) );
  //p->info_value = 0.0;
  //p = new OctreeNode;
  p->son = NULL;
  if(color == GRAY) {
    int i;
    i = 1;
    p->color = i;
    p->son = (struct child_pointer*) malloc(sizeof(struct child_pointer));
    
    p->son->pointer[0]= read_node(tree_structure);
    p->son->pointer[1]= read_node(tree_structure);
    p->son->pointer[2]= read_node(tree_structure);
    p->son->pointer[3]= read_node(tree_structure);
    p->son->pointer[4]= read_node(tree_structure);
    p->son->pointer[5]= read_node(tree_structure);
    p->son->pointer[6]= read_node(tree_structure);
    p->son->pointer[7]= read_node(tree_structure);
    p->son->pointer[0]->father =p;
    p->son->pointer[1]->father =p;
    p->son->pointer[2]->father =p;
    p->son->pointer[3]->father =p;
    p->son->pointer[4]->father =p;
    p->son->pointer[5]->father =p;
    p->son->pointer[6]->father =p;
    p->son->pointer[7]->father =p;
  }
  else
  {
    p->color = color;
    p->son = NULL;
    
  }
  return(p);   
}

void save_node(OctreeNode* node, FILE  *tree_structure)
{
  if( node->color == WHITE)
  {
    input_bitbucket (WHITE,false,tree_structure);
  }
  if( node->color == BLACK)
  {
    input_bitbucket (BLACK,false,tree_structure);
  }
  if( node->color == GRAY)
  {
    input_bitbucket (GRAY,false,tree_structure);
  }
  
  if(node->son != NULL)
  {
    save_node(node->son->pointer[0], tree_structure);
    save_node(node->son->pointer[1], tree_structure);
    save_node(node->son->pointer[2], tree_structure);
    save_node(node->son->pointer[3], tree_structure);
    save_node(node->son->pointer[4], tree_structure);
    save_node(node->son->pointer[5], tree_structure);
    save_node(node->son->pointer[6], tree_structure);
    save_node(node->son->pointer[7], tree_structure);
  }
  if ( node->father == NULL )
  {
    input_bitbucket (2,true,tree_structure);
  }
  
  return;
}

int volume = 0;
void octree_volume( OctreeNode* node, int level)
{
  if(node->father == NULL) volume = 0;
  if( node->color == WHITE)
  {
    volume += (int) pow(2.0, (double) level);
  }
  
  if(node->son != NULL)
  {
    octree_volume(node->son->pointer[0], level -1);
    octree_volume(node->son->pointer[1], level -1);
    octree_volume(node->son->pointer[2], level -1);
    octree_volume(node->son->pointer[3], level -1);
    octree_volume(node->son->pointer[4], level -1);
    octree_volume(node->son->pointer[5], level -1);
    octree_volume(node->son->pointer[6], level -1);
    octree_volume(node->son->pointer[7], level -1);
  }
}

void negative( OctreeNode* node )
{
  /*negative an octree , tree root is not changed*/
  
  if( node->color == WHITE)
  {
    node->color = BLACK;
    return;
  }
  if( node->color == BLACK)
  {
    node->color = WHITE;
    return;
  }
  if(node->son != NULL)
  {
    negative(node->son->pointer[0]);
    negative(node->son->pointer[1]);
    negative(node->son->pointer[2]);
    negative(node->son->pointer[3]);
    negative(node->son->pointer[4]);
    negative(node->son->pointer[5]);
    negative(node->son->pointer[6]);
    negative(node->son->pointer[7]);
  }
  return;
}




OctreeNode* create_node(char color_node)
{
  /*creat a node with specified color */
  OctreeNode *q;
  //int i;
  q = (OctreeNode *)malloc(sizeof(OctreeNode));
  if(color_node == GRAY)
  {
    q->son = (struct child_pointer*) malloc(sizeof(struct child_pointer));
    //for(i =0; i< 8; i++) 
    //q->son->pointer[i] = NULL;
  }
  else
  {
    q->son = NULL;
  }
  q-> color = color_node;
  q->father = NULL;
  //q->info_value = 0.0;
  return(q);
}

OctreeNode* copy(OctreeNode *node)
{
  /* make a copy of an octree, oringal one is not changed */
  OctreeNode *p;
  int i;
  p = create_node(node->color);
  if(node->color==GRAY)
  {
    for(i=0; i< 8; i++)
    {
      p->son->pointer[i]= copy(node->son->pointer[i]);
      p->son->pointer[i]->father= p;
    }
  }
  return(p);
}



void clean(OctreeNode *node)
{
  /* clean nodes with all their child nodes having the same color */
  int i;
  if(node->color==GRAY)
  {
    for(i=0; i< 8; i++)   
    {
      clean(node->son->pointer[i]);
    }
    if( (node->son->pointer[0]->color== BLACK) && (node->son->pointer[1]->color== BLACK) && 
      (node->son->pointer[2]->color== BLACK) && (node->son->pointer[3]->color== BLACK) &&
      (node->son->pointer[4]->color== BLACK) && (node->son->pointer[5]->color== BLACK) &&
      (node->son->pointer[6]->color== BLACK) && (node->son->pointer[7]->color== BLACK) )
    {
      for(i=0; i<8; i++)
        delete_node(node->son->pointer[i]);
      node->color= BLACK;
      free(node->son);
      node->son = NULL;
    }
    else if( (node->son->pointer[0]->color== WHITE) && (node->son->pointer[1]->color== WHITE) && 
      (node->son->pointer[2]->color== WHITE) && (node->son->pointer[3]->color== WHITE) &&
      (node->son->pointer[4]->color== WHITE) && (node->son->pointer[5]->color== WHITE) &&
      (node->son->pointer[6]->color== WHITE) && (node->son->pointer[7]->color== WHITE) )
    {
      for(i=0; i<8; i++)
        delete_node(node->son->pointer[i]);
      node->color= WHITE;
      free(node->son);
      node->son = NULL;
    }
    
  }
}



void detach(OctreeNode *node)
{
  /* detatch a branch from its tree */
  OctreeNode *father;
  int i;
  
  if(node == NULL ) return;
  if(node->father == NULL ) return;
  father = node->father;
  for(i = 0; i< 8; i++)
  {
    if(father->son->pointer[i] == node) 
    {
      father->son->pointer[i] = NULL;
      break;
    }
  }
  if(father->son != NULL)
    if( (father->son->pointer[0]==NULL) && (father->son->pointer[1]==NULL) &&
      (father->son->pointer[2]==NULL) && (father->son->pointer[3]==NULL) &&
      (father->son->pointer[4]==NULL) && (father->son->pointer[5]==NULL) &&
      (father->son->pointer[6]==NULL) && (father->son->pointer[7]==NULL) )
    {
      free(father->son);
      father->son = NULL;
    } 
}



int check_node_containment(OctreeNode *node, struct cubic_d *worldsize, 
                            vector_d target)
//return values;
//0 contained in black node
//1 contained in white node
//2 don't know (outside of octree box range)
{
  struct cubic_d sub_c;
  double x_mid, y_mid, z_mid;
  int retvalue;
    
  //failure case is: target point not inside currently considered box
  if ( (target.x > worldsize->x1) ||
  (target.y > worldsize->y1) ||
  (target.z > worldsize->z1) ||
  (target.x <= worldsize->x0) ||
  (target.y <= worldsize->y0) ||
  (target.z <= worldsize->z0) )
  {
    return 2;
  }

  //now we know point is contained
  if (node->color==WHITE)
  {
    //in known free space
    return 1;
  }

  if (node->color==BLACK)
  {
    //in unknown space
    return 0;
  }
  
  //now we know brick can be subdivided
  if (node->color==GRAY)
  {
    
    x_mid = (worldsize->x1 + worldsize->x0)/2.0;
    y_mid = (worldsize->y1 + worldsize->y0)/2.0;
    z_mid = (worldsize->z1 + worldsize->z0)/2.0;
    
    sub_c.x0= worldsize->x0;
    sub_c.y0= worldsize->y0;
    sub_c.z0= worldsize->z0;
    sub_c.x1= x_mid;
    sub_c.y1= y_mid;
    sub_c.z1= z_mid; 
    retvalue = check_node_containment(node->son->pointer[0], &sub_c, target);
    if (retvalue != 2) 
      goto check_node_containment_end;
    //sorry about the goto, but I think this is a good way
    
    sub_c.x0= worldsize->x0;
    sub_c.y0= worldsize->y0;
    sub_c.z0= z_mid;
    sub_c.x1= x_mid;
    sub_c.y1= y_mid;
    sub_c.z1= worldsize->z1;
    retvalue = check_node_containment(node->son->pointer[1], &sub_c, target);
    if (retvalue != 2) 
      goto check_node_containment_end; 
    
    sub_c.x0= worldsize->x0;
    sub_c.y0= y_mid;
    sub_c.z0= worldsize->z0;
    sub_c.x1= x_mid;
    sub_c.y1= worldsize->y1;
    sub_c.z1= z_mid;
    retvalue = check_node_containment(node->son->pointer[2], &sub_c, target);
    if (retvalue != 2) 
      goto check_node_containment_end;    

    sub_c.x0= worldsize->x0;
    sub_c.y0= y_mid;
    sub_c.z0= z_mid;
    sub_c.x1= x_mid;
    sub_c.y1= worldsize->y1;
    sub_c.z1= worldsize->z1;
    retvalue = check_node_containment(node->son->pointer[3], &sub_c, target);
    if (retvalue != 2) 
      goto check_node_containment_end;    

    sub_c.x0= x_mid;
    sub_c.y0= worldsize->y0;
    sub_c.z0= worldsize->z0;
    sub_c.x1= worldsize->x1;
    sub_c.y1= y_mid;
    sub_c.z1= z_mid;
    retvalue = check_node_containment(node->son->pointer[4], &sub_c, target);
    if (retvalue != 2) 
      goto check_node_containment_end;    

    sub_c.x0= x_mid;
    sub_c.y0= worldsize->y0;
    sub_c.z0= z_mid;
    sub_c.x1= worldsize->x1;
    sub_c.y1= y_mid;
    sub_c.z1= worldsize->z1;
    retvalue = check_node_containment(node->son->pointer[5], &sub_c, target);
    if (retvalue != 2) 
      goto check_node_containment_end;    

    sub_c.x0= x_mid;
    sub_c.y0= y_mid;
    sub_c.z0= worldsize->z0;
    sub_c.x1= worldsize->x1;
    sub_c.y1= worldsize->y1;
    sub_c.z1= z_mid;
    retvalue = check_node_containment(node->son->pointer[6], &sub_c, target);
    if (retvalue != 2) 
      goto check_node_containment_end;    

    sub_c.x0= x_mid;
    sub_c.y0= y_mid;
    sub_c.z0= z_mid;
    sub_c.x1= worldsize->x1;
    sub_c.y1= worldsize->y1;
    sub_c.z1= worldsize->z1;
    retvalue = check_node_containment(node->son->pointer[7], &sub_c, target);
    if (retvalue != 2)
      goto check_node_containment_end;
  }

  check_node_containment_end:
  return retvalue;
}

OctreeNode * create_block_node(int level, 
                                struct cubic *desired,
                                struct cubic *worldsize, int maxlevel)
{
  OctreeNode *p;
  struct cubic sub_c;
  int x_mid, y_mid, z_mid;
  int next_level;
  
  if ( (desired->x0 > worldsize->x1) ||
    (desired->y0 > worldsize->y1) ||
    (desired->z0 > worldsize->z1) ||
    (desired->x1 <= worldsize->x0) ||
    (desired->y1 <= worldsize->y0) ||
    (desired->z1 <= worldsize->z0) )
  {
    p = assign_black_node();
    if (level == 0)
      p->father = NULL;
    return(p);
  }; 
  
  //now we know brick is inside
  
  if ( ( (desired->x0 <= worldsize->x0) && (desired->x1 >= worldsize->x1) &&
    (desired->y0 <= worldsize->y0) && (desired->y1 >= worldsize->y1) &&
    (desired->z0 <= worldsize->z0) && (desired->z1 >= worldsize->z1) ) ||
    (level >= maxlevel) )
  {
    p = assign_white_node();
    if (level == 0)
      p->father = NULL;
    return(p);
  }; 
  
  //now we know brick can be subdivided.
  
  x_mid = (worldsize->x1 + worldsize->x0)/2;
  y_mid = (worldsize->y1 + worldsize->y0)/2;
  z_mid = (worldsize->z1 + worldsize->z0)/2;
  
  p = (OctreeNode *) malloc(sizeof(OctreeNode));    
  p->son = (struct child_pointer*) malloc(sizeof(struct child_pointer));
  if ((p == NULL) || (p->son == NULL))
  {
    printf("space max reached\n");
  } 
  p->color = GRAY;
  
  next_level = level+1;
  sub_c.x0= worldsize->x0;
  sub_c.y0= worldsize->y0;
  sub_c.z0= worldsize->z0;
  sub_c.x1= x_mid;
  sub_c.y1= y_mid;
  sub_c.z1= z_mid; 
  p->son->pointer[0] = create_block_node(next_level, desired, &sub_c, maxlevel);
  p->son->pointer[0]->father= p;
  
  sub_c.x0= worldsize->x0;
  sub_c.y0= worldsize->y0;
  sub_c.z0= z_mid;
  sub_c.x1= x_mid;
  sub_c.y1= y_mid;
  sub_c.z1= worldsize->z1;
  p->son->pointer[1] = create_block_node(next_level, desired, &sub_c, maxlevel);
  p->son->pointer[1]->father= p;
  
  sub_c.x0= worldsize->x0;
  sub_c.y0= y_mid;
  sub_c.z0= worldsize->z0;
  sub_c.x1= x_mid;
  sub_c.y1= worldsize->y1;
  sub_c.z1= z_mid;
  p->son->pointer[2] = create_block_node(next_level, desired, &sub_c, maxlevel);
  p->son->pointer[2]->father= p;
  
  sub_c.x0= worldsize->x0;
  sub_c.y0= y_mid;
  sub_c.z0= z_mid;
  sub_c.x1= x_mid;
  sub_c.y1= worldsize->y1;
  sub_c.z1= worldsize->z1;
  p->son->pointer[3] = create_block_node(next_level, desired, &sub_c, maxlevel);
  p->son->pointer[3]->father= p;
  
  sub_c.x0= x_mid;
  sub_c.y0= worldsize->y0;
  sub_c.z0= worldsize->z0;
  sub_c.x1= worldsize->x1;
  sub_c.y1= y_mid;
  sub_c.z1= z_mid;
  p->son->pointer[4] = create_block_node(next_level, desired, &sub_c, maxlevel);
  p->son->pointer[4]->father= p;
  
  sub_c.x0= x_mid;
  sub_c.y0= worldsize->y0;
  sub_c.z0= z_mid;
  sub_c.x1= worldsize->x1;
  sub_c.y1= y_mid;
  sub_c.z1= worldsize->z1;
  p->son->pointer[5] = create_block_node(next_level, desired, &sub_c, maxlevel);
  p->son->pointer[5]->father= p;
  
  sub_c.x0= x_mid;
  sub_c.y0= y_mid;
  sub_c.z0= worldsize->z0;
  sub_c.x1= worldsize->x1;
  sub_c.y1= worldsize->y1;
  sub_c.z1= z_mid;
  p->son->pointer[6] = create_block_node(next_level, desired, &sub_c, 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= worldsize->x1;
  sub_c.y1= worldsize->y1;
  sub_c.z1= worldsize->z1;
  p->son->pointer[7] = create_block_node(next_level, desired, &sub_c, maxlevel);
  p->son->pointer[7]->father= p;
  
  if (level == 0)
    p->father = NULL;
  return(p);
}

OctreeNode * create_line_node(int level, 
                               vector_i *center,
                               struct cubic *worldsize, int maxlevel)
{
  OctreeNode *p;
  struct cubic sub_c;
  int x_mid, y_mid, z_mid;
  int next_level,i;
  bool lineptfound;
  
  lineptfound = false;
  for (i=0;i<MAXLINELENGTH;i++)
  {
    //if there is any point in the line which is not outside the current one
    if ( !( (center[i].x > worldsize->x1) ||
      (center[i].y > worldsize->y1) ||
      (center[i].z > worldsize->z1) ||
      (center[i].x <= worldsize->x0) ||
      (center[i].y <= worldsize->y0) ||
      (center[i].z <= worldsize->z0) ) )
    {
      lineptfound = true;
    }
  }
  
  
  if ( lineptfound == false )
  {
    p = assign_black_node();
    if (level == 0)
      p->father = NULL;
    return(p);
  }; 
  
  //now we know brick is inside
  //look for a minimum sized brick to make white
  
  
  for (i=0;i<MAXLINELENGTH;i++)
  {
    
    if ( ( (center[i].x <= worldsize->x0) && (center[i].x >= worldsize->x1) &&
      (center[i].y <= worldsize->y0) && (center[i].y >= worldsize->y1) &&
      (center[i].z <= worldsize->z0) && (center[i].z >= worldsize->z1) ) ||
      (level >= maxlevel) )
    {
      p = assign_white_node();
      if (level == 0)
        p->father = NULL;
      return(p);
    }; 
  }
  
  //now we know brick can be subdivided.
  
  x_mid = (worldsize->x1 + worldsize->x0)/2;
  y_mid = (worldsize->y1 + worldsize->y0)/2;
  z_mid = (worldsize->z1 + worldsize->z0)/2;
  
  p = (OctreeNode *) malloc(sizeof(OctreeNode));    
  p->son = (struct child_pointer*) malloc(sizeof(struct child_pointer));
  if ((p == NULL) || (p->son == NULL))
  {
    printf("space max reached\n");
  } 
  p->color = GRAY;
  
  next_level = level+1;
  sub_c.x0= worldsize->x0;
  sub_c.y0= worldsize->y0;
  sub_c.z0= worldsize->z0;
  sub_c.x1= x_mid;
  sub_c.y1= y_mid;
  sub_c.z1= z_mid; 
  p->son->pointer[0] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[0]->father= p;
  
  sub_c.x0= worldsize->x0;
  sub_c.y0= worldsize->y0;
  sub_c.z0= z_mid;
  sub_c.x1= x_mid;
  sub_c.y1= y_mid;
  sub_c.z1= worldsize->z1;
  p->son->pointer[1] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[1]->father= p;
  
  sub_c.x0= worldsize->x0;
  sub_c.y0= y_mid;
  sub_c.z0= worldsize->z0;
  sub_c.x1= x_mid;
  sub_c.y1= worldsize->y1;
  sub_c.z1= z_mid;
  p->son->pointer[2] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[2]->father= p;
  
  sub_c.x0= worldsize->x0;
  sub_c.y0= y_mid;
  sub_c.z0= z_mid;
  sub_c.x1= x_mid;
  sub_c.y1= worldsize->y1;
  sub_c.z1= worldsize->z1;
  p->son->pointer[3] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[3]->father= p;
  
  sub_c.x0= x_mid;
  sub_c.y0= worldsize->y0;
  sub_c.z0= worldsize->z0;
  sub_c.x1= worldsize->x1;
  sub_c.y1= y_mid;
  sub_c.z1= z_mid;
  p->son->pointer[4] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[4]->father= p;
  
  sub_c.x0= x_mid;
  sub_c.y0= worldsize->y0;
  sub_c.z0= z_mid;
  sub_c.x1= worldsize->x1;
  sub_c.y1= y_mid;
  sub_c.z1= worldsize->z1;
  p->son->pointer[5] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[5]->father= p;
  
  sub_c.x0= x_mid;
  sub_c.y0= y_mid;
  sub_c.z0= worldsize->z0;
  sub_c.x1= worldsize->x1;
  sub_c.y1= worldsize->y1;
  sub_c.z1= z_mid;
  p->son->pointer[6] = create_line_node(next_level, center, &sub_c, 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= worldsize->x1;
  sub_c.y1= worldsize->y1;
  sub_c.z1= worldsize->z1;
  p->son->pointer[7] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[7]->father= p;
  
  if (level == 0)
    p->father = NULL;
  return(p);
}

OctreeNode * create_view_node(int level, 
                               vector_i *center,
                               int arraysize,
                               struct cubic *worldsize, int maxlevel)
{
  OctreeNode *p;
  struct cubic sub_c;
  int x_mid, y_mid, z_mid;
  int next_level,i;
  bool lineptfound;
  
  lineptfound = false;
  for (i=0;i<arraysize;i++)
  {
    //if there is any point in the line which is not outside the current one
    if ( !( (center[i].x > worldsize->x1) ||
      (center[i].y > worldsize->y1) ||
      (center[i].z > worldsize->z1) ||
      (center[i].x <= worldsize->x0) ||
      (center[i].y <= worldsize->y0) ||
      (center[i].z <= worldsize->z0) ) )
    {
      lineptfound = true;
    }
  }
  
  
  if ( lineptfound == false )
  {
    p = assign_black_node();
    if (level == 0)
      p->father = NULL;
    return(p);
  }; 
  
  //now we know brick is inside
  //look for a minimum sized brick to make white
  
  
  for (i=0;i<arraysize;i++)
  {
    
    if ( ( (center[i].x <= worldsize->x0) && (center[i].x >= worldsize->x1) &&
      (center[i].y <= worldsize->y0) && (center[i].y >= worldsize->y1) &&
      (center[i].z <= worldsize->z0) && (center[i].z >= worldsize->z1) ) ||
      (level >= maxlevel) )
    {
      p = assign_white_node();
      if (level == 0)
        p->father = NULL;
      return(p);
    }; 
  }
  
  //now we know brick can be subdivided.
  
  x_mid = (worldsize->x1 + worldsize->x0)/2;
  y_mid = (worldsize->y1 + worldsize->y0)/2;
  z_mid = (worldsize->z1 + worldsize->z0)/2;
  
  p = (OctreeNode *) malloc(sizeof(OctreeNode));    
  p->son = (struct child_pointer*) malloc(sizeof(struct child_pointer));
  if ((p == NULL) || (p->son == NULL))
  {
    printf("space max reached\n");
  } 
  p->color = GRAY;
  
  next_level = level+1;
  sub_c.x0= worldsize->x0;
  sub_c.y0= worldsize->y0;
  sub_c.z0= worldsize->z0;
  sub_c.x1= x_mid;
  sub_c.y1= y_mid;
  sub_c.z1= z_mid; 
  p->son->pointer[0] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[0]->father= p;
  
  sub_c.x0= worldsize->x0;
  sub_c.y0= worldsize->y0;
  sub_c.z0= z_mid;
  sub_c.x1= x_mid;
  sub_c.y1= y_mid;
  sub_c.z1= worldsize->z1;
  p->son->pointer[1] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[1]->father= p;
  
  sub_c.x0= worldsize->x0;
  sub_c.y0= y_mid;
  sub_c.z0= worldsize->z0;
  sub_c.x1= x_mid;
  sub_c.y1= worldsize->y1;
  sub_c.z1= z_mid;
  p->son->pointer[2] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[2]->father= p;
  
  sub_c.x0= worldsize->x0;
  sub_c.y0= y_mid;
  sub_c.z0= z_mid;
  sub_c.x1= x_mid;
  sub_c.y1= worldsize->y1;
  sub_c.z1= worldsize->z1;
  p->son->pointer[3] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[3]->father= p;
  
  sub_c.x0= x_mid;
  sub_c.y0= worldsize->y0;
  sub_c.z0= worldsize->z0;
  sub_c.x1= worldsize->x1;
  sub_c.y1= y_mid;
  sub_c.z1= z_mid;
  p->son->pointer[4] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[4]->father= p;
  
  sub_c.x0= x_mid;
  sub_c.y0= worldsize->y0;
  sub_c.z0= z_mid;
  sub_c.x1= worldsize->x1;
  sub_c.y1= y_mid;
  sub_c.z1= worldsize->z1;
  p->son->pointer[5] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[5]->father= p;
  
  sub_c.x0= x_mid;
  sub_c.y0= y_mid;
  sub_c.z0= worldsize->z0;
  sub_c.x1= worldsize->x1;
  sub_c.y1= worldsize->y1;
  sub_c.z1= z_mid;
  p->son->pointer[6] = create_line_node(next_level, center, &sub_c, 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= worldsize->x1;
  sub_c.y1= worldsize->y1;
  sub_c.z1= worldsize->z1;
  p->son->pointer[7] = create_line_node(next_level, center, &sub_c, maxlevel);
  p->son->pointer[7]->father= p;
  
  if (level == 0)
    p->father = NULL;
  return(p);
}


OctreeNode * union_node(OctreeNode *node1, OctreeNode *node2)
{
/* construct the union of two octrees, both of the orginal 
  trees are NOT destroyed */
  int  i;
  OctreeNode *q, *p[8];
  
  if( white_node(node1) || white_node(node2) )
  {
    q= create_node(WHITE);
    //delete_node(node1);
    //delete_node(node2);
    return(q);
  }
  else if  (black_node( node1))
  {
    //delete_node(node1);
    //detach(node2);
    return(copy(node2));
  }
  else if  (black_node( node2))
  {
    //delete_node(node2);
    //detach(node1);
    return(copy(node1)); 
  }
  else
  {
    for(i = 0; i < 8; i++)
    {
      p[i]=  union_node(node1->son->pointer[i], node2->son->pointer[i]);
    }
    if(white_node(p[0])&&white_node(p[1])&&white_node(p[2])&&white_node(p[3])&&
      white_node(p[4])&&white_node(p[5])&&white_node(p[6])&&white_node(p[7]))
    {
      for( i = 0; i < 8; i++) 
      {
        free(p[i]);
      }
      q= create_node(WHITE);
      return(q); 
    }
    else
    {
      q = create_node(GRAY);
      for( i = 0; i < 8; i++) 
      {
        q->son->pointer[i] = p[i];
        q->son->pointer[i] -> father = q;
      }
      //delete_node(node1);
      //delete_node(node2);
      return(q);
    }
  }
}

OctreeNode *intersection_node(OctreeNode *node1, OctreeNode *node2)
{
/* construct the intersection of two octrees, both of the orginal 
  trees are NOT destroyed */
  int  i;  
  OctreeNode *q, *p[8];   
  
  if( black_node(node1) || black_node(node2) )
  {   
    q= create_node(BLACK);
    //delete_node(node1);  
    //delete_node(node2);
    return(q);  
  }   
  else if  (white_node( node1))
  {   
    //delete_node(node1);
    //detach(node2);
    return(copy(node2));    
  }   
  else if  (white_node( node2))
  {   
    //delete_node(node2);
    //detach(node1); 
    return(copy(node1));    
  }   
  else 
  {
    for(i = 0; i < 8; i++)
    {
      p[i]=  intersection_node(node1->son->pointer[i], node2->son->pointer[i]);
    }
    if(black_node(p[0])&&black_node(p[1])&&black_node(p[2])&&black_node(p[3])&&
      black_node(p[4])&&black_node(p[5])&&black_node(p[6])&&black_node(p[7]))
    {
      for( i = 0; i < 8; i++) free(p[i]);
      
      q= create_node(BLACK);
      return(q); 
    }
    else 
    {
      q = create_node(GRAY);  
      for( i = 0; i < 8; i++) 
      {
        q->son->pointer[i] = p[i];
        q->son->pointer[i] -> father = q;
      }
      //delete_node(node1);  
      //delete_node(node2);
      return(q);
    }
  }    
}

OctreeNode * construct_octree_wrapper (struct free_space * pic, int maxlevel, Range_Sensor* jc)
//note: if the intersection point isn't inside the octree world, problems
//(fuzziness in generated octree) result.
{
  struct cubic incube;
  OctreeNode *root;
  
  incube.x0 = incube.y0 = incube.z0 = 0;   //note: other values for incube can result in errors;
  incube.x1 = incube.y1 = incube.z1 = 256; //should use this value...
  
  calculate_scaled_distance (pic,jc);
  //expand_image (pic,-5);
  root = construct_octree(pic, &incube, 0, maxlevel);
  root->father = NULL;
  return (root);
  
}

OctreeNode * create_block_wrapper (vector_i center, cubic dimensions, int maxlevel)
{
  struct cubic incube,descube;
  
  incube.x0 = incube.y0 = incube.z0 = 0;   //note: other values for incube can result in errors;
  incube.x1 = incube.y1 = incube.z1 = 256; //should use this value...
  
  descube.x0 = LIMIT + center.x - dimensions.x0;
  descube.x1 = LIMIT + center.x + dimensions.x1;
  descube.y0 = LIMIT + center.y - dimensions.y0;
  descube.y1 = LIMIT + center.y + dimensions.y1;
  descube.z0 = LIMIT - center.z - dimensions.z0;
  descube.z1 = LIMIT - center.z + dimensions.z1;
  
  return (create_block_node(0,&descube,&incube,maxlevel));
}



bool operator< (const PointClass& p1, const PointClass& p2)
{
  return ( memcmp( p1.p, p2.p, 3*sizeof(char)) < 0);
}

bool operator== (const PointClass& p1, const PointClass& p2)
{
  return ( memcmp( p1.p, p2.p, 3*sizeof(char)) == 0);
}


void update_cubes_set (std::set<PointClass> &s1, Range_Sensor* camera, 
                       OctreeNode* node, struct cubic_d worldsize)
{
  int i,j;
  PointClass temp;
  vector_d target;

  for (i=0; i<256; i++)
  {
    for (j=0; j<256; j++)
    {
      if ((camera->ray_depth_map[i][j] < camera->GetMaxRange()) && (camera->ray_depth_map[i][j] > 0))
      {
        temp.p[0] = (int) camera->ray_coord_map[i][j][0];
        temp.p[1] = (int) camera->ray_coord_map[i][j][1];
        temp.p[2] = (int) camera->ray_coord_map[i][j][2];
        target.x = camera->ray_coord_map[i][j][0];
        target.y = camera->ray_coord_map[i][j][1];
        target.z = camera->ray_coord_map[i][j][2];
        //TRACE("\ninto cubes set (%d,%d,%d)",temp.p[0],temp.p[1],temp.p[2]);
        //double x0 = worldsize.x0;
        //double x1 = worldsize.x1;
        //if element isn't already in set and is contained in black node, insert it
        if ((s1.find(temp) == s1.end()) && (check_node_containment(node, &worldsize, target) == 0))
        {
          s1.insert(temp);          
        }
      }
    }
  }
}

void extract_cubes_set (std::set<PointClass> &s1, double magnifyfactor, 
                                                            std::vector<Square> &squarevector, int maxlevel)
{  
  PointClass pointmarker;
  vector_d point, farpoint;

  double a[3],b[3],c[3],d[3],e[3],f[3],g[3],h[3];

  Square tempsq;

  std::set<PointClass>::iterator it;

  it = s1.begin();
  while (it != s1.end())
  {
    pointmarker = *it;

    //TRACE("\nout of cubes set (%d,%d,%d)", pointmarker.p[0],pointmarker.p[1],pointmarker.p[2]);

    double baseunitshift = magnifyfactor;

    point.x = pointmarker.p[0] * magnifyfactor;
    point.y = pointmarker.p[1] * magnifyfactor;
    point.z = pointmarker.p[2] * magnifyfactor;

    farpoint.x = (pointmarker.p[0]+1) * magnifyfactor;
    farpoint.y = (pointmarker.p[1]+1) * magnifyfactor;
    farpoint.z = (pointmarker.p[2]+1) * magnifyfactor;

    a[0] = point.x;     a[1] = point.y;     a[2] = farpoint.z;
    b[0] = point.x;     b[1] = farpoint.y;  b[2] = farpoint.z;
    c[0] = farpoint.x;  c[1] = point.y;     c[2] = farpoint.z;
    d[0] = farpoint.x;  d[1] = farpoint.y;  d[2] = farpoint.z;
    e[0] = point.x;     e[1] = point.y;     e[2] = point.z;
    f[0] = point.x;     f[1] = farpoint.y;  f[2] = point.z;
    g[0] = farpoint.x;  g[1] = point.y;     g[2] = point.z;
    h[0] = farpoint.x;  h[1] = farpoint.y;  h[2] = point.z;

    //TRACE("\nwe have a cube at (%f,%f,%f)", a[0],a[1],a[2]);
    
    //the following is messy but helps speed the Square < and == operators
    //sigh
    tempsq.p[0][0] = a[0];tempsq.p[0][1] = a[1];tempsq.p[0][2] = a[2]; 
    tempsq.p[1][0] = b[0];tempsq.p[1][1] = b[1];tempsq.p[1][2] = b[2]; 
    tempsq.p[2][0] = d[0];tempsq.p[2][1] = d[1];tempsq.p[2][2] = d[2]; 
    tempsq.p[3][0] = c[0];tempsq.p[3][1] = c[1];tempsq.p[3][2] = c[2]; 
    squarevector.push_back(tempsq);
    tempsq.p[0][0] = e[0];tempsq.p[0][1] = e[1];tempsq.p[0][2] = e[2]; 
    tempsq.p[1][0] = f[0];tempsq.p[1][1] = f[1];tempsq.p[1][2] = f[2]; 
    tempsq.p[2][0] = h[0];tempsq.p[2][1] = h[1];tempsq.p[2][2] = h[2]; 
    tempsq.p[3][0] = g[0];tempsq.p[3][1] = g[1];tempsq.p[3][2] = g[2]; 
    squarevector.push_back(tempsq);
    tempsq.p[0][0] = c[0];tempsq.p[0][1] = c[1];tempsq.p[0][2] = c[2]; 
    tempsq.p[1][0] = d[0];tempsq.p[1][1] = d[1];tempsq.p[1][2] = d[2]; 
    tempsq.p[2][0] = h[0];tempsq.p[2][1] = h[1];tempsq.p[2][2] = h[2]; 
    tempsq.p[3][0] = g[0];tempsq.p[3][1] = g[1];tempsq.p[3][2] = g[2]; 
    squarevector.push_back(tempsq);
    tempsq.p[0][0] = a[0];tempsq.p[0][1] = a[1];tempsq.p[0][2] = a[2]; 
    tempsq.p[1][0] = b[0];tempsq.p[1][1] = b[1];tempsq.p[1][2] = b[2]; 
    tempsq.p[2][0] = f[0];tempsq.p[2][1] = f[1];tempsq.p[2][2] = f[2]; 
    tempsq.p[3][0] = e[0];tempsq.p[3][1] = e[1];tempsq.p[3][2] = e[2]; 
    squarevector.push_back(tempsq);
    tempsq.p[0][0] = d[0];tempsq.p[0][1] = d[1];tempsq.p[0][2] = d[2]; 
    tempsq.p[1][0] = b[0];tempsq.p[1][1] = b[1];tempsq.p[1][2] = b[2]; 
    tempsq.p[2][0] = f[0];tempsq.p[2][1] = f[1];tempsq.p[2][2] = f[2]; 
    tempsq.p[3][0] = h[0];tempsq.p[3][1] = h[1];tempsq.p[3][2] = h[2]; 
    squarevector.push_back(tempsq);
    tempsq.p[0][0] = c[0];tempsq.p[0][1] = c[1];tempsq.p[0][2] = c[2]; 
    tempsq.p[1][0] = a[0];tempsq.p[1][1] = a[1];tempsq.p[1][2] = a[2]; 
    tempsq.p[2][0] = e[0];tempsq.p[2][1] = e[1];tempsq.p[2][2] = e[2]; 
    tempsq.p[3][0] = g[0];tempsq.p[3][1] = g[1];tempsq.p[3][2] = g[2]; 
    squarevector.push_back(tempsq);

    it ++;
  }
    
  return;
}

void extract_block_node(OctreeNode *node, struct cubic_d *worldsize, 
                                                std::vector<Square> &squarevector, bool firstlevel, int colour)
{
  struct cubic_d sub_c;
  double x_mid, y_mid, z_mid;
  
  double a[3],b[3],c[3],d[3],e[3],f[3],g[3],h[3];
  Square tempsq;
  
  if ((node->color==colour) || ((firstlevel==true) && (node->color==WHITE)) )
  {
    //    TRACE ("\nBLACK(%f,%f,%f)(%f,%f,%f)"
    //      ,worldsize->x0,worldsize->y0,worldsize->z0,worldsize->x1,worldsize->y1,worldsize->z1);
    a[0] = worldsize->x0; a[1] = worldsize->y0; a[2] = worldsize->z1;
    b[0] = worldsize->x0; b[1] = worldsize->y1; b[2] = worldsize->z1;
    c[0] = worldsize->x1; c[1] = worldsize->y0; c[2] = worldsize->z1;
    d[0] = worldsize->x1; d[1] = worldsize->y1; d[2] = worldsize->z1;
    e[0] = worldsize->x0; e[1] = worldsize->y0; e[2] = worldsize->z0;
    f[0] = worldsize->x0; f[1] = worldsize->y1; f[2] = worldsize->z0;
    g[0] = worldsize->x1; g[1] = worldsize->y0; g[2] = worldsize->z0;
    h[0] = worldsize->x1; h[1] = worldsize->y1; h[2] = worldsize->z0;
    
    
    //the following is messy but helps speed the Square < and == operators
    //sigh
    tempsq.p[0][0] = a[0];tempsq.p[0][1] = a[1];tempsq.p[0][2] = a[2]; 
    tempsq.p[1][0] = b[0];tempsq.p[1][1] = b[1];tempsq.p[1][2] = b[2]; 
    tempsq.p[2][0] = d[0];tempsq.p[2][1] = d[1];tempsq.p[2][2] = d[2]; 
    tempsq.p[3][0] = c[0];tempsq.p[3][1] = c[1];tempsq.p[3][2] = c[2]; 
    squarevector.push_back(tempsq);
    tempsq.p[0][0] = e[0];tempsq.p[0][1] = e[1];tempsq.p[0][2] = e[2]; 
    tempsq.p[1][0] = f[0];tempsq.p[1][1] = f[1];tempsq.p[1][2] = f[2]; 
    tempsq.p[2][0] = h[0];tempsq.p[2][1] = h[1];tempsq.p[2][2] = h[2]; 
    tempsq.p[3][0] = g[0];tempsq.p[3][1] = g[1];tempsq.p[3][2] = g[2]; 
    squarevector.push_back(tempsq);
    tempsq.p[0][0] = c[0];tempsq.p[0][1] = c[1];tempsq.p[0][2] = c[2]; 
    tempsq.p[1][0] = d[0];tempsq.p[1][1] = d[1];tempsq.p[1][2] = d[2]; 
    tempsq.p[2][0] = h[0];tempsq.p[2][1] = h[1];tempsq.p[2][2] = h[2]; 
    tempsq.p[3][0] = g[0];tempsq.p[3][1] = g[1];tempsq.p[3][2] = g[2]; 
    squarevector.push_back(tempsq);
    tempsq.p[0][0] = a[0];tempsq.p[0][1] = a[1];tempsq.p[0][2] = a[2]; 
    tempsq.p[1][0] = b[0];tempsq.p[1][1] = b[1];tempsq.p[1][2] = b[2]; 
    tempsq.p[2][0] = f[0];tempsq.p[2][1] = f[1];tempsq.p[2][2] = f[2]; 
    tempsq.p[3][0] = e[0];tempsq.p[3][1] = e[1];tempsq.p[3][2] = e[2]; 
    squarevector.push_back(tempsq);
    tempsq.p[0][0] = d[0];tempsq.p[0][1] = d[1];tempsq.p[0][2] = d[2]; 
    tempsq.p[1][0] = b[0];tempsq.p[1][1] = b[1];tempsq.p[1][2] = b[2]; 
    tempsq.p[2][0] = f[0];tempsq.p[2][1] = f[1];tempsq.p[2][2] = f[2]; 
    tempsq.p[3][0] = h[0];tempsq.p[3][1] = h[1];tempsq.p[3][2] = h[2]; 
    squarevector.push_back(tempsq);
    tempsq.p[0][0] = c[0];tempsq.p[0][1] = c[1];tempsq.p[0][2] = c[2]; 
    tempsq.p[1][0] = a[0];tempsq.p[1][1] = a[1];tempsq.p[1][2] = a[2]; 
    tempsq.p[2][0] = e[0];tempsq.p[2][1] = e[1];tempsq.p[2][2] = e[2]; 
    tempsq.p[3][0] = g[0];tempsq.p[3][1] = g[1];tempsq.p[3][2] = g[2]; 
    squarevector.push_back(tempsq);
    
    return;
  }
  
  firstlevel = false;
  //now we know brick can be subdivided.
  if (node->color==GRAY)
  {
    
    x_mid = (worldsize->x1 + worldsize->x0)/2.0;
    y_mid = (worldsize->y1 + worldsize->y0)/2.0;
    z_mid = (worldsize->z1 + worldsize->z0)/2.0;
    
    sub_c.x0= worldsize->x0;
    sub_c.y0= worldsize->y0;
    sub_c.z0= worldsize->z0;
    sub_c.x1= x_mid;
    sub_c.y1= y_mid;
    sub_c.z1= z_mid; 
    extract_block_node(node->son->pointer[0], &sub_c, squarevector, firstlevel, colour);
    
    sub_c.x0= worldsize->x0;
    sub_c.y0= worldsize->y0;
    sub_c.z0= z_mid;
    sub_c.x1= x_mid;
    sub_c.y1= y_mid;
    sub_c.z1= worldsize->z1;
    extract_block_node(node->son->pointer[1], &sub_c, squarevector, firstlevel, colour);
    
    sub_c.x0= worldsize->x0;
    sub_c.y0= y_mid;
    sub_c.z0= worldsize->z0;
    sub_c.x1= x_mid;
    sub_c.y1= worldsize->y1;
    sub_c.z1= z_mid;
    extract_block_node(node->son->pointer[2], &sub_c, squarevector, firstlevel, colour);
    
    sub_c.x0= worldsize->x0;
    sub_c.y0= y_mid;
    sub_c.z0= z_mid;
    sub_c.x1= x_mid;
    sub_c.y1= worldsize->y1;
    sub_c.z1= worldsize->z1;
    extract_block_node(node->son->pointer[3], &sub_c, squarevector, firstlevel, colour);
    
    sub_c.x0= x_mid;
    sub_c.y0= worldsize->y0;
    sub_c.z0= worldsize->z0;
    sub_c.x1= worldsize->x1;
    sub_c.y1= y_mid;
    sub_c.z1= z_mid;
    extract_block_node(node->son->pointer[4], &sub_c, squarevector, firstlevel, colour);
    
    sub_c.x0= x_mid;
    sub_c.y0= worldsize->y0;
    sub_c.z0= z_mid;
    sub_c.x1= worldsize->x1;
    sub_c.y1= y_mid;
    sub_c.z1= worldsize->z1;
    extract_block_node(node->son->pointer[5], &sub_c, squarevector, firstlevel, colour);
    
    sub_c.x0= x_mid;
    sub_c.y0= y_mid;
    sub_c.z0= worldsize->z0;
    sub_c.x1= worldsize->x1;
    sub_c.y1= worldsize->y1;
    sub_c.z1= z_mid;
    extract_block_node(node->son->pointer[6], &sub_c, squarevector, firstlevel, colour);
    
    sub_c.x0= x_mid;
    sub_c.y0= y_mid;
    sub_c.z0= z_mid;
    sub_c.x1= worldsize->x1;
    sub_c.y1= worldsize->y1;
    sub_c.z1= worldsize->z1;
    extract_block_node(node->son->pointer[7], &sub_c, squarevector, firstlevel, colour);
  }
  
  return;
}

bool operator< (const Square& p1, const Square& p2)
{
  return ( memcmp( p1.p, p2.p, 12*sizeof(double)) < 0);
}

bool operator== (const Square& p1, const Square& p2)
{
  return ( memcmp( p1.p, p2.p, 12*sizeof(double)) == 0);
}

void calc_cube_facets(std::vector<Square> &squarevector, GL_Mesh &mesh)
{
  
  Vector4 tempvector;
  tempvector[3] = 1;
  
  //construct cubes out of 12 triangles using 8 given vertices
  std::vector<Square>::iterator iter;
  std::vector<Square>::iterator dup;
  
  mesh.GetVertexes().clear();
  mesh.GetFacets().clear();
  
  std::sort (squarevector.begin(),squarevector.end());
  
  for (iter = squarevector.begin(); iter != squarevector.end() ; iter++)
  {
    if ((*iter == *(iter+1)) && (iter != squarevector.end()))
    {
      dup = iter;
      do
      {
        iter++;
      } while (*(iter+1) == *dup);
    } else
    {
      //cout << "single found" << *iter << endl;
      
      //these should hopefully be all the non duplicate triangles only
      tempvector[0] = (*iter).p[0][0];
      tempvector[1] = (*iter).p[0][1];
      tempvector[2] = (*iter).p[0][2];
      mesh.AddVertex(tempvector);
      tempvector[0] = (*iter).p[1][0];
      tempvector[1] = (*iter).p[1][1];
      tempvector[2] = (*iter).p[1][2];
      mesh.AddVertex(tempvector);
      tempvector[0] = (*iter).p[2][0];
      tempvector[1] = (*iter).p[2][1];
      tempvector[2] = (*iter).p[2][2];
      mesh.AddVertex(tempvector);
      tempvector[0] = (*iter).p[3][0];
      tempvector[1] = (*iter).p[3][1];
      tempvector[2] = (*iter).p[3][2];
      mesh.AddVertex(tempvector);
    }
  }
  
  //TRACE ("\nSize of squarevector: %d",squarevector.size());
  //don't need this anymore;
  squarevector.clear();
  
  Facet facet;
  facet.vertexNumbers.reserve(3);
  facet.direction = 1; // 1 == clockwise?
  //include all the triangles in the new mesh
  
  int meshsize = mesh.GetVertexes().size();
  
  for (int i = 0; i < meshsize; i=i+4)
  {
    facet.vertexNumbers.clear();
    facet.vertexNumbers.push_back(i);
    facet.vertexNumbers.push_back(i+1);
    facet.vertexNumbers.push_back(i+2);
    mesh.AddFacet(facet);
    
    facet.vertexNumbers.clear();
    facet.vertexNumbers.push_back(i);
    facet.vertexNumbers.push_back(i+2);
    facet.vertexNumbers.push_back(i+3);
    mesh.AddFacet(facet);
    //assert (facet.vertexNumbers.size() == 3);
  }
  
}

---