Search.cpp
Go to the documentation of this file.
00001 #include <sbpl_interface/bfs3d/BFS_3D.h>
00002 #include <iostream>
00003 #include <boost/thread.hpp>
00004 
00005 namespace sbpl_interface{
00006 
00007 #define EXPAND_NEIGHBOR(offset)                         \
00008   if (distance_grid[currentNode + offset] < 0) {        \
00009     queue[queue_tail++] = currentNode + offset;         \
00010     distance_grid[currentNode + offset] = currentCost;  \
00011     boost::this_thread::interruption_point();           \
00012       }
00013 
00014 void BFS_3D::search(int width, int planeSize, int volatile* distance_grid, int* queue, int &queue_head, int &queue_tail) {
00015   while (queue_head < queue_tail) {
00016     int currentNode = queue[queue_head++];
00017     int currentCost = distance_grid[currentNode] + 1;
00018 
00019     EXPAND_NEIGHBOR(-width);
00020     EXPAND_NEIGHBOR(1);
00021     EXPAND_NEIGHBOR(width);
00022     EXPAND_NEIGHBOR(-1);
00023     EXPAND_NEIGHBOR(-width-1);
00024     EXPAND_NEIGHBOR(-width+1);
00025     EXPAND_NEIGHBOR(width+1);
00026     EXPAND_NEIGHBOR(width-1);
00027     EXPAND_NEIGHBOR(planeSize);
00028     EXPAND_NEIGHBOR(-width+planeSize);
00029     EXPAND_NEIGHBOR(1+planeSize);
00030     EXPAND_NEIGHBOR(width+planeSize);
00031     EXPAND_NEIGHBOR(-1+planeSize);
00032     EXPAND_NEIGHBOR(-width-1+planeSize);
00033     EXPAND_NEIGHBOR(-width+1+planeSize);
00034     EXPAND_NEIGHBOR(width+1+planeSize);
00035     EXPAND_NEIGHBOR(width-1+planeSize);
00036     EXPAND_NEIGHBOR(-planeSize);
00037     EXPAND_NEIGHBOR(-width-planeSize);
00038     EXPAND_NEIGHBOR(1-planeSize);
00039     EXPAND_NEIGHBOR(width-planeSize);
00040     EXPAND_NEIGHBOR(-1-planeSize);
00041     EXPAND_NEIGHBOR(-width-1-planeSize);
00042     EXPAND_NEIGHBOR(-width+1-planeSize);
00043     EXPAND_NEIGHBOR(width+1-planeSize);
00044     EXPAND_NEIGHBOR(width-1-planeSize);
00045   }
00046   //std::cerr << "Search thread done" << std::endl;
00047   running = false;
00048 }
00049 }


sbpl_interface
Author(s): Gil Jones
autogenerated on Sun Jan 17 2016 12:57:03