From 382b99e11c033f1952cbe2b9e082cd35812a13e3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 12 May 2012 02:02:03 +0200 Subject: digital/ui-hub: add debug draw module to debug path finding --- digital/io-hub/src/guybrush/path.c | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) (limited to 'digital/io-hub/src/guybrush/path.c') diff --git a/digital/io-hub/src/guybrush/path.c b/digital/io-hub/src/guybrush/path.c index 5696a2f8..47427a93 100644 --- a/digital/io-hub/src/guybrush/path.c +++ b/digital/io-hub/src/guybrush/path.c @@ -39,6 +39,12 @@ #include "debug.host.h" #endif +#define PATH_DEBUG_DRAW 0 + +#if PATH_DEBUG_DRAW +#include "debug_draw.host.h" +#endif + /** * This year, due to the large number of obstacles, a grid like structure is * used for path finding on the playground. The A* algorithm is used to find @@ -425,6 +431,23 @@ path_update (void) PATH_OBSTACLES_NB); } #endif +#if PATH_DEBUG_DRAW + uint8_t i; + debug_draw_start (); + for (i = 0; i < UTILS_COUNT (path_blocking_point); i++) + debug_draw_circle (&path_blocking_point[i].pos, + path_blocking_point[i].radius, 0); + for (i = 0; i < UTILS_COUNT (path_blocking_segment); i++) + debug_draw_segment (&path_blocking_segment[i][0], + &path_blocking_segment[i][1], 0); + for (i = 0; i < PATH_NODES_NB; i++) + { + vect_t pos; + path_pos (i, &pos); + debug_draw_point (&pos, 1); + } + debug_draw_send (); +#endif } uint8_t -- cgit v1.2.3