From e29194cf2afec5d37583804d21c8049c6ab91b44 Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Sat, 13 Apr 2013 01:38:40 +0200 Subject: Add prepare_score and get_score --- digital/io-hub/src/common-cc/path.cc | 65 ++++++++++++++++++-------- digital/io-hub/src/common-cc/path.hh | 88 +++++++++++++++++------------------- 2 files changed, 86 insertions(+), 67 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 78011eb3..f6b58f63 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -27,6 +27,7 @@ #include "bot.hh" #include "robot.hh" #include "playground.hh" +#include "playground_2013.hh" extern "C" { #include "modules/math/fixed/fixed.h" @@ -34,7 +35,7 @@ extern "C" { #include "modules/utils/utils.h" } -#define PATH_DEBUG 1 +#define PATH_DEBUG 1 #ifdef HOST #include "../../digital/ai/src/common/debug.host.h" @@ -63,19 +64,18 @@ Path::Path() : void Path::reset() { vect_t nul = {0,0}; - vect_t cake = {pg_width>>1, pg_length}; /* Reset everything */ DPRINTF( "Path reset\n"); obstacles_nb = 0; - points_nb = PATH_FIXED_POINTS_NB; + points_nb = PATH_RESERVED_POINTS_NB; points[PATH_POINT_SRC_IDX] = nul; points[PATH_POINT_DST_IDX] = nul; next_node = 0; escape_factor = 0; /* Declare the cake as an obstacle */ - add_obstacle(cake, 500 + BOT_SIZE_SIDE, 0, PATH_CAKE_POINTS_NB); + add_obstacle(pg_cake_pos, pg_cake_radius + BOT_SIZE_SIDE * 4 / 3, 0, PATH_CAKE_POINTS_NB); } void Path::add_obstacle(const vect_t &c, const int r, const int factor, const int num) @@ -105,7 +105,7 @@ void Path::add_obstacle(const vect_t &c, const int r, const int factor, const in (fixed_mul_f824(rot_b,rot_b) >> 16) + (1L<<8) ); fact_r += 1; /* To counter rounding operations */ - fact_r += 13; /* Add 10% margin */ + //fact_r += 13; /* Add 10% margin */ /* First point is at the relative position (r;0) */ x = ((r * fact_r) >> 8); @@ -137,11 +137,11 @@ void Path::add_obstacle(const vect_t &c, const int r, const int factor, const in #ifdef HOST // Plot obstacle points - robot->hardware.simu_report.pos( &points[PATH_FIXED_POINTS_NB], points_nb-PATH_FIXED_POINTS_NB, PATH_PLOT_ID); + robot->hardware.simu_report.pos( &points[PATH_RESERVED_POINTS_NB], points_nb-PATH_RESERVED_POINTS_NB, PATH_PLOT_ID); #if 0 // Draw the last obstacle points[points_nb] = points[points_nb - num]; - robot->hardware.simu_report.path( &points[points_nb - num], points_nb + 1); + robot->hardware.simu_report.path( &points[points_nb - num], num + 1); #endif #endif } @@ -205,28 +205,38 @@ bool Path::get_next(vect_t &p) return path_found; } -vect_t& Path::get_point(uint8_t index) +vect_t& Path::get_point_vect(const uint8_t index) { + //assert(index validated w=%u\n", weight); neighbors[neighbors_nb].node = i; - neighbors[neighbors_nb].weight = (uint16_t)UTILS_MIN(weight,0xFFFF); + neighbors[neighbors_nb].weight = weight; neighbors_nb++; } else @@ -256,7 +267,7 @@ uint8_t Path::find_neighbors(uint8_t cur_point, struct astar_neighbor_t *neighbo #if PATH_DEBUG DPRINTF("\tFound %u neighbors: ", neighbors_nb); - for(i=0;ipath.get_point(node); - DPRINTF("AC_ASTAR_NEIGHBOR_CALLBACK node=%u (%u;%u)\n", node, node_v.x, node_v.y); + vect_t point_v = robot->path.get_point_vect(node); + DPRINTF("AC_ASTAR_NEIGHBOR_CALLBACK node=%u (%u;%u)\n", node, point_v.x, point_v.y); #endif return robot->path.find_neighbors(node, neighbors); } @@ -277,9 +302,9 @@ AC_ASTAR_NEIGHBOR_CALLBACK (uint8_t node, struct astar_neighbor_t *neighbors) extern "C" uint16_t AC_ASTAR_HEURISTIC_CALLBACK (uint8_t node) { - vect_t node_a = robot->path.get_point(node); - vect_t node_b = robot->path.get_point(PATH_POINT_SRC_IDX); - int16_t dist = distance_point_point(&node_a, &node_b); + vect_t point_a = robot->path.get_point_vect(node); + vect_t point_b = robot->path.get_point_vect(PATH_POINT_SRC_IDX); + int16_t dist = distance_point_point(&point_a, &point_b); DPRINTF("Heuristic node=%u dist=%u\n", node, dist); return dist; } diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh index 19533593..f49188fa 100644 --- a/digital/io-hub/src/common-cc/path.hh +++ b/digital/io-hub/src/common-cc/path.hh @@ -29,25 +29,22 @@ extern "C" { #include "modules/path/astar/astar.h" } -/** Number of possible obstacles. */ -#define PATH_OBSTACLES_NB (4+1/*cake*/) - +#if 0 + /** Number of possible obstacles. */ + static const uint8_t PATH_OBSTACLES_NB = (4+1/*cake*/); /** Number of points for the cake */ #define PATH_CAKE_POINTS_NB 10 - /** Number of points per standard obstacle. */ #define PATH_OBSTACLES_POINTS_NB 6 - -#define PATH_FIXED_POINTS_NB 2 - +#define PATH_RESERVED_POINTS_NB 2 /** Number of points. */ -#define PATH_POINTS_NB (PATH_FIXED_POINTS_NB + \ +#define PATH_POINTS_NB (PATH_RESERVED_POINTS_NB + \ (PATH_OBSTACLES_NB * PATH_OBSTACLES_POINTS_NB) + \ (PATH_CAKE_POINTS_NB - PATH_OBSTACLES_POINTS_NB) + \ 1 /*debug*/) - /** Number of nodes. */ #define PATH_NODES_NB PATH_POINTS_NB +#endif /** Angle between obstacles points. */ #define PATH_OBSTACLES_POINTS_ANGLE(pOINTS_NB) ((1L << 24) / (pOINTS_NB)) @@ -63,72 +60,69 @@ typedef struct path_obstacle_t int factor; } path_obstacle_t; -// Path finding class. +/** Path finding class */ class Path { public: - // Initialise path + /** Initialise path */ Path(); - - // Reset path computation, remove every obstacles. + /** Reset path computation, remove every obstacles */ void reset(void); - - // Add an obstacle on the field + /** Add an obstacle on the field */ void add_obstacle(const vect_t &c, const int r, const int factor, const int points_nb); - - // Set a moving obstacle position, radius and factor. + /** Set a moving obstacle position, radius and factor*/ void obstacle(int index, const vect_t &c, int r, int factor = 0); - - // Set path source and destination. + /** Set path source and destination */ void endpoints(const vect_t &src, const vect_t &dst); - - // Compute path with the given escape factor. + /** Compute path with the given escape factor */ void compute(int factor = 0); - - // Return a point by index - vect_t& get_point(uint8_t index); - - // Get next point in computed path, return false if none (no path found - // or last point given yet). + /** Return a point vector by index */ + vect_t& get_point_vect(const uint8_t index); + /** Return a point index */ + int get_point_index(const vect_t& point); + /** Get next point in computed path, return false if none + (no path found or last point given yet) */ bool get_next(vect_t &p); - - //Find all neighbors of a given node, fill the astar_neighbor structure + /** Find all neighbors of a given node, fill the astar_neighbor structure */ uint8_t find_neighbors(uint8_t node, struct astar_neighbor_t *neighbors); - -#if 0 - // Prepare score computation for the given source, with given escape factor. - void prepare_score (const vect_t &src, int factor = 0); - - // Return score for a given destination, using a previous preparation - // (also reuse previously given escape factor). - int get_score (const vect_t &dst); -#endif + /** Prepare score computation for the given source, with given escape factor */ + void prepare_score(const vect_t &src, int factor = 0); + /** Return score for a given destination, using a previous preparation + (also reuse previously given escape factor) */ + uint16_t get_score(const vect_t &dst); private: - + /** Number of possible obstacles. */ + static const uint8_t PATH_OBSTACLES_NB = (4+1/*cake*/); + /** Number of points for the cake */ + static const uint8_t PATH_CAKE_POINTS_NB = 10; + /** Number of points per standard obstacle. */ + static const uint8_t PATH_OBSTACLES_POINTS_NB = 6; + /** Number of reserved points for the 2 endpoints */ + static const uint8_t PATH_RESERVED_POINTS_NB = 2; + /** Number of points. */ + static const uint8_t PATH_POINTS_NB = (PATH_RESERVED_POINTS_NB + + (PATH_OBSTACLES_NB * PATH_OBSTACLES_POINTS_NB) + + (PATH_CAKE_POINTS_NB - PATH_OBSTACLES_POINTS_NB) + + 1 /*debug*/); + /** Number of nodes. */ + static const uint8_t PATH_NODES_NB = PATH_POINTS_NB; /** Borders, any point outside borders is eliminated. */ const int16_t border_xmin, border_ymin, border_xmax, border_ymax; - /** Escape factor, 0 if none. */ uint8_t escape_factor; - /** List of obstacles. */ path_obstacle_t obstacles[PATH_OBSTACLES_NB]; - + /** Number of obstacles */ uint8_t obstacles_nb; - /** List of navigation points */ vect_t points[PATH_POINTS_NB]; - /** Number of navigation points */ uint8_t points_nb; - /** List of nodes used for A*. */ struct astar_node_t astar_nodes[PATH_NODES_NB]; - /** Which node to look at for next step. */ uint8_t next_node; - /** TRUE when a path has been found */ bool path_found; }; -- cgit v1.2.3