summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
authorJérôme Jutteau2011-06-01 21:00:22 +0200
committerJérôme Jutteau2011-06-01 21:00:22 +0200
commit409b07274f855c01d27046400610cc88a3d6ef60 (patch)
treedde1859a7e53cf112d1fd86c3dbab2990b1958db /digital/io-hub/src/robospierre
parent2ccee32e49bce84a31c571cd1b9fff1a03ad1b9e (diff)
digital/io-hub: element block path or not
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/element.c40
-rw-r--r--digital/io-hub/src/robospierre/element.h8
2 files changed, 48 insertions, 0 deletions
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 */