From 72f865523b14953d13a207599068ba68e9df0b70 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 17 Jun 2008 22:57:24 +0200 Subject: * digital/avr/modules/path, digital/io/src: - moved path finding to a separated module. --- digital/avr/modules/path/Makefile.module | 1 + digital/avr/modules/path/README | 25 +++ digital/avr/modules/path/avrconfig.h | 38 ++++ digital/avr/modules/path/path.c | 351 ++++++++++++++++++++++++++++++ digital/avr/modules/path/path.h | 83 +++++++ digital/avr/modules/path/test/Makefile | 12 + digital/avr/modules/path/test/avrconfig.h | 39 ++++ digital/avr/modules/path/test/test_path.c | 52 +++++ 8 files changed, 601 insertions(+) create mode 100644 digital/avr/modules/path/Makefile.module create mode 100644 digital/avr/modules/path/README create mode 100644 digital/avr/modules/path/avrconfig.h create mode 100644 digital/avr/modules/path/path.c create mode 100644 digital/avr/modules/path/path.h create mode 100644 digital/avr/modules/path/test/Makefile create mode 100644 digital/avr/modules/path/test/avrconfig.h create mode 100644 digital/avr/modules/path/test/test_path.c (limited to 'digital/avr') diff --git a/digital/avr/modules/path/Makefile.module b/digital/avr/modules/path/Makefile.module new file mode 100644 index 00000000..9d537233 --- /dev/null +++ b/digital/avr/modules/path/Makefile.module @@ -0,0 +1 @@ +path_SOURCES = path.c diff --git a/digital/avr/modules/path/README b/digital/avr/modules/path/README new file mode 100644 index 00000000..618e4b79 --- /dev/null +++ b/digital/avr/modules/path/README @@ -0,0 +1,25 @@ +avr.path - Path finding module. + +Path finding among obstacles represented as circles. See modules README for +more details about AVR modules. + + +Copyright (C) 2008 Nicolas Schodet + +APBTeam: + Web: http://apbteam.org/ + Email: team AT apbteam DOT org + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. diff --git a/digital/avr/modules/path/avrconfig.h b/digital/avr/modules/path/avrconfig.h new file mode 100644 index 00000000..9f25d772 --- /dev/null +++ b/digital/avr/modules/path/avrconfig.h @@ -0,0 +1,38 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - Path module configuration template. */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/* path - Path finding module. */ +/** Report path found for debug. */ +#define AC_PATH_REPORT defined (HOST) +/** Report function name. */ +#define AC_PATH_REPORT_CALLBACK path_report +/** Number of possible obstacles. */ +#define AC_PATH_OBSTACLES_NB 2 +/** Number of points per obstacle. */ +#define AC_PATH_OBSTACLES_POINTS_NB 8 + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/path/path.c b/digital/avr/modules/path/path.c new file mode 100644 index 00000000..30cc03f8 --- /dev/null +++ b/digital/avr/modules/path/path.c @@ -0,0 +1,351 @@ +/* path.c - Find a path between obstables. */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" + +#include "modules/math/fixed/fixed.h" +#include "modules/utils/utils.h" + +#include "path.h" + +#ifdef HOST +# include +#endif + +/** Number of possible obstacles. */ +#define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB + +/** Number of points per obstacle. */ +#define PATH_OBSTACLES_POINTS_NB AC_PATH_OBSTACLES_POINTS_NB + +/** Angle between obstacles points. */ +#define PATH_OBSTACLES_POINTS_ANGLE ((1L << 24) / PATH_OBSTACLES_POINTS_NB) + +/** Number of points. */ +#define PATH_POINTS_NB (2 + PATH_OBSTACLES_NB * PATH_OBSTACLES_POINTS_NB) + +/** Point with extra data for Dijkstra algorithm. */ +struct path_point_t +{ + /** Coordinates. */ + int16_t x, y; + /** Weight. */ + uint16_t w; + /** Next point (preceding in the usual Dijkstra meaning). */ + uint8_t next; + /** Is this point taken yet? */ + uint8_t taken; +}; + +/** Path finding context. */ +struct path_t +{ + /** Borders, any point outside borders is eliminated. */ + int16_t border_xmin, border_ymin, border_xmax, border_ymax; + /** Complex number used to position obstacles points. + * The obstacles points are placed evenly on a circle around the center + * point. Complex multiplication is used to make a point rotate around + * the center point. */ + uint32_t rot_a, rot_b; + /** Margin to take into account approximation of the circle. */ + uint32_t margin; + /** List of obstacles. */ + struct path_obstacle_t obstacles[PATH_OBSTACLES_NB]; + /** List of points. First two points are the destination and source + * points. Then comes the obstacles points. */ + struct path_point_t points[PATH_POINTS_NB]; + /** Number of points. */ + uint8_t points_nb; + /** Connection matrix, value is arc weight, 0xffff for no connection. */ + uint16_t arcs[PATH_POINTS_NB][PATH_POINTS_NB]; +}; + +/** Global context. */ +struct path_t path; + +/** Initialise path finder. */ +void +path_init (int16_t border_xmin, int16_t border_ymin, + int16_t border_xmax, int16_t border_ymax) +{ + /** Save borders. */ + path.border_xmin = border_xmin; + path.border_ymin = border_ymin; + path.border_xmax = border_xmax; + path.border_ymax = border_ymax; + /** Complex number of modulus 1 and rotation angle. */ + path.rot_a = fixed_cos_f824 (PATH_OBSTACLES_POINTS_ANGLE); + path.rot_b = fixed_sin_f824 (PATH_OBSTACLES_POINTS_ANGLE); + path.margin = + fixed_div_f824 (0x11L << 20, + fixed_cos_f824 (PATH_OBSTACLES_POINTS_ANGLE / 2)); +} + +/** Compute points from obstacles. */ +static void +path_compute_points (void) +{ + /* Do not touch first two points, they are destination and source. */ + uint8_t i, j, p = 2; + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid) + { + /* Compute obstacle points positions around a circle. */ + uint32_t x, y, nx; + x = path.obstacles[i].r; y = 0; + x = fixed_mul_f824 (x, path.margin); + path.points[p].x = path.obstacles[i].x + (uint16_t) x; + path.points[p].y = path.obstacles[i].y; + p++; + for (j = 0; j < PATH_OBSTACLES_POINTS_NB - 1; j++) + { + nx = fixed_mul_f824 (x, path.rot_a) + - fixed_mul_f824 (y, path.rot_b); + y = fixed_mul_f824 (y, path.rot_a) + + fixed_mul_f824 (x, path.rot_b); + x = nx; + path.points[p].x = path.obstacles[i].x + (uint16_t) x; + path.points[p].y = path.obstacles[i].y + (uint16_t) y; + /* Check it is in playground. */ + if (path.points[p].x >= path.border_xmin + && path.points[p].y >= path.border_ymin + && path.points[p].x < path.border_xmax + && path.points[p].y < path.border_ymax) + /* Accept point. */ + p++; + } + } + } + path.points_nb = p; +} + +/** Compute and return distance between two points if there is no obstacle + * between them. Return 0xffff if there is an obstacle. */ +static uint16_t +path_compute_weight (uint8_t a, uint8_t b) +{ + int32_t dx, dy; + int16_t ab, d, m, f; + uint8_t i; + /* Compute distance. */ + dx = path.points[b].x - path.points[a].x; + dy = path.points[b].y - path.points[a].y; + ab = fixed_sqrt_ui32 (dx * dx + dy * dy); + if (ab == 0) + return 0; + /* Is there an intersection with a circle. */ + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid) + { + /* Compute distance to center. + * Use a scalar product between a segment perpendicular vector and + * the vector from one segment end to obstacle center. */ + int32_t acx, acy; + acx = path.obstacles[i].x - path.points[a].x; + acy = path.obstacles[i].y - path.points[a].y; + d = (acx * dy - acy * dx) / ab; + d = UTILS_ABS (d); + /* If out of the circle, no problem, else, more tests. */ + if ((uint16_t) d <= path.obstacles[i].r) + { + /* If the line intersect the circle, the segment intersect the + * circle if it as one point on each side of an intersection. */ + m = (acx * dx + acy * dy) / ab; + f = fixed_sqrt_ui32 (path.obstacles[i].r * path.obstacles[i].r + - d * d); + if (!((m - f > 0 && m + f > 0 && m - f > ab && m + f > ab) + || (m - f < 0 && m + f < 0 && m - f < ab && m + f < ab))) + return 0xffff; + } + } + } + return ab; +} + +/** Fill the arc matrix. */ +static void +path_compute_arcs (void) +{ + uint8_t i, j; + for (i = 0; i < path.points_nb; i++) + { + path.arcs[i][i] = 0xffff; + for (j = 0; j < i; j++) + { + path.arcs[i][j] = path_compute_weight (i, j); + path.arcs[j][i] = path.arcs[i][j]; + } + } +} + +/** Apply the Dijkstra algorithm. */ +static void +path_dijkstra (void) +{ + uint8_t i, u; + uint16_t d, wmin; + /** Initialise each points. */ + for (i = 0; i < path.points_nb; i++) + { + path.points[i].w = 0xffff; + path.points[i].next = 0xff; + path.points[i].taken = 0; + } + /** Start with the destination point. */ + u = 0; + path.points[u].w = 0; + path.points[u].taken = 1; + do + { + /* Relax every connected points. */ + for (i = 0; i < path.points_nb; i++) + { + if (path.arcs[i][u] != 0xffff) + { + d = path.points[u].w + path.arcs[i][u]; + if (d < path.points[i].w) + { + path.points[i].w = d; + path.points[i].next = u; + } + } + } + /* Find the next best point. */ + wmin = 0xffff; + u = 0xff; + for (i = 0; i < path.points_nb; i++) + { + if (!path.points[i].taken && path.points[i].w < wmin) + { + u = i; + wmin = path.points[u].w; + } + } + if (u != 0xff) + path.points[u].taken = 1; + /* Until the source point is found, or no solution. */ + } while (u != 1 && u != 0xff); +} + +/** Setup end points (source and destination coordinates). */ +void +path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy) +{ + path.points[0].x = dx; + path.points[0].y = dy; + path.points[1].x = sx; + path.points[1].y = sy; +} + +/** Set up an obstacle at given position with the given radius and validity + * period. */ +void +path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint16_t valid) +{ + path.obstacles[i].x = x; + path.obstacles[i].y = y; + path.obstacles[i].r = r; + path.obstacles[i].valid = valid; +} + +/** Slowly make the obstacles disappear. */ +void +path_decay (void) +{ + uint8_t i; + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid && path.obstacles[i].valid != 0xffff) + path.obstacles[i].valid--; + } +} + +/** Compute shortest path. */ +void +path_update (void) +{ + path_compute_points (); + path_compute_arcs (); + path_dijkstra (); +#if AC_PATH_REPORT + uint8_t len, i; + uint16_t points[PATH_POINTS_NB * 2]; + len = 0; + for (i = 1; i != 0xff; i = path.points[i].next) + { + points[len++] = path.points[i].x; + points[len++] = path.points[i].y; + } + AC_PATH_REPORT_CALLBACK (points, len, path.obstacles, PATH_OBSTACLES_NB); +#endif /* AC_PATH_REPORT */ +} + +/** Retrieve first path point coordinates. Return 0 on failure. */ +uint8_t +path_get_next (uint16_t *x, uint16_t *y) +{ + uint8_t next; + next = path.points[1].next; + if (next == 0xff) + return 0; + else + { + *x = path.points[next].x; + *y = path.points[next].y; + return 1; + } +} + +#ifdef HOST + +#include + +/** Output graph in Graphviz format. */ +void +path_print_graph (void) +{ + int i, j; + printf ("graph map {\n overlap = scale\n sep = 0.5\n"); + for (i = 0; i < path.points_nb; i++) + { + printf (" %d [ shape = Mrecord, label = \"{%d|{%d|%d}}\", " + "pos = \"%d,%d\" ]\n", i, i, path.points[i].x, + path.points[i].y, path.points[i].x, path.points[i].y); + for (j = 0; j < i; j++) + { + if (path.arcs[i][j] != 0xffff) + { + printf (" %d -- %d [ label = \"%d\" ]\n", i, j, + path.arcs[i][j]); + } + } + } + printf ("}\n// Path:\n"); + for (i = 1; i != 0xff; i = path.points[i].next) + printf ("// %d, %d\n", path.points[i].x, path.points[i].y); +} + +#endif diff --git a/digital/avr/modules/path/path.h b/digital/avr/modules/path/path.h new file mode 100644 index 00000000..8255bf65 --- /dev/null +++ b/digital/avr/modules/path/path.h @@ -0,0 +1,83 @@ +#ifndef path_h +#define path_h +/* path.h */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Obstacle. */ +struct path_obstacle_t +{ + /** Center. */ + int16_t x, y; + /** Radius. */ + uint16_t r; + /** Validity counter, when this is zero, the obstacle is ignored. */ + uint16_t valid; +}; + +/** Initialise path finder. */ +void +path_init (int16_t border_xmin, int16_t border_ymin, + int16_t border_xmax, int16_t border_ymax); + +/** Setup end points (source and destination coordinates). */ +void +path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy); + +/** Set up an obstacle at given position with the given radius and validity + * period. */ +void +path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint16_t valid); + +/** Slowly make the obstacles disappear. */ +void +path_decay (void); + +/** Compute shortest path. */ +void +path_update (void); + +/** Retrieve first path point coordinates. Return 0 on failure. */ +uint8_t +path_get_next (uint16_t *x, uint16_t *y); + +#if AC_PATH_REPORT + +/** Report computed path. */ +void +AC_PATH_REPORT_CALLBACK (uint16_t *points, uint8_t len, + struct path_obstacle_t *obstacles, + uint8_t obstacles_nb); + +#endif + +#ifdef HOST + +/** Output graph in Graphviz format. */ +void +path_print_graph (void); + +#endif + +#endif /* path_h */ diff --git a/digital/avr/modules/path/test/Makefile b/digital/avr/modules/path/test/Makefile new file mode 100644 index 00000000..698ee538 --- /dev/null +++ b/digital/avr/modules/path/test/Makefile @@ -0,0 +1,12 @@ +BASE = ../../.. +HOST_PROGS = test_path +test_path_SOURCES = test_path.c +MODULES = path math/fixed utils +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 + +include $(BASE)/make/Makefile.gen diff --git a/digital/avr/modules/path/test/avrconfig.h b/digital/avr/modules/path/test/avrconfig.h new file mode 100644 index 00000000..476fee86 --- /dev/null +++ b/digital/avr/modules/path/test/avrconfig.h @@ -0,0 +1,39 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - Path module configuration template. */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/* path - Path finding module. */ +/** Report path found for debug. */ +#define AC_PATH_REPORT defined (HOST) +/** Report function name. */ +#define AC_PATH_REPORT_CALLBACK path_report +/** Number of possible obstacles. */ +#define AC_PATH_OBSTACLES_NB 2 +/** Number of points per obstacle. */ +#define AC_PATH_OBSTACLES_POINTS_NB 8 + + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/path/test/test_path.c b/digital/avr/modules/path/test/test_path.c new file mode 100644 index 00000000..8b88e5d7 --- /dev/null +++ b/digital/avr/modules/path/test/test_path.c @@ -0,0 +1,52 @@ +/* test_path.c */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "modules/path/path.h" + +#include + +int +main (void) +{ + path_init (0, 0, 3000, 2100); + path_endpoints (300, 1000, 1200, 1000); + path_obstacle (0, 600, 930, 100, 1); + path_obstacle (1, 900, 1070, 100, 1); + path_update (); + path_print_graph (); + uint16_t x, y; + if (path_get_next (&x, &y)) + printf ("// Next point: %d, %d\n", x, y); + else + printf ("// Failure\n"); + return 0; +} + +void +path_report (uint16_t *points, uint8_t len, + struct path_obstacle_t *obstacles, uint8_t obstacles_nb) +{ +} + -- cgit v1.2.3