From edba4c1f20ca40734a0fe69699fba7d06e8afc59 Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Fri, 12 Apr 2013 23:54:46 +0200 Subject: First commit - testing the escape factor --- digital/io-hub/src/common-cc/path.hh | 118 +++++++++++++++++++++++++++++------ 1 file changed, 100 insertions(+), 18 deletions(-) (limited to 'digital/io-hub/src/common-cc/path.hh') diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh index 3c281e46..19533593 100644 --- a/digital/io-hub/src/common-cc/path.hh +++ b/digital/io-hub/src/common-cc/path.hh @@ -25,30 +25,112 @@ // }}} #include "defs.hh" -/// Path finding class. -/// TODO: dummy implementation. +extern "C" { +#include "modules/path/astar/astar.h" +} + +/** Number of possible obstacles. */ +#define 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 + +/** Number of points. */ +#define PATH_POINTS_NB (PATH_FIXED_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 + +/** Angle between obstacles points. */ +#define PATH_OBSTACLES_POINTS_ANGLE(pOINTS_NB) ((1L << 24) / (pOINTS_NB)) + +/** Obstacle */ +typedef struct path_obstacle_t +{ + /** Center. */ + vect_t c; + /** Radius. */ + uint16_t r; + /** factor */ + int factor; +} path_obstacle_t; + +// Path finding class. class Path { public: - /// Reset path computation, remove every obstacles. - void reset () { } - /// Set an obstacle position, radius and factor. - void obstacle (int index, const vect_t &c, int r, int factor = 0) { } - /// Set path source and destination. - void endpoints (const vect_t &src, const vect_t &dst) { dst_ = dst; } - /// Compute path with the given escape factor. - void compute (int factor = 0) { } - /// Get next point in computed path, return false if none (no path found - /// or last point given yet). - bool get_next (vect_t &p) { p = dst_; return true; } - /// Prepare score compuration for the given source, with given escape - /// factor. + // Initialise path + Path(); + + // Reset path computation, remove every obstacles. + void reset(void); + + // 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. + void obstacle(int index, const vect_t &c, int r, int factor = 0); + + // Set path source and destination. + void endpoints(const vect_t &src, const vect_t &dst); + + // 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). + bool get_next(vect_t &p); + + //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). + + // Return score for a given destination, using a previous preparation + // (also reuse previously given escape factor). int get_score (const vect_t &dst); +#endif + private: - vect_t dst_; + + /** 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]; + + 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; }; #endif // path_hh -- cgit v1.2.3