From ce62267e8872db43c2da008361e8d014601f7b5a Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Wed, 1 May 2013 17:01:29 +0200 Subject: digital/io-hub/src/common-cc/path.cc: fix bug about the escape factor + fix rounding error in the radius of the first navigation points layer + remove commented code --- digital/io-hub/src/common-cc/path.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'digital/io-hub/src/common-cc') diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index 226b00ec..32aed5f0 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -103,7 +103,7 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int host_debug("Add obstacle c=(%u;%u) r=%u nodes=%u layers=%u\n",c.x, c.y, r, nodes, nlayers); /* Enlarge the obstacle radius by the robot size and clearance area width */ - r += BOT_SIZE_RADIUS /*BOT_SIZE_SIDE*/ + clearance; + r += BOT_SIZE_RADIUS + clearance; /* Store obstacle */ //assert(obstacles_nb < PATH_OBSTACLES_NB); @@ -120,11 +120,10 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int /* from one to another without collision with the */ /* obstacle circle. New radius is r / cos(angle/2) */ nr = PATH_ANGLE_824(nodes*2); - x = fixed_div_f824(r, fixed_cos_f824(nr)) + 1 /* margin */; + x = fixed_div_f824(r, fixed_cos_f824(nr)) + 2 /* margin for the unprecise fixed point computation */; y = 0; /* Add a number of sets of navigation points with different weights */ - //for(weight=PATH_OBSTACLES_NAVPOINTS_LAYERS; weight>0; weight--) for(layer=nlayers-1; layer>=0; layer--) { /* Compute obstacle points positions around a circle */ @@ -196,7 +195,9 @@ 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)) { /* Allow this navigation point with an extra cost */ -- cgit v1.2.3