collisiondetectors/CD_rangesensor/Octree_Data_Fuser.cpp

Go to the documentation of this file.

#pragma warning( disable : 4786 )

//Octree_Data_Fuser
#include "Octree_Data_Fuser.h"
#include <gl/gl.h>
#include <opengl/gl_material.h>


Octree_Data_Fuser::Octree_Data_Fuser()
{
  this->maxlevel = 6;
  this->octree_display_mode = octree_whitedisplay;
  this->scalefactor = 128.0;  //<- the default used by Yong's octree code
  this->expected_vertexes = 80000;
  this->expected_facets =  40000;
  this->expected_squarevectors = 53333;
  
  root2 = create_node (BLACK);
  root2->father = NULL;
  this->lu = NULL;
  this->display_lu = NULL;
  this->track_sensed_obstacles = false;
  
  obstacle_points.clear();
  
}

Octree_Data_Fuser::Octree_Data_Fuser (const Octree_Data_Fuser& right)
//I hope this doesn't do anything with the universe because it's not the universe
//we want to use for the sensor based collison dectection
{
  TRACE("\n(**warning: untested)Octree_Data_Fuser copy constructor");
  
  this->maxlevel = right.maxlevel;
  this->octree_display_mode = right.octree_display_mode;
  this->scalefactor = right.scalefactor;  //where 30.0 is the world x,y and z extents from (0,0,0)
  this->expected_vertexes = right.expected_vertexes;
  this->expected_facets =  right.expected_facets;
  this->expected_squarevectors =  right.expected_squarevectors;
  
  this->root2 = copy (right.root2); //(ie. deep copy) (assumed free space around robot also copied)
  this->lu = dynamic_cast<GL_Universe *>(right.lu->Clone());
  this->display_lu = dynamic_cast<GL_Universe *>(right.display_lu->Clone());
  this->track_sensed_obstacles = right.track_sensed_obstacles;
  
  this->obstacle_points = right.obstacle_points;
  
}



Octree_Data_Fuser::~Octree_Data_Fuser()
{
  //delete all octree nodes, free_space struct
  delete_node (root2);
  //delete display local universe
  if (display_lu != NULL)
  {
    delete display_lu;
    display_lu = NULL;
  }
}


void Octree_Data_Fuser::new_local_universe (Universe &robot_universe)
{
  std::vector< Entity* > tempEntities;
  tempEntities.clear();
  tempEntities = robot_universe.GetAllEntities() ;
  
  //  Take the passed in Universe object, and scan through to the find all
  //  RobotBase objects.
  
  lu = new GL_Universe; 
  lu->Empty();
  int tsize = tempEntities.size() ;
  for( int i = 0; i < tsize; i++ )
  {
    const R_OpenChain* r_openchain = dynamic_cast< const R_OpenChain* >( tempEntities[ i ] ) ;
    if( r_openchain != NULL )
    {
      //  Then, using AddEntity() add them to a custom universe object.  This will
      //  have the effect of copying all the robot geometries but none of geometries
      //  for the rest of the world.
      Entity* addme = tempEntities[ i ]->Clone() ;
      addme->SetFrameManager( lu->GetFrameManager() ) ;
      lu->AddEntity( addme );
      //int size = lu->GetFrameManager()->GetNumberOfFrames() ;
      delete addme;
      addme = NULL;
      //TRACE("\nR_OpenChain found.");
    }
  }
}

void Octree_Data_Fuser::Add_Block(cubic worldextents, cubic newblocksize)
{
  OctreeNode *root,*union_result;
  
  root = create_block_node(0,&newblocksize,&worldextents,maxlevel);
  root->father = NULL;
  union_result = union_node (root,root2);
  delete_node (root);
  delete_node (root2);
  root2 = union_result;
}

void Octree_Data_Fuser::Save_State(const char * filename)
//saves current octree to disk.
{
  //TRACE("\nOctree_Data_Fuser::Save_State()");
  FILE *fpointer;
  
  fpointer = fopen (filename,"w");
  
  //root2 should already be cleaned
  //clean (root2);
  reset_bitbuckets ();
  save_node(root2,fpointer);  
  fclose (fpointer);
  
}

void Octree_Data_Fuser::Load_State(const char * filename)
//loads inital octree file from disk.
{
  //TRACE("\nOctree_Data_Fuser::Load_State()");
  //struct node * retvalue;
  FILE *fpointer;
  
  delete_node(root2);
  fpointer = NULL;
  fpointer = fopen (filename,"rb");
  assert(fpointer != NULL);
  reset_bitbuckets ();
  root2 = read_node(fpointer);
  root2->father = NULL;
  
  fclose (fpointer);
}

