summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-01 19:16:38 +0200
committerNicolas Schodet2011-06-01 21:48:47 +0200
commitdadddea9392f78a360969b72a4dc045b4d87dfec (patch)
treea5a4454a0cdbbad9f621a69d7ef5a485fa9b8763
parent8d758c96d257b15edc49dd4cb774ce67becf8cf1 (diff)
digital/io-hub: handle element with path finding
-rw-r--r--digital/io-hub/src/robospierre/element.c60
-rw-r--r--digital/io-hub/src/robospierre/element.h4
-rw-r--r--digital/io-hub/src/robospierre/path.c45
3 files changed, 75 insertions, 34 deletions
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
@@ -704,6 +704,13 @@ element_get_pos (uint8_t element_id)
}
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)
{
uint8_t i;
@@ -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
{