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 }