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.
46double 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