From 409b07274f855c01d27046400610cc88a3d6ef60 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Wed, 1 Jun 2011 21:00:22 +0200 Subject: digital/io-hub: element block path or not --- digital/io-hub/src/robospierre/element.c | 40 ++++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/element.h | 8 +++++++ 2 files changed, 48 insertions(+) (limited to 'digital/io-hub/src/robospierre') diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 4ea005f1..f73d380d 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -711,3 +711,43 @@ element_get_pos (uint8_t element_id) } return pos; } + +uint8_t +element_blocking_path (vect_t a, vect_t b, int16_t ab) +{ + uint8_t i; + element_t e; + /* For each obstacle, try to find an intersection. */ + 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 + { + 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; + } + return 0; +} diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index fa9fced7..07cb3b6b 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -181,4 +181,12 @@ element_get (uint8_t element_id) return element_table[element_id]; } +/** Return whether an element is blocking a line segment. + * - a: line segment first point. + * - b: line segment second point. + * - ab: line segment length. + * - returns: 1 if the path should not be used. */ +uint8_t +element_blocking_path (vect_t a, vect_t b, int16_t ab); + #endif /* element_h */ -- cgit v1.2.3