summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/common-cc/path.cc
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/common-cc/path.cc')
-rw-r--r--digital/io-hub/src/common-cc/path.cc207
1 files changed, 109 insertions, 98 deletions
diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc
index cc5e290a..99b9b3f1 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -5,7 +5,7 @@
// Copyright (C) 2013 Olivier Lanneluc
//
// APBTeam:
-// Web: http://apbteam.org/
+// Web: http://apbteam.org/
// Email: team AT apbteam DOT org
//
// This program is free software; you can redistribute it and/or modify
@@ -82,12 +82,13 @@ void Path::reset()
add_obstacle(pg_cake_pos, pg_cake_radius, PATH_CAKE_NAVPOINTS_NB);
}
-void Path::add_obstacle(const vect_t &c, uint16_t r, int num)
+void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes)
{
uint32_t rot_a, rot_b, nr;
uint32_t x, y, nx;
+ int npt, weight;
- DPRINTF("Add obstacle c=(%u;%u) r=%u num=%u\n",c.x, c.y, r, num);
+ DPRINTF("Add obstacle c=(%u;%u) r=%u num=%u\n",c.x, c.y, r, nodes);
/* Enlarge the obstacle radius by the robot size and clearance area width */
r += BOT_SIZE_SIDE + Obstacles::clearance_mm;
@@ -99,39 +100,48 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, int num)
obstacles_nb++;
/* Complex number of modulus 1 and rotation angle */
- nr = PATH_ANGLE_824(num);
+ nr = PATH_ANGLE_824(nodes);
rot_a = fixed_cos_f824(nr);
rot_b = fixed_sin_f824(nr);
/* Extend the points radius to allow the robot to go */
/* from one to another without collision with the */
/* obstacle circle. New radius is r / cos(angle/2) */
- nr = PATH_ANGLE_824(2*num);
+ nr = PATH_ANGLE_824(nodes*2);
x = fixed_div_f824(r, fixed_cos_f824(nr)) + 1 /* margin */;
y = 0;
- /* Compute obstacle points positions around a circle */
- for (; num>0; num--)
+ /* Add 2 sets of navigation points with different weights */
+ for(weight=PATH_NAVPOINTS_LAYERS; weight>0; weight--)
{
- /* Compute the point absolute position */
- navpoints[navpoints_nb].x = c.x + (vect_value_t)x;
- navpoints[navpoints_nb].y = c.y + (vect_value_t)y;
-
- /* Check it is in playground */
- if (navpoints[navpoints_nb].x >= border_xmin
- && navpoints[navpoints_nb].y >= border_ymin
- && navpoints[navpoints_nb].x <= border_xmax
- && navpoints[navpoints_nb].y <= border_ymax)
+ /* Compute obstacle points positions around a circle */
+ for (npt=0; npt<nodes; npt++)
{
- /* Accept point */
- DPRINTF("Add point %u (%u;%u)\n", navpoints_nb, navpoints[navpoints_nb].x, navpoints[navpoints_nb].y);
- navpoints_nb++;
+ /* Compute the point absolute position */
+ navpoints[navpoints_nb].x = c.x + (vect_value_t)x;
+ navpoints[navpoints_nb].y = c.y + (vect_value_t)y;
+
+ /* Check it is in playground */
+ if (navpoints[navpoints_nb].x >= border_xmin
+ && navpoints[navpoints_nb].y >= border_ymin
+ && navpoints[navpoints_nb].x <= border_xmax
+ && navpoints[navpoints_nb].y <= border_ymax)
+ {
+ /* Accept point */
+ DPRINTF("Add point %u (%u;%u) w=%u\n",
+ navpoints_nb, navpoints[navpoints_nb].x, navpoints[navpoints_nb].y, weight);
+ navweights[navpoints_nb] = weight;
+ navpoints_nb++;
+ }
+
+ /* Complex multiply with A = cos(angle) + i sin(angle) */
+ nx = fixed_mul_f824(x, rot_a) - fixed_mul_f824(y, rot_b);
+ y = fixed_mul_f824(y, rot_a) + fixed_mul_f824(x, rot_b);
+ x = nx;
}
- /* Complex multiply with A = cos(angle) + i sin(angle) */
- nx = fixed_mul_f824(x, rot_a) - fixed_mul_f824(y, rot_b);
- y = fixed_mul_f824(y, rot_a) + fixed_mul_f824(x, rot_b);
- x = nx;
+ /* Enlarge the circle */
+ x += BOT_SIZE_RADIUS;
}
#ifdef HOST
@@ -145,20 +155,70 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, int num)
#endif
}
-void Path::obstacle(int index, const vect_t &c, uint16_t r, int f)
+int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors)
{
- add_obstacle(c, r, PATH_OBSTACLES_NAVPOINTS_NB);
-}
+ int neighbors_nb = 0;
-void Path::endpoints(const vect_t &src, const vect_t &dst)
-{
- DPRINTF("Set path endpoints src=(%u;%u) dst=(%u;%u)\n",
- src.x, src.y, dst.x, dst.y);
- navpoints[PATH_NAVPOINT_SRC_IDX] = src;
- navpoints[PATH_NAVPOINT_DST_IDX] = dst;
+ /* Parse all nodes */
+ for(int i=0; i<navpoints_nb; i++)
+ {
+ /* Except the current one */
+ if (i!=cur_point)
+ {
+ /* Get segment length */
+ uint16_t weight = navweights[i] * distance_point_point(&navpoints[cur_point], &navpoints[i]);
+ DPRINTF("- Node %u (%u;%u) w=%u ", i, navpoints[i].x, navpoints[i].y, weight);
+
+ /* Check every obstacle */
+ for(int j=0; j<obstacles_nb; j++)
+ {
+ /* Check for intersection with obstacle */
+ uint16_t d = distance_segment_point(&navpoints[cur_point], &navpoints[i], &obstacles[j].c);
+ if (d < obstacles[j].r)
+ {
+ /* Collision: try to escape when node is the source point */
+ if (i==PATH_NAVPOINT_SRC_IDX &&
+ (cur_point!=PATH_NAVPOINT_DST_IDX ||
+ (navpoints[cur_point].x==pg_cake_pos.x && navpoints[cur_point].y==pg_cake_pos.y)))
+ {
+ weight *= escape_factor; /* allow this navigation point, but it costs */
+ }
+ else
+ {
+ weight = 0; /* disable this navigation point */
+ }
+ DPRINTF("in collision with c=(%u;%u) r=%u w=%u ",
+ obstacles[j].c.x, obstacles[j].c.y, obstacles[j].r, weight);
+ break; /* Stop checking obstacle for this node */
+ }
+ }
+
+ /* Add neighbor if valid */
+ if (weight)
+ {
+ DPRINTF("=> validated w=%u\n", weight);
+ neighbors[neighbors_nb].node = i;
+ neighbors[neighbors_nb].weight = weight;
+ neighbors_nb++;
+ }
+ else
+ {
+ DPRINTF("=> skipped\n");
+ }
+ }
+ }
+
+#if PATH_DEBUG
+ DPRINTF("\tFound %u neighbors: ", neighbors_nb);
+ for(int i=0;i<neighbors_nb;i++)
+ DPRINTF("%u (%u) ", neighbors[i].node, neighbors[i].weight);
+ DPRINTF("\n");
+#endif
+
+ return neighbors_nb;
}
-void Path::compute(int escape)
+void Path::compute(uint16_t escape)
{
DPRINTF("** Path compute(start) escape=%u\n", escape);
@@ -194,6 +254,21 @@ void Path::compute(int escape)
DPRINTF("** Path compute(end) found=%u escape=%u\n", path_found, escape);
}
+void Path::obstacle(int index, const vect_t &c, uint16_t r, int f)
+{
+ add_obstacle(c, r, PATH_OBSTACLES_NAVPOINTS_NB);
+}
+
+void Path::endpoints(const vect_t &src, const vect_t &dst)
+{
+ DPRINTF("Set path endpoints src=(%u;%u) dst=(%u;%u)\n",
+ src.x, src.y, dst.x, dst.y);
+ navpoints[PATH_NAVPOINT_SRC_IDX] = src;
+ navweights[PATH_NAVPOINT_SRC_IDX] = 1;
+ navpoints[PATH_NAVPOINT_DST_IDX] = dst;
+ navweights[PATH_NAVPOINT_DST_IDX] = 1;
+}
+
bool Path::get_next(vect_t &p)
{
if (path_found)
@@ -220,70 +295,7 @@ int Path::get_point_index(const vect_t& point)
return -1;
}
-int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors)
-{
- int neighbors_nb = 0;
-
- /* Parse all nodes */
- for(int i=0; i<navpoints_nb; i++)
- {
- /* Except the current one */
- if (i!=cur_point)
- {
- /* Get segment length */
- uint16_t weight = distance_point_point(&navpoints[cur_point], &navpoints[i]);
- DPRINTF("- Node %u (%u;%u) w=%u ", i, navpoints[i].x, navpoints[i].y, weight);
-
- /* Check every obstacle */
- for(int j=0; j<obstacles_nb; j++)
- {
- /* Check for intersection with obstacle */
- uint16_t d = distance_segment_point(&navpoints[cur_point], &navpoints[i], &obstacles[j].c);
- if (d < obstacles[j].r)
- {
- /* Collision: try to escape when node is the source point */
- if (i==PATH_NAVPOINT_SRC_IDX &&
- (cur_point!=PATH_NAVPOINT_DST_IDX ||
- (navpoints[cur_point].x==pg_cake_pos.x && navpoints[cur_point].y==pg_cake_pos.y)))
- {
- weight *= escape_factor; /* allow this navigation point, but it costs */
- }
- else
- {
- weight = 0; /* disable this navigation point */
- }
- DPRINTF("in collision with c=(%u;%u) r=%u w=%u ",
- obstacles[j].c.x, obstacles[j].c.y, obstacles[j].r, weight);
- break; /* Stop checking obstacle for this node */
- }
- }
-
- /* Add neighbor if valid */
- if (weight)
- {
- DPRINTF("=> validated w=%u\n", weight);
- neighbors[neighbors_nb].node = i;
- neighbors[neighbors_nb].weight = weight;
- neighbors_nb++;
- }
- else
- {
- DPRINTF("=> skipped\n");
- }
- }
- }
-
-#if PATH_DEBUG
- DPRINTF("\tFound %u neighbors: ", neighbors_nb);
- for(int i=0;i<neighbors_nb;i++)
- DPRINTF("%u (%u) ", neighbors[i].node, neighbors[i].weight);
- DPRINTF("\n");
-#endif
-
- return neighbors_nb;
-}
-
-void Path::prepare_score(const vect_t &src, int escape)
+void Path::prepare_score(const vect_t &src, uint16_t escape)
{
DPRINTF("Path prepare score from src=(%u;%u) escape=%u\n", src.x, src.y, escape);
escape_factor = escape;
@@ -316,4 +328,3 @@ AC_ASTAR_HEURISTIC_CALLBACK (uint8_t node)
DPRINTF("Heuristic node=%u dist=%u\n", node, dist);
return dist;
}
-