Fawkes API  Fawkes Development Version
map_range.c
1 
2 /***************************************************************************
3  * map_range.c: Range routines
4  *
5  * Created: Thu May 24 18:48:02 2012
6  * Copyright 2000 Brian Gerkey
7  * 2000 Kasper Stoy
8  * 2012 Tim Niemueller [www.niemueller.de]
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 /* From:
25  * Player - One Hell of a Robot Server (LGPL)
26  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27  * gerkey@usc.edu kaspers@robotics.usc.edu
28  */
29 /**************************************************************************
30  * Desc: Range routines
31  * Author: Andrew Howard
32  * Date: 18 Jan 2003
33 **************************************************************************/
34 
35 #include <math.h>
36 #include <string.h>
37 #include <stdlib.h>
38 
39 #include "map.h"
40 
41 /// @cond EXTERNAL
42 
43 // Extract a single range reading from the map. Unknown cells and/or
44 // out-of-bound cells are treated as occupied, which makes it easy to
45 // use Stage bitmap files.
46 double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
47 {
48  // Bresenham raytracing
49  int x0,x1,y0,y1;
50  int x,y;
51  int xstep, ystep;
52  char steep;
53  int deltax, deltay, error, deltaerr;
54 
55  x0 = MAP_GXWX(map,ox);
56  y0 = MAP_GYWY(map,oy);
57 
58  x1 = MAP_GXWX(map,ox + max_range * cos(oa));
59  y1 = MAP_GYWY(map,oy + max_range * sin(oa));
60 
61  if(abs(y1-y0) > abs(x1-x0))
62  steep = 1;
63  else
64  steep = 0;
65 
66  if(steep)
67  {
68  int tmp = x0;
69  x0 = y0;
70  y0 = tmp;
71 
72  tmp = x1;
73  x1 = y1;
74  y1 = tmp;
75  }
76 
77  deltax = abs(x1-x0);
78  deltay = abs(y1-y0);
79  error = 0;
80  deltaerr = deltay;
81 
82  x = x0;
83  y = y0;
84 
85  if(x0 < x1)
86  xstep = 1;
87  else
88  xstep = -1;
89  if(y0 < y1)
90  ystep = 1;
91  else
92  ystep = -1;
93 
94  if(steep)
95  {
96  if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
97  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
98  }
99  else
100  {
101  if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
102  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
103  }
104 
105  while(x != (x1 + xstep * 1))
106  {
107  x += xstep;
108  error += deltaerr;
109  if(2*error >= deltax)
110  {
111  y += ystep;
112  error -= deltax;
113  }
114 
115  if(steep)
116  {
117  if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
118  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
119  }
120  else
121  {
122  if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
123  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
124  }
125  }
126  return max_range;
127 }
128 
129 /// @endcond