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.h,v 1.1.2.7 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 * The algorithm is based on the "visible point" algorithm.
27 * - the play ground (basically the table, here a rectangle)
28 * - the objects to avoid, represented by polygones
29 * - start/stop points (A, B)
31 * The algorithm will first find every ray formed by 2 points that can
32 * "see" each others. Basically, if a polygon is between two points,
33 * they cannot see each others. A side of a polygon is composed by 2
34 * points that can se each others.
36 * From all these rays, we can create a graph. We affect for each ray
37 * a weight with its own length.
39 * The algorithm executes Dijkstra to find the shortest path to go
44 * As we run on 4Ko ram uC, we have static structures arrays to store:
45 * - MAX_POLY => represent the maximum polygons to avoid in the area.
46 * - MAX_PTS => maximize the sum of every polygons vertices.
47 * - MAX_RAYS => maximum number of rays.
48 * - MAX_CHKPOINTS => maximum accepted checkpoints in the resulting path.
49 * - PLAYGROUND XXX => dimensions of the playground.
52 #ifndef _OBSTACLE_AVOIDANCE_H_
53 #define _OBSTACLE_AVOIDANCE_H_
55 #include <obstacle_avoidance_config.h>
57 struct obstacle_avoidance {
58 poly_t polys[MAX_POLY]; /* tab of polygons (obstacles) */
59 point_t points[MAX_PTS]; /* tab of points, referenced by polys */
60 uint8_t valid[MAX_PTS];
61 int32_t pweight[MAX_PTS];
71 uint16_t weight[MAX_RAYS];
73 uint8_t rays[MAX_RAYS*2];
74 point_t res[MAX_CHKPOINTS];
78 /* To save memory space here is the moemory representation of
81 * We have an array of points (oa_ext_point_t points):
82 * _____ _____ _____ _____ _____ _____ _____ _____ _____
84 * | p0 | p1 | p0 | p1 | p2 | p3 | p0 | p1 | p2 |
85 * |_____|_____|_____|_____|_____|_____|_____|_____|_____|
90 * -polygon 0 -polygon 1 -polygon 2
91 * -2 vertices -4 vertices -3 vertices
94 * And each polygon is represented by the sub array starting with the
95 * point represented by oa_ext_point_t * pts and composed of uint8_t l;
96 * (in the oa_poly_t structure)
99 /* reset oa without cleaning points */
103 /** Init the oa structure */
107 * Set the start and destination point.
109 void oa_start_end_points(int32_t st_x, int32_t st_y, int32_t en_x, int32_t en_y);
112 * Create a new obstacle polygon. Return NULL on error.
114 poly_t *oa_new_poly(uint8_t size);
123 * set a point of the polygon.
125 void oa_poly_set_point(poly_t *pol, int32_t x, int32_t y, uint8_t i);
129 * process the path from start to end. Return 0 on sucess.
131 int8_t oa_process(void);
134 * get the result after a call to oa_process()
136 point_t * oa_get_path(void);
138 #endif /* _OBSTACLE_AVOIDANCE_H_ */