From fd7fc5373ddf2abc42750ac87749f7a374f11fdf Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Fri, 3 May 2013 16:13:51 +0200 Subject: digital/io-hub/src/common-cc/path: prevent a possible overflow about navpoints weights Also increase the weight of navpoints around the first layer to tend to avoid them. --- digital/io-hub/src/common-cc/path.cc | 16 +++++++++------- digital/io-hub/src/common-cc/path.hh | 7 ++++--- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index 198b269b..d1d1f132 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -156,7 +156,7 @@ void Path::add_obstacle( const vect_t &c, && navpoints[navpoints_nb].y <= border_ymax) { /* Accept point */ - navweights[navpoints_nb] = (1 << PATH_WEIGHT_PRECISION) + (layer * PATH_WEIGHT_STEP); + navweights[navpoints_nb] = (layer * PATH_WEIGHT_STEP); host_debug("Add point %u (%u;%u) w=%u\n", navpoints_nb, navpoints[navpoints_nb].x, navpoints[navpoints_nb].y, navweights[navpoints_nb]); navpoints_nb++; @@ -187,15 +187,17 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors) int neighbors_nb = 0; ucoo::assert(cur_point>= PATH_WEIGHT_PRECISION; + /* Compute the segment weight */ + /* 1st: compute the distance to go */ + weight_t weight = (weight_t)distance_point_point(&navpoints[cur_point], &navpoints[i]); + /* 2nd: Add the target navpoint extra weigth */ + weight += (weight * navweights[cur_point]) >> PATH_WEIGHT_PRECISION; host_debug("- Node %u (%u;%u) w=%u (%u) ", i, navpoints[i].x, navpoints[i].y, weight, navweights[i]); /* Check every obstacle */ @@ -320,8 +322,8 @@ void Path::endpoints(const vect_t &src, const vect_t &dst) navpoints[PATH_NAVPOINT_DST_IDX] = dst; /* Init endpoints weights */ - navweights[PATH_NAVPOINT_SRC_IDX] = (1<