map_range.c
Go to the documentation of this file.
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00004  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00005  *
00006  *  This library is free software; you can redistribute it and/or
00007  *  modify it under the terms of the GNU Lesser General Public
00008  *  License as published by the Free Software Foundation; either
00009  *  version 2.1 of the License, or (at your option) any later version.
00010  *
00011  *  This library is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  *  Lesser General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU Lesser General Public
00017  *  License along with this library; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /**************************************************************************
00022  * Desc: Range routines
00023  * Author: Andrew Howard
00024  * Date: 18 Jan 2003
00025  * CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $
00026 **************************************************************************/
00027 
00028 #include <assert.h>
00029 #include <math.h>
00030 #include <string.h>
00031 #include <stdlib.h>
00032 
00033 #include "nav2d_localizer/map.h"
00034 
00035 // Extract a single range reading from the map.  Unknown cells and/or
00036 // out-of-bound cells are treated as occupied, which makes it easy to
00037 // use Stage bitmap files.
00038 double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
00039 {
00040   // Bresenham raytracing
00041   int x0,x1,y0,y1;
00042   int x,y;
00043   int xstep, ystep;
00044   char steep;
00045   int tmp;
00046   int deltax, deltay, error, deltaerr;
00047 
00048   x0 = MAP_GXWX(map,ox);
00049   y0 = MAP_GYWY(map,oy);
00050   
00051   x1 = MAP_GXWX(map,ox + max_range * cos(oa));
00052   y1 = MAP_GYWY(map,oy + max_range * sin(oa));
00053 
00054   if(abs(y1-y0) > abs(x1-x0))
00055     steep = 1;
00056   else
00057     steep = 0;
00058 
00059   if(steep)
00060   {
00061     tmp = x0;
00062     x0 = y0;
00063     y0 = tmp;
00064 
00065     tmp = x1;
00066     x1 = y1;
00067     y1 = tmp;
00068   }
00069 
00070   deltax = abs(x1-x0);
00071   deltay = abs(y1-y0);
00072   error = 0;
00073   deltaerr = deltay;
00074 
00075   x = x0;
00076   y = y0;
00077 
00078   if(x0 < x1)
00079     xstep = 1;
00080   else
00081     xstep = -1;
00082   if(y0 < y1)
00083     ystep = 1;
00084   else
00085     ystep = -1;
00086 
00087   if(steep)
00088   {
00089     if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
00090       return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
00091   }
00092   else
00093   {
00094     if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
00095       return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
00096   }
00097 
00098   while(x != (x1 + xstep * 1))
00099   {
00100     x += xstep;
00101     error += deltaerr;
00102     if(2*error >= deltax)
00103     {
00104       y += ystep;
00105       error -= deltax;
00106     }
00107 
00108     if(steep)
00109     {
00110       if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
00111         return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
00112     }
00113     else
00114     {
00115       if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
00116         return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
00117     }
00118   }
00119   return max_range;
00120 }


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 04:05:16