Fawkes API  Fawkes Development Version
map_range.c
00001 
00002 /***************************************************************************
00003  *  map_range.c: Range routines
00004  *
00005  *  Created: Thu May 24 18:48:02 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  * Desc: Range routines
00031  * Author: Andrew Howard
00032  * Date: 18 Jan 2003
00033 **************************************************************************/
00034 
00035 #include <math.h>
00036 #include <string.h>
00037 #include <stdlib.h>
00038 
00039 #include "map.h"
00040 
00041 /// @cond EXTERNAL
00042 
00043 // Extract a single range reading from the map.  Unknown cells and/or
00044 // out-of-bound cells are treated as occupied, which makes it easy to
00045 // use Stage bitmap files.
00046 double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
00047 {
00048   // Bresenham raytracing
00049   int x0,x1,y0,y1;
00050   int x,y;
00051   int xstep, ystep;
00052   char steep;
00053   int tmp;
00054   int deltax, deltay, error, deltaerr;
00055 
00056   x0 = MAP_GXWX(map,ox);
00057   y0 = MAP_GYWY(map,oy);
00058   
00059   x1 = MAP_GXWX(map,ox + max_range * cos(oa));
00060   y1 = MAP_GYWY(map,oy + max_range * sin(oa));
00061 
00062   if(abs(y1-y0) > abs(x1-x0))
00063     steep = 1;
00064   else
00065     steep = 0;
00066 
00067   if(steep)
00068   {
00069     tmp = x0;
00070     x0 = y0;
00071     y0 = tmp;
00072 
00073     tmp = x1;
00074     x1 = y1;
00075     y1 = tmp;
00076   }
00077 
00078   deltax = abs(x1-x0);
00079   deltay = abs(y1-y0);
00080   error = 0;
00081   deltaerr = deltay;
00082 
00083   x = x0;
00084   y = y0;
00085 
00086   if(x0 < x1)
00087     xstep = 1;
00088   else
00089     xstep = -1;
00090   if(y0 < y1)
00091     ystep = 1;
00092   else
00093     ystep = -1;
00094 
00095   if(steep)
00096   {
00097     if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
00098       return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
00099   }
00100   else
00101   {
00102     if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
00103       return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
00104   }
00105 
00106   while(x != (x1 + xstep * 1))
00107   {
00108     x += xstep;
00109     error += deltaerr;
00110     if(2*error >= deltax)
00111     {
00112       y += ystep;
00113       error -= deltax;
00114     }
00115 
00116     if(steep)
00117     {
00118       if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
00119         return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
00120     }
00121     else
00122     {
00123       if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
00124         return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
00125     }
00126   }
00127   return max_range;
00128 }
00129 
00130 /// @endcond