From d26a6b404e42b529d1083c56b6e675b255ec6758 Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Fri, 3 May 2013 15:27:28 +0200 Subject: digital/io-hub/src/common-cc/path.cc: better way to handle the escape factor --- digital/io-hub/src/common-cc/path.cc | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index a9cb5948..bb025405 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -62,6 +62,13 @@ enum { PATH_NAVPOINT_DST_IDX }; +static int32_t pos_dot_product(vect_t* pa, vect_t* pb, vect_t* pc, vect_t* pd) +{ + vect_t vab = *pb; vect_sub(&vab, pa); + vect_t vcd = *pd; vect_sub(&vcd, pc); + return vect_dot_product(&vab, &vcd); +} + Path::Path() : border_xmin(pg_border_distance + pg_plate_size_border*2 + BOT_SIZE_RADIUS/2), border_ymin(pg_border_distance), @@ -222,15 +229,15 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors) } /* Collision while trying to escape the source point */ /* if and only if the source point is in this obstacle */ - /* and the distance is less than the obstacle radius */ - else if (i==PATH_NAVPOINT_SRC_IDX && - distance_point_point(&navpoints[cur_point], &navpoints[i]) < obstacles[j].r && - PATH_IN_CIRCLE(&navpoints[i], &obstacles[j].c, obstacles[j].r)) + /* and we are going away from the obstacle center */ + else if (escape_factor!=0 && i==PATH_NAVPOINT_SRC_IDX && + PATH_IN_CIRCLE(&navpoints[i], &obstacles[j].c, obstacles[j].r) && + pos_dot_product(&navpoints[i], &obstacles[j].c, &navpoints[i], &navpoints[cur_point])<=0) { /* Allow this navigation point with an extra cost */ weight *= escape_factor; } - /* Collision during a standard move */ + /* Other collisions are not allowed */ else { /* Disable this navigation point */ -- cgit v1.2.3