2 * Copyright Droids Corporation (2007)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: obstacle_avoidance.c,v 1.1.2.8 2009-05-02 10:00:35 zer0 Exp $
20 * Main code and algorithm: Fabrice DESCLAUX <serpilliere@droids-corp.org>
21 * Integration in Aversive: Olivier MATZ <zer0@droids-corp.org>
25 #include <aversive/error.h>
32 #include <vect_base.h>
36 #include <obstacle_avoidance.h>
38 #define GET_PT(a) (&(a) - &(oa.points[0]))
40 static struct obstacle_avoidance oa;
42 static void __oa_start_end_points(int32_t st_x, int32_t st_y,
43 int32_t en_x, int32_t en_y);
45 /* reset oa without reseting points coord */
48 DEBUG(E_OA, "%s()", __FUNCTION__);
50 memset(oa.valid, 0, sizeof(oa.valid));
51 memset(oa.pweight, 0, sizeof(oa.pweight));
52 memset(oa.weight, 0, sizeof(oa.weight));
53 memset(oa.p, 0, sizeof(oa.p));
54 memset(oa.pt, 0, sizeof(oa.pt));
59 /** Init the oa structure. Note: In the algorithm, the first polygon
60 * is a dummy one, and is used to represent the START and END points
61 * (so it has 2 vertices) */
64 DEBUG(E_OA, "%s()", __FUNCTION__);
65 memset(&oa, 0, sizeof(oa));
67 /* set a default start and point, reserve the first poly and
68 * the first 2 points for it */
69 oa.polys[0].pts = oa.points;
71 __oa_start_end_points(0, 0, 100, 100);
77 * Set the start and destination point. Return 0 on sucess
79 static void __oa_start_end_points(int32_t st_x, int32_t st_y,
80 int32_t en_x, int32_t en_y)
82 /* we always use the first 2 points of the table for start and end */
83 oa.points[0].x = en_x;
84 oa.points[0].y = en_y;
86 /* Each point processed by Dijkstra is marked as valid. If we
87 * have unreachable points (out of playground or points inside
88 * polygons) Disjkstra won't mark them as valid. At the end of
89 * the algorithm, if the destination point is not marked as
90 * valid, there's no valid path to reach it. */
92 oa.valid[GET_PT(oa.points[0])] = 0;
93 /* the real dest is the start point for the algorithm */
94 oa.pweight[GET_PT(oa.points[0])] = 1;
96 oa.points[1].x = st_x;
97 oa.points[1].y = st_y;
98 oa.valid[GET_PT(oa.points[1])] = 0;
99 oa.pweight[GET_PT(oa.points[1])] = 0;
103 * Set the start and destination point. Return 0 on sucess
105 void oa_start_end_points(int32_t st_x, int32_t st_y,
106 int32_t en_x, int32_t en_y)
108 DEBUG(E_OA, "%s() (%ld,%ld) (%ld,%ld)", __FUNCTION__,
109 st_x, st_y, en_x, en_y);
111 __oa_start_end_points(st_x, st_y, en_x, en_y);
116 * Create a new obstacle polygon. Return NULL on error.
118 poly_t *oa_new_poly(uint8_t size)
120 DEBUG(E_OA, "%s(size=%d)", __FUNCTION__, size);
122 if (oa.cur_pt_idx + size > MAX_PTS)
124 if (oa.cur_poly_idx + 1 > MAX_POLY)
127 oa.polys[oa.cur_poly_idx].l = size;
128 oa.polys[oa.cur_poly_idx].pts = &oa.points[oa.cur_pt_idx];
129 oa.cur_pt_idx += size;
131 return &oa.polys[oa.cur_poly_idx++];
135 * Add a point to the polygon.
137 void oa_poly_set_point(poly_t *pol,
138 int32_t x, int32_t y, uint8_t i)
140 DEBUG(E_OA, "%s() (%ld,%ld)", __FUNCTION__, x, y);
144 oa.valid[GET_PT(pol->pts[i])] = 0;
145 oa.pweight[GET_PT(pol->pts[i])] = 0;
148 point_t * oa_get_path(void)
159 printf_P(PSTR("-- OA dump --\r\n"));
160 printf_P(PSTR("nb_polys: %d\r\n"), oa.cur_poly_idx);
161 printf_P(PSTR("nb_pts: %d\r\n"), oa.cur_pt_idx);
162 for (i=0; i<oa.cur_poly_idx; i++) {
164 printf_P(PSTR("poly #%d\r\n"), i);
165 for (j=0; j<poly->l; j++) {
167 printf_P(PSTR(" pt #%d (%2.2f,%2.2f)\r\n"), j, pt->x, pt->y);
172 /* Iterative Dijkstra algorithm: The valid filed is used to determine if:
173 * 1: this point has been visited, his weight is correct.
174 * 2: the point must be visited.
176 * The algorithm does: find a point that must be visited (2) update
177 * his weight, mark it as (1) and update all his neightbours has 2.
179 * The algorithm ends when no (2) points are found
181 * A point with weight 0 is a point that has not been visited (so
182 * weight is not affected yet); This explain why first point must have
183 * a start weight different than 0.
185 * When the algo finds a shorter path to reach a point B from point A,
186 * it will store in (p, pt) the parent point. This is important to
187 * remenber and extract the solution path. */
189 dijkstra(uint8_t start_p, uint8_t start)
194 /* weight == 0 means not visited */
195 /* weight == 1 for start */
197 /* find all point linked to start */
199 oa.valid[GET_PT(oa.polys[start_p].pts[start])] = 2;
204 for (start_p = 0;start_p<MAX_POLY;start_p++) {
205 for (start = 0;start<oa.polys[start_p].l;start++) {
206 if (oa.valid[GET_PT(oa.polys[start_p].pts[start])] != 2)
210 /* For all points that must be
211 * visited, we look for rays that
212 * start or stop at this point. As
213 * ray array is (poly_num, point_num)
215 for (i=0 ; i<oa.ray_n ; i+=2) {
217 /* If index is even in the
218 * aray, we have a start point
219 * ray, so connected point is
222 * If index is odd, we are in stop
223 * point and ray start point is at
227 if (start_p != oa.u.rays[i] || start != oa.u.rays[i+1])
230 if ((oa.pweight[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])] != 0) &&
231 (oa.pweight[GET_PT(oa.polys[start_p].pts[start])]+oa.weight[i/4] >=
232 oa.pweight[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])]))
235 oa.p[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])] = start_p;
236 oa.pt[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])] = start;
237 oa.valid[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])]=2;
238 oa.pweight[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])] =
239 oa.pweight[GET_PT(oa.polys[start_p].pts[start])]+oa.weight[i/4];
241 oa.valid[GET_PT(oa.polys[start_p].pts[start])] = 1;
243 DEBUG(E_OA, "%s() (%ld,%ld p=%ld) %ld (%ld,%ld p=%ld)\r\n", __FUNCTION__,
244 oa.polys[start_p].pts[start].x, oa.polys[start_p].pts[start].y,
245 oa.pweight[GET_PT(oa.polys[start_p].pts[start])],oa.weight[i/4],
247 oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]].x,
248 oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]].y,
249 oa.pweight[GET_PT(oa.polys[oa.u.rays[i+add]].pts[oa.u.rays[i+add+1]])]
258 /* display the path */
260 get_path(poly_t *polys, uint8_t *rays)
262 uint8_t p, pt, p1, pt1, i;
268 /* forget the first point */
270 while (!(p==0 && pt==0)) {
272 if (i>=MAX_CHKPOINTS)
275 if (oa.valid[GET_PT(polys[p].pts[pt])]==0) {
276 DEBUG(E_OA, "invalid path!");
280 p1 = oa.p[GET_PT(polys[p].pts[pt])];
281 pt1 = oa.pt[GET_PT(polys[p].pts[pt])];
283 oa.u.res[i].x = polys[p].pts[pt].x;
284 oa.u.res[i].y = polys[p].pts[pt].y;
285 DEBUG(E_OA, "result[%d]: %d, %d", i, oa.u.res[i].x, oa.u.res[i].y);
298 /* First we compute the visibility graph */
299 ret = calc_rays(oa.polys, oa.cur_poly_idx, oa.u.rays);
300 DEBUG(E_OA, "nb_rays = %d", ret);
302 DEBUG(E_OA, "Ray list");
303 for (i=0;i<ret;i+=4) {
304 DEBUG(E_OA, "%d,%d -> %d,%d", oa.u.rays[i], oa.u.rays[i+1],
305 oa.u.rays[i+2], oa.u.rays[i+3]);
308 /* Then we affect the rays lengths to their weights */
309 calc_rays_weight(oa.polys, oa.cur_poly_idx,
310 oa.u.rays, ret, oa.weight);
312 DEBUG(E_OA, "Ray weights");
313 for (i=0;i<ret;i+=4) {
314 DEBUG(E_OA, "%d,%d -> %d,%d (%d)",
315 (int)oa.polys[oa.u.rays[i]].pts[oa.u.rays[i+1]].x,
316 (int)oa.polys[oa.u.rays[i]].pts[oa.u.rays[i+1]].y,
317 (int)oa.polys[oa.u.rays[i+2]].pts[oa.u.rays[i+3]].x,
318 (int)oa.polys[oa.u.rays[i+2]].pts[oa.u.rays[i+3]].y,
322 /* We aplly dijkstra on the visibility graph from the start
323 * point (point 0 of the polygon 0) */
325 DEBUG(E_OA, "dijkstra ray_n = %d", ret);
328 /* As dijkstra sets the parent points in the resulting graph,
329 * we can backtrack the solution path. */
330 return get_path(oa.polys, oa.u.rays);