Fawkes API  Fawkes Development Version
map_cspace.cpp
00001 
00002 /***************************************************************************
00003  *  cspace.cpp: Cached distance map
00004  *
00005  *  Created: Thu May 24 18:46:12 2012
00006  *  Copyright  2000  Brian Gerkey
00007  *             2000  Kasper Stoy
00008  *             2012  Tim Niemueller [www.niemueller.de]
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 /*  From:
00025  *  Player - One Hell of a Robot Server (LGPL)
00026  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00027  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00028  */
00029 
00030 #include <queue>
00031 #include <math.h>
00032 #include <stdlib.h>
00033 #include <string.h>
00034 #include "map.h"
00035 
00036 /// @cond EXTERNAL
00037 
00038 class CellData
00039 {
00040   public:
00041     map_t* map_;
00042     unsigned int i_, j_;
00043     unsigned int src_i_, src_j_;
00044 };
00045 
00046 class CachedDistanceMap
00047 {
00048   public:
00049     CachedDistanceMap(double scale, double max_dist) : 
00050       distances_(NULL), scale_(scale), max_dist_(max_dist) 
00051     {
00052       cell_radius_ = max_dist / scale;
00053       distances_ = new double *[cell_radius_+2];
00054       for(int i=0; i<=cell_radius_+1; i++)
00055       {
00056         distances_[i] = new double[cell_radius_+2];
00057         for(int j=0; j<=cell_radius_+1; j++)
00058         {
00059           distances_[i][j] = sqrt(i*i + j*j);
00060         }
00061       }
00062     }
00063     ~CachedDistanceMap()
00064     {
00065       if(distances_)
00066       {
00067         for(int i=0; i<=cell_radius_+1; i++)
00068           delete[] distances_[i];
00069         delete[] distances_;
00070       }
00071     }
00072     double** distances_;
00073     double scale_;
00074     double max_dist_;
00075     int cell_radius_;
00076 };
00077 
00078 
00079 bool operator<(const CellData& a, const CellData& b)
00080 {
00081   return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
00082 }
00083 
00084 CachedDistanceMap*
00085 get_distance_map(double scale, double max_dist)
00086 {
00087   static CachedDistanceMap* cdm = NULL;
00088 
00089   if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist))
00090   {
00091     if(cdm)
00092       delete cdm;
00093     cdm = new CachedDistanceMap(scale, max_dist);
00094   }
00095 
00096   return cdm;
00097 }
00098 
00099 void enqueue(map_t* map, unsigned int i, unsigned int j, 
00100              unsigned int src_i, unsigned int src_j,
00101              std::priority_queue<CellData>& Q,
00102              CachedDistanceMap* cdm,
00103              unsigned char* marked)
00104 {
00105   if(marked[MAP_INDEX(map, i, j)])
00106     return;
00107 
00108   unsigned int di = abs(i - src_i);
00109   unsigned int dj = abs(j - src_j);
00110   double distance = cdm->distances_[di][dj];
00111 
00112   if(distance > cdm->cell_radius_)
00113     return;
00114 
00115   map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
00116 
00117   CellData cell;
00118   cell.map_ = map;
00119   cell.i_ = i;
00120   cell.j_ = j;
00121   cell.src_i_ = src_i;
00122   cell.src_j_ = src_j;
00123 
00124   Q.push(cell);
00125 
00126   marked[MAP_INDEX(map, i, j)] = 1;
00127 }
00128 
00129 // Update the cspace distance values
00130 void map_update_cspace(map_t *map, double max_occ_dist)
00131 {
00132   unsigned char* marked;
00133   std::priority_queue<CellData> Q;
00134 
00135   marked = new unsigned char[map->size_x*map->size_y];
00136   memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);
00137 
00138   map->max_occ_dist = max_occ_dist;
00139 
00140   CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
00141 
00142   // Enqueue all the obstacle cells
00143   CellData cell;
00144   cell.map_ = map;
00145   for(int i=0; i<map->size_x; i++)
00146   {
00147     cell.src_i_ = cell.i_ = i;
00148     for(int j=0; j<map->size_y; j++)
00149     {
00150       if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
00151       {
00152         map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
00153         cell.src_j_ = cell.j_ = j;
00154         marked[MAP_INDEX(map, i, j)] = 1;
00155         Q.push(cell);
00156       }
00157       else
00158         map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
00159     }
00160   }
00161 
00162   while(!Q.empty())
00163   {
00164     CellData current_cell = Q.top();
00165     if(current_cell.i_ > 0)
00166       enqueue(map, current_cell.i_-1, current_cell.j_, 
00167               current_cell.src_i_, current_cell.src_j_,
00168               Q, cdm, marked);
00169     if(current_cell.j_ > 0)
00170       enqueue(map, current_cell.i_, current_cell.j_-1, 
00171               current_cell.src_i_, current_cell.src_j_,
00172               Q, cdm, marked);
00173     if((int)current_cell.i_ < map->size_x - 1)
00174       enqueue(map, current_cell.i_+1, current_cell.j_, 
00175               current_cell.src_i_, current_cell.src_j_,
00176               Q, cdm, marked);
00177     if((int)current_cell.j_ < map->size_y - 1)
00178       enqueue(map, current_cell.i_, current_cell.j_+1, 
00179               current_cell.src_i_, current_cell.src_j_,
00180               Q, cdm, marked);
00181 
00182     Q.pop();
00183   }
00184 
00185   delete[] marked;
00186 }
00187 
00188 #if 0
00189 // TODO: replace this with a more efficient implementation.  Not crucial,
00190 // because we only do it once, at startup.
00191 void map_update_cspace(map_t *map, double max_occ_dist)
00192 {
00193   int i, j;
00194   int ni, nj;
00195   int s;
00196   double d;
00197   map_cell_t *cell, *ncell;
00198 
00199   map->max_occ_dist = max_occ_dist;
00200   s = (int) ceil(map->max_occ_dist / map->scale);
00201 
00202   // Reset the distance values
00203   for (j = 0; j < map->size_y; j++)
00204   {
00205     for (i = 0; i < map->size_x; i++)
00206     {
00207       cell = map->cells + MAP_INDEX(map, i, j);
00208       cell->occ_dist = map->max_occ_dist;
00209     }
00210   }
00211 
00212   // Find all the occupied cells and update their neighbours
00213   for (j = 0; j < map->size_y; j++)
00214   {
00215     for (i = 0; i < map->size_x; i++)
00216     {
00217       cell = map->cells + MAP_INDEX(map, i, j);
00218       if (cell->occ_state != +1)
00219         continue;
00220           
00221       cell->occ_dist = 0;
00222 
00223       // Update adjacent cells
00224       for (nj = -s; nj <= +s; nj++)
00225       {
00226         for (ni = -s; ni <= +s; ni++)
00227         {
00228           if (!MAP_VALID(map, i + ni, j + nj))
00229             continue;
00230 
00231           ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
00232           d = map->scale * sqrt(ni * ni + nj * nj);
00233 
00234           if (d < ncell->occ_dist)
00235             ncell->occ_dist = d;
00236         }
00237       }
00238     }
00239   }
00240   
00241   return;
00242 }
00243 #endif
00244 
00245 /// @endcond