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 tmp;
54  int deltax, deltay, error, deltaerr;
55 
56  x0 = MAP_GXWX(map,ox);
57  y0 = MAP_GYWY(map,oy);
58 
59  x1 = MAP_GXWX(map,ox + max_range * cos(oa));
60  y1 = MAP_GYWY(map,oy + max_range * sin(oa));
61 
62  if(abs(y1-y0) > abs(x1-x0))
63  steep = 1;
64  else
65  steep = 0;
66 
67  if(steep)
68  {
69  tmp = x0;
70  x0 = y0;
71  y0 = tmp;
72 
73  tmp = x1;
74  x1 = y1;
75  y1 = tmp;
76  }
77 
78  deltax = abs(x1-x0);
79  deltay = abs(y1-y0);
80  error = 0;
81  deltaerr = deltay;
82 
83  x = x0;
84  y = y0;
85 
86  if(x0 < x1)
87  xstep = 1;
88  else
89  xstep = -1;
90  if(y0 < y1)
91  ystep = 1;
92  else
93  ystep = -1;
94 
95  if(steep)
96  {
97  if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
98  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
99  }
100  else
101  {
102  if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
103  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
104  }
105 
106  while(x != (x1 + xstep * 1))
107  {
108  x += xstep;
109  error += deltaerr;
110  if(2*error >= deltax)
111  {
112  y += ystep;
113  error -= deltax;
114  }
115 
116  if(steep)
117  {
118  if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
119  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
120  }
121  else
122  {
123  if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
124  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
125  }
126  }
127  return max_range;
128 }
129 
130 /// @endcond