config: update test config files to current aversive config.in
[aversive.git] / modules / devices / robot / obstacle_avoidance / obstacle_avoidance.c
1 /*  
2  *  Copyright Droids Corporation (2007)
3  * 
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.
8  *
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.
13  *
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
17  *
18  *  Revision : $Id: obstacle_avoidance.c,v 1.1.2.8 2009-05-02 10:00:35 zer0 Exp $
19  *
20  *  Main code and algorithm: Fabrice DESCLAUX <serpilliere@droids-corp.org>
21  *  Integration in Aversive: Olivier MATZ <zer0@droids-corp.org>
22  */
23
24 #include <aversive.h>
25 #include <aversive/error.h>
26
27 #include <math.h>
28 #include <stdio.h>
29 #include <stdlib.h>
30 #include <string.h>
31
32 #include <vect_base.h>
33 #include <lines.h>
34 #include <polygon.h>
35
36 #include <obstacle_avoidance.h>
37
38 #define GET_PT(a) (&(a) - &(oa.points[0]))
39
40 static struct obstacle_avoidance oa;
41
42 static void __oa_start_end_points(int32_t st_x, int32_t st_y,
43                                   int32_t en_x, int32_t en_y);
44
45 /* reset oa without reseting points coord */
46 void oa_reset(void)
47 {
48         DEBUG(E_OA, "%s()", __FUNCTION__);
49
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));
55
56         
57 }
58
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) */
62 void oa_init(void)
63 {
64         DEBUG(E_OA, "%s()", __FUNCTION__);
65         memset(&oa, 0, sizeof(oa));
66         
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;
70         oa.polys[0].l = 2;
71         __oa_start_end_points(0, 0, 100, 100);
72         oa.cur_pt_idx = 2;
73         oa.cur_poly_idx = 1;
74 }
75
76 /** 
77  * Set the start and destination point. Return 0 on sucess
78  */
79 static void __oa_start_end_points(int32_t st_x, int32_t st_y,
80                                   int32_t en_x, int32_t en_y)
81 {
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;
85
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. */
91
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;
95
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;
100 }
101
102 /** 
103  * Set the start and destination point. Return 0 on sucess
104  */
105 void oa_start_end_points(int32_t st_x, int32_t st_y,
106                          int32_t en_x, int32_t en_y)
107 {
108         DEBUG(E_OA, "%s() (%ld,%ld) (%ld,%ld)", __FUNCTION__,
109               st_x, st_y, en_x, en_y);
110
111         __oa_start_end_points(st_x, st_y, en_x, en_y);
112 }
113
114
115 /**
116  * Create a new obstacle polygon. Return NULL on error.
117  */
118 poly_t *oa_new_poly(uint8_t size)
119 {
120         DEBUG(E_OA, "%s(size=%d)", __FUNCTION__, size);
121
122         if (oa.cur_pt_idx + size > MAX_PTS)
123                 return NULL;
124         if (oa.cur_poly_idx + 1 > MAX_POLY)
125                 return NULL;
126
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;
130
131         return &oa.polys[oa.cur_poly_idx++];
132 }
133
134 /**
135  * Add a point to the polygon.
136  */
137 void oa_poly_set_point(poly_t *pol, 
138                          int32_t x, int32_t y, uint8_t i)
139 {
140         DEBUG(E_OA, "%s() (%ld,%ld)", __FUNCTION__, x, y);
141         
142         pol->pts[i].x = x;
143         pol->pts[i].y = y;
144         oa.valid[GET_PT(pol->pts[i])] = 0;
145         oa.pweight[GET_PT(pol->pts[i])] = 0;
146 }
147
148 point_t * oa_get_path(void)
149 {
150         return oa.u.res;
151 }
152
153 void oa_dump(void)
154 {
155         uint8_t i,j;
156         poly_t *poly;
157         point_t *pt;
158
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++) {
163                 poly = &oa.polys[i];
164                 printf_P(PSTR("poly #%d\r\n"), i);
165                 for (j=0; j<poly->l; j++) {
166                         pt = &poly->pts[j];
167                         printf_P(PSTR("  pt #%d (%2.2f,%2.2f)\r\n"), j, pt->x, pt->y);
168                 }
169         }
170 }
171
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.
175  *
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.
178  *
179  * The algorithm ends when no (2) points are found
180  *
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.
184  *
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. */
188 void 
189 dijkstra(uint8_t start_p, uint8_t start)
190 {
191         uint8_t i;
192         int8_t add;
193         int8_t finish = 0;
194         /* weight == 0 means not visited */
195         /* weight == 1 for start */
196         
197         /* find all point linked to start */
198
199         oa.valid[GET_PT(oa.polys[start_p].pts[start])] = 2;
200
201         while (!finish){
202                 finish = 1;
203
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)
207                                         continue;
208                                 add = -2;               
209
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)
214                                  * we wtep 2 by 2. */
215                                 for (i=0 ; i<oa.ray_n ; i+=2) {
216
217                                         /* If index is even in the
218                                          * aray, we have a start point
219                                          * ray, so connected point is
220                                          * i+2;
221                                          *
222                                          * If index is odd, we are in stop
223                                          * point and ray start point is at
224                                          * i-2 pos */
225                                         add = -add;                                     
226                                         
227                                         if (start_p != oa.u.rays[i] || start != oa.u.rays[i+1])
228                                                 continue;
229
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]])]))
233                                                 continue;
234                                         
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];
240                                                         
241                                         oa.valid[GET_PT(oa.polys[start_p].pts[start])] = 1;
242                                         finish = 0;                             
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],
246                                               
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]])]
250                                               );
251                                 }
252                         }
253                 }
254         }
255 }
256
257
258 /* display the path */
259 int8_t
260 get_path(poly_t *polys, uint8_t *rays)
261 {
262         uint8_t p, pt, p1, pt1, i;
263
264         p=0;
265         pt=1;
266         i=0;
267         
268         /* forget the first point */
269         
270         while (!(p==0 && pt==0)) {
271
272                 if (i>=MAX_CHKPOINTS)
273                         return -1;
274
275                 if (oa.valid[GET_PT(polys[p].pts[pt])]==0) {
276                         DEBUG(E_OA, "invalid path!");
277                         return -1;
278                 }
279
280                 p1 = oa.p[GET_PT(polys[p].pts[pt])];
281                 pt1 =  oa.pt[GET_PT(polys[p].pts[pt])];
282                 p = p1; pt = pt1;
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);
286                 i++;
287         }
288         
289         return i;
290 }
291
292 int8_t 
293 oa_process(void)
294 {
295         uint8_t ret;
296         uint8_t i;
297
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);
301
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]);
306         }
307         
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);
311         
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,
319                        oa.weight[i/4]);
320         }
321         
322         /* We aplly dijkstra on the visibility graph from the start
323          * point (point 0 of the polygon 0) */
324         oa.ray_n = ret;
325         DEBUG(E_OA, "dijkstra ray_n = %d", ret);
326         dijkstra(0, 0);
327
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);
331 }