void Octree_Data_Fuser::GL_Display_Octree ()
{
  //display robot and octree mesh in main robot window
  //    BOOL success = wglMakeCurrent( this->cWindow_ptr->GetDC()->m_hDC, hglrc_var ); // call OpenGL APIs as desired ... 
  if( display_lu != NULL )
  {
    ::glClearColor( 1.0, 1.0, 1.0, 1.0 );
    ::glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    
    glEnable(GL_LIGHTING) ;             
    glEnable(GL_BLEND) ;        
    glLightModeli( GL_LIGHT_MODEL_TWO_SIDE, 1 );
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) ;
    //glShadeModel( GL_FLAT ) ;
    glShadeModel( GL_SMOOTH ) ;
    
    double r = 0.8f ;
    double g = 0.8f ;
    double b = 0.8f ;
    float ambient[]             = { 0, 0, 0 } ; 
    float diffuse[]             = { r, g, b } ; 
    float specular[]    = { r, g, b } ; 
    float position[ ]   = {50, 50, 50, 1};
    
    glEnable( GL_LIGHTING );
    glEnable( GL_LIGHT0 ) ;
    glLightfv( GL_LIGHT0, GL_POSITION, position ) ;
    glLightfv( GL_LIGHT0, GL_AMBIENT, ambient );
    glLightfv( GL_LIGHT0, GL_DIFFUSE, diffuse );
    glLightfv( GL_LIGHT0, GL_SPECULAR, specular );
    
    //::glDisable( GL_DEPTH_TEST );
    GL_Material& matTransparentGreen = GL_Material::matGreenPlastic ;
    matTransparentGreen.SetTransparancy( 1.0 );
    matTransparentGreen.GLDraw() ;
    display_lu->GLDraw();
  }
  //::glFlush();
  //glUniverse->GLDraw();
  //    HDC hdc = this->cWindow_ptr->GetSafeHdc();
  //    BOOL swapSuccess = wglSwapLayerBuffers( this->cWindow_ptr->GetDC()->m_hDC, WGL_SWAP_MAIN_PLANE );
  //    success = wglMakeCurrent( NULL, NULL );
}





void Octree_Data_Fuser::Update (Range_Sensor *camera)
{
                //TRACE("\nAfter.Ray Depth map maxrange is: %f, angle is %f",camera->GetMaxRange(), camera->GetAngle());
                //TRACE("\nAfter.Ray Coord map middle is: %f,%f,%f",camera->ray_coord_map[128][128][0],camera->ray_coord_map[128][128][1],camera->ray_coord_map[128][128][2]);
                //keep the huge free_space var alive for as short a time as possible
    
    //adjust scalefactor before passing pic into octree library
    for (int i=0; i< 256; i++)
    {
      for (int j=0; j < 256; j++)
      {  
        camera->ray_coord_map[i][j][0] = camera->ray_coord_map[i][j][0] * scalefactor ;
        camera->ray_coord_map[i][j][1] = camera->ray_coord_map[i][j][1] * scalefactor ;
        camera->ray_coord_map[i][j][2] = camera->ray_coord_map[i][j][2] * scalefactor ;
      }
    }
    
    //update set containing obstacle points
    if (track_sensed_obstacles)
    {
      cubic_d worldsize;
  
      // using scalefactor, scale worldsize to fit with Yong's octree algorithim worldsize
      worldsize.x0 = worldsize.y0 = worldsize.z0 = -128.0 ;
      worldsize.x1 = worldsize.y1 = worldsize.z1 = 127.0 ; 

      update_cubes_set (obstacle_points, camera, root2, worldsize);
    }

    //update octree containing free/unknown space
    OctreeNode *root,*union_result;
        struct free_space* pic;

    pic = new free_space;
    pic->apex.x = camera->ray_source[0] * scalefactor ;
    pic->apex.y = camera->ray_source[1] * scalefactor ;
    pic->apex.z = camera->ray_source[2] * scalefactor ;

    //am passing in camera only to give the ray_coord_map
    root = construct_octree_wrapper(pic,maxlevel,camera);
    delete pic;
    pic = NULL;
    
    union_result = union_node (root,root2);
    delete_node(root);
    delete_node(root2);
    root2 = union_result;               
    //root is deleted by union_node?
    
    clean (root2);

}



