From dadddea9392f78a360969b72a4dc045b4d87dfec Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 19:16:38 +0200 Subject: digital/io-hub: handle element with path finding --- digital/io-hub/src/robospierre/element.c | 60 +++++++++++++++++++------------- digital/io-hub/src/robospierre/element.h | 4 +++ digital/io-hub/src/robospierre/path.c | 45 +++++++++++++++++++----- 3 files changed, 75 insertions(+), 34 deletions(-) (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 896382b6..b0c38265 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -703,6 +703,13 @@ element_get_pos (uint8_t element_id) return pos; } +uint8_t +element_blocking (uint8_t element_id) +{ + element_t e = element_get (element_id); + return e.type == ELEMENT_TOWER; +} + uint8_t element_blocking_path (vect_t a, vect_t b, int16_t ab) { @@ -712,33 +719,36 @@ element_blocking_path (vect_t a, vect_t b, int16_t ab) for (i = 0; i < UTILS_COUNT (element_table); i++) { e = element_get (i); - /* Compute square of distance to obstacle, see - * distance_segment_point in modules/math/geometry for the method - * explanation. */ - int32_t absq = (int32_t) ab * ab; - vect_t vab = b; vect_sub (&vab, &a); - vect_t vao = e.pos; vect_sub (&vao, &a); - int32_t dp = vect_dot_product (&vab, &vao); - int32_t dsq; - if (dp < 0) - { - dsq = vect_dot_product (&vao, &vao); - } - else if (dp > absq) - { - vect_t vbo = e.pos; vect_sub (&vbo, &b); - dsq = vect_dot_product (&vbo, &vbo); - } - else + if (e.type == ELEMENT_TOWER) { - vect_t vabn = vab; vect_normal (&vabn); - dsq = vect_dot_product (&vabn, &vao) / ab; - dsq *= dsq; + /* Compute square of distance to obstacle, see + * distance_segment_point in modules/math/geometry for the method + * explanation. */ + int32_t absq = (int32_t) ab * ab; + vect_t vab = b; vect_sub (&vab, &a); + vect_t vao = e.pos; vect_sub (&vao, &a); + int32_t dp = vect_dot_product (&vab, &vao); + int32_t dsq; + if (dp < 0) + { + dsq = vect_dot_product (&vao, &vao); + } + else if (dp > absq) + { + vect_t vbo = e.pos; vect_sub (&vbo, &b); + dsq = vect_dot_product (&vbo, &vbo); + } + else + { + vect_t vabn = vab; vect_normal (&vabn); + dsq = vect_dot_product (&vabn, &vao) / ab; + dsq *= dsq; + } + /* Compare with square of authorised distance. */ + if (dsq < (int32_t) (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20) * + (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20)) + return 1; } - /* Compare with square of authorised distance. */ - if (dsq < (int32_t) (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20) * - (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20)) - return 1; } return 0; } diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 362c7ff9..a31d9f4d 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -179,6 +179,10 @@ element_get (uint8_t element_id) return element_table[element_id]; } +/** Return whether an element is blocking robot. */ +uint8_t +element_blocking (uint8_t element_id); + /** Return whether an element is blocking a line segment. * - a: line segment first point. * - b: line segment second point. diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c index 61ea9293..049c577f 100644 --- a/digital/io-hub/src/robospierre/path.c +++ b/digital/io-hub/src/robospierre/path.c @@ -27,6 +27,7 @@ #include "path.h" #include "bot.h" #include "playground_2011.h" +#include "element.h" #include "modules/path/astar/astar.h" #include "modules/utils/utils.h" @@ -196,6 +197,29 @@ path_pos (uint8_t node, vect_t *pos) } } +static uint8_t +path_element_blocking (uint8_t node) +{ + vect_t pos; + path_pos (node, &pos); + int16_t square_x = (pos.x - 450 - 1) / 350; + int16_t square_y = (2100 - pos.y - 1) / 350; + uint8_t element_id = ELEMENT_UNLOAD_START + square_x + 6 * square_y; + if (element_blocking (element_id)) + return 1; + uint8_t intersection = ((pos.x - 450) / 350) != square_x; + if (intersection) + { + if (element_blocking (element_id + 1)) + return 1; + if (element_blocking (element_id + 6)) + return 1; + if (element_blocking (element_id + 6 + 1)) + return 1; + } + return 0; +} + /** Return 1 if the direct path between a and b nodes is blocked, also compute * distance. */ static uint8_t @@ -239,25 +263,27 @@ path_blocking (uint8_t a, uint8_t b, int16_t *dp) blocking = 1; } } + /* Compute distance. */ + int16_t d = distance_point_point (&va, &vb); + if (d == 0) + { + *dp = 0; + return 0; + } + /* Test for a blocking element. */ + if (element_blocking_path (va, vb, d)) + blocking = 1; /* Handle escaping. */ if (blocking) { if (escape_factor) { - int16_t d = distance_point_point (&va, &vb); *dp = d * escape_factor; return 0; } else return 1; } - /* Compute distance. */ - int16_t d = distance_point_point (&va, &vb); - if (d == 0) - { - *dp = 0; - return 0; - } /* No blocking. */ *dp = d * factor; return 0; @@ -272,7 +298,8 @@ path_blocked_update (void) { uint8_t valid = 1; /* First, gather information from tables. */ - if (!path_nodes[i].usable) + if (!path_nodes[i].usable + || path_element_blocking (i)) valid = 0; else { -- cgit v1.2.3