Fawkes API
Fawkes Development Version
|
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