map_range.c
Go to the documentation of this file.
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
4  * gerkey@usc.edu kaspers@robotics.usc.edu
5  *
6  * This library is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * This library is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this library; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  *
20  */
21 /**************************************************************************
22  * Desc: Range routines
23  * Author: Andrew Howard
24  * Date: 18 Jan 2003
25  * CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $
26 **************************************************************************/
27 
28 #include <assert.h>
29 #include <math.h>
30 #include <string.h>
31 #include <stdlib.h>
32 
33 #include "amcl/map/map.h"
34 
35 // Extract a single range reading from the map. Unknown cells and/or
36 // out-of-bound cells are treated as occupied, which makes it easy to
37 // use Stage bitmap files.
38 double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
39 {
40  // Bresenham raytracing
41  int x0,x1,y0,y1;
42  int x,y;
43  int xstep, ystep;
44  char steep;
45  int tmp;
46  int deltax, deltay, error, deltaerr;
47 
48  x0 = MAP_GXWX(map,ox);
49  y0 = MAP_GYWY(map,oy);
50 
51  x1 = MAP_GXWX(map,ox + max_range * cos(oa));
52  y1 = MAP_GYWY(map,oy + max_range * sin(oa));
53 
54  if(abs(y1-y0) > abs(x1-x0))
55  steep = 1;
56  else
57  steep = 0;
58 
59  if(steep)
60  {
61  tmp = x0;
62  x0 = y0;
63  y0 = tmp;
64 
65  tmp = x1;
66  x1 = y1;
67  y1 = tmp;
68  }
69 
70  deltax = abs(x1-x0);
71  deltay = abs(y1-y0);
72  error = 0;
73  deltaerr = deltay;
74 
75  x = x0;
76  y = y0;
77 
78  if(x0 < x1)
79  xstep = 1;
80  else
81  xstep = -1;
82  if(y0 < y1)
83  ystep = 1;
84  else
85  ystep = -1;
86 
87  if(steep)
88  {
89  if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
90  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
91  }
92  else
93  {
94  if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
95  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
96  }
97 
98  while(x != (x1 + xstep * 1))
99  {
100  x += xstep;
101  error += deltaerr;
102  if(2*error >= deltax)
103  {
104  y += ystep;
105  error -= deltax;
106  }
107 
108  if(steep)
109  {
110  if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
111  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
112  }
113  else
114  {
115  if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
116  return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
117  }
118  }
119  return max_range;
120 }
#define MAP_GXWX(map, x)
Definition: map.h:137
#define MAP_VALID(map, i, j)
Definition: map.h:141
map_cell_t * cells
Definition: map.h:73
#define MAP_INDEX(map, i, j)
Definition: map.h:144
double scale
Definition: map.h:67
#define MAP_GYWY(map, y)
Definition: map.h:138
TFSIMD_FORCE_INLINE const tfScalar & y() const
Definition: map.h:61
TFSIMD_FORCE_INLINE const tfScalar & x() const
double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
Definition: map_range.c:38


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:36