From 7903151f6c2a4d83ba875da0ffabc7516eb8991b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 24 Aug 2009 22:01:05 +0200 Subject: * digital/avr/modules/path: - continue to search for obstacles until one is really blocking (closes #98). --- digital/avr/modules/path/path.c | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) (limited to 'digital') diff --git a/digital/avr/modules/path/path.c b/digital/avr/modules/path/path.c index 3b1982cb..ceb36082 100644 --- a/digital/avr/modules/path/path.c +++ b/digital/avr/modules/path/path.c @@ -151,6 +151,7 @@ path_compute_weight (uint8_t a, uint8_t b) int32_t dx, dy; int16_t ab, d, m, f; uint8_t i; + uint8_t max_factor; /* Compute distance. */ dx = path.points[b].x - path.points[a].x; dy = path.points[b].y - path.points[a].y; @@ -158,9 +159,11 @@ path_compute_weight (uint8_t a, uint8_t b) if (ab == 0) return 0; /* Is there an intersection with a circle. */ - for (i = 0; i < PATH_OBSTACLES_NB; i++) + max_factor = 1; + for (i = 0; i < PATH_OBSTACLES_NB && max_factor != 0; i++) { - if (path.obstacles[i].valid) + uint8_t factor = path.obstacles[i].factor; + if (path.obstacles[i].valid && (factor == 0 || factor > max_factor)) { /* Compute distance to center. * Use a scalar product between a segment perpendicular vector and @@ -180,19 +183,18 @@ path_compute_weight (uint8_t a, uint8_t b) - d * d); if (!((m - f > 0 && m + f > 0 && m - f > ab && m + f > ab) || (m - f < 0 && m + f < 0 && m - f < ab && m + f < ab))) - { - uint8_t factor = path.obstacles[i].factor; - if (path.escape_factor - && (a == 1 || b == 1) - && (a != 0 && b != 0) - && (factor == 0u || factor > path.escape_factor)) - factor = path.escape_factor; - return factor == 0 ? 0xffffu : factor * (uint16_t) ab; - } + max_factor = factor; } } } - return ab; + /* Limit factor if trying to escape. */ + if (path.escape_factor + && (a == 1 || b == 1) + && (a != 0 && b != 0) + && (max_factor == 0 || max_factor > path.escape_factor)) + max_factor = path.escape_factor; + /* Return. */ + return max_factor == 0 ? 0xffffu : max_factor * (uint16_t) ab; } /** Fill the arc matrix. */ -- cgit v1.2.3