void Octree_Data_Fuser::Convert ( CD_Vcollide* &lv_free,  CD_Vcollide* &lv_obstacle, 
                            Universe &robot_universe, bool valid_config)
{
  //CD_Vcollide* lv;
  GL_Mesh* mesh;
  
  std::vector<Square> squarevector;
  squarevector.reserve(expected_squarevectors);
  
  
  cubic_d worldsize;
  
  // using scalefactor, scale worldsize to scale with Yong's octree algorithim size
  // back up to orginal size
  worldsize.x0 = worldsize.y0 = worldsize.z0 = -(1.0/scalefactor)*128.0 ;
  worldsize.x1 = worldsize.y1 = worldsize.z1 = (1.0/scalefactor)*127.0 ; 
  
  if (lu != NULL)
  {
    delete lu;
    lu = NULL;
  }
  if (display_lu != NULL)
  {
    delete display_lu;
    display_lu = NULL;
  }
  
  new_local_universe (robot_universe);
  
  //only care about updating config if we currently have a valid one
  if (valid_config)
  {    
    if (track_sensed_obstacles)
    {
      mesh = new GL_Mesh(lu->GetFrameManager());
      
      mesh->GetVertexes().reserve( expected_vertexes );
      mesh->GetVertexes().reserve( expected_facets );
      mesh->SetName( "octreeMeshRed" );
      
      squarevector.clear();
      
      extract_cubes_set (obstacle_points, 1.0/scalefactor, squarevector, maxlevel);
      
      if (obstacle_points.size() > 0)
      {
        calc_cube_facets (squarevector, *mesh);
        //TRACE ("\nSize of (RED) intermediate mesh-> sqcap %d, vert %d, face %d",squarevector.size(),mesh->vertexes.size(), mesh->facets.size());
        
        //updating info on robot arm position is unnecesscary when the known/unknown boundary is not transparent
        lu->SetConfiguration(showconfig);
        
        lu->AddEntity(mesh);
        
        lv_obstacle = new CD_Vcollide (*lu);
        
        if (octree_display_mode == octree_obstacledisplay)
        {
          //display this local universe in Planner window (don't delete it, we need to display it)
          display_lu = lu;
        } else
        {
          //delete this lu if we don't need to display it
          delete lu;
        }
        
        new_local_universe(robot_universe);
      }
      delete mesh;
      mesh = NULL;
    }
    
    //updating info on robot arm position is unnecesscary when the known/unknown boundary is not transparent
    lu->SetConfiguration(showconfig);
  }
  
  if (octree_display_mode == octree_whitedisplay)
  {
    mesh = new GL_Mesh(lu->GetFrameManager());
    //expect to get an octree -> vcollide mesh of this size, to avoid costly resizing
    mesh->GetVertexes().reserve( expected_vertexes );
    mesh->GetFacets().reserve( expected_facets );
    mesh->SetName( "octreeMeshWhite" );
    
    squarevector.clear();
    
    //create a list of squares from the octree
    extract_block_node(root2, &worldsize, squarevector, true, WHITE);
    //create a GL_Mesh from optimized list of squares
    calc_cube_facets(squarevector, *mesh);
    
    //output the octree, in mesh form to a vcollide mesh
    lu->AddEntity(mesh);
    //TRACE ("\nSize of (WHITE) intermediate mesh: sqcap %d, vert %d, face %d",squarevector.capacity(),mesh->vertexes.size(), mesh->facets.size());
    
    //clone of mesh has been placed in universe, don't need the original anymore
    delete mesh;
    mesh = NULL;
    //display this local universe in Planner window (don't delete it, we need to display it)
    display_lu = lu;
    new_local_universe(robot_universe);
  }
  
  mesh = new GL_Mesh(lu->GetFrameManager());
  
  //expect to get an octree -> vcollide mesh of this size, to avoid costly resizing
  mesh->GetVertexes().reserve( expected_vertexes );
  mesh->GetFacets().reserve( expected_facets );
  mesh->SetName( "octreeMeshBlack" );
  
  squarevector.clear();
                
  extract_block_node(root2, &worldsize, squarevector, true, BLACK);
  //push_back the facets
  calc_cube_facets(squarevector, *mesh);
  
  //free memory used by squarevector now, its not needed anymore
  squarevector.reserve(0);
  
  //output the octree, in mesh form to a vcollide mesh
  lu->AddEntity(mesh);
  //TRACE ("\nSize of (BLACK) intermediate mesh: vert %d, face %d",mesh->vertexes.size(), mesh->facets.size());
  
                
  delete mesh;
  mesh = NULL;
  lv_free = new CD_Vcollide (*lu);
  
  if (octree_display_mode == octree_blackdisplay)
  {
    //display this local universe in Planner window
    display_lu = lu;
  } else
  {
    delete lu; //if we don't care about displaying it, delete it to save memory
    lu = NULL;
  }
  
  //void extract_cubes_set (std::set<PointClass> &s1, struct cubic_d *worldsize, 
  //                                                        std::vector<Square> &squarevector, int maxlevel);
  //void extract_block_node(struct node *node, struct cubic_d *worldsize, 
  //                        std::vector<Square> &squarevector, bool firstlevel, int colour);
  //void calc_cube_facets(std::vector<Square> &squarevector, GL_Mesh &mesh);
  
  //return lv_white;
}

---