summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/common-cc/path.hh
blob: 8003f32b20ec41098ea9a02922e1a200d688ddfd (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
#ifndef path_hh
#define path_hh
// io-hub - Modular Input/Output. {{{
//
// Copyright (C) 2013 Nicolas Schodet
// Copyright (C) 2013 Olivier Lanneluc
//
// APBTeam:
//      Web: http://apbteam.org/
//      Email: team AT apbteam DOT org
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// }}}
#include "defs.hh"
#include "playground_2013.hh"

extern "C" {
#include "modules/path/astar/astar.h"
}

/** Obstacle */
typedef struct path_obstacle_t
{
    /** Center. */
    vect_t c;
    /** Radius. */
    uint16_t r;
} path_obstacle_t;

typedef uint16_t weight_t;

/** Path finding class */
class Path
{
  public:
    /** Initialise path */
    Path();
    /** Reset path computation, remove every obstacles */
    void reset(void);
    /** Set a moving obstacle position, radius and factor*/
    void obstacle(int index, const vect_t &c, uint16_t r, int f = 0);
    /** Set path source and destination */
    void endpoints(const vect_t &src, const vect_t &dst, const bool force_move=false);
    /** Compute path with the given escape factor */
    void compute(weight_t escape = 0);
    /** Return a point vector by index */
    vect_t& get_point_vect(const int 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 */
    int find_neighbors(int node, struct astar_neighbor_t *neighbors);
    /** Prepare score computation for the given source, with given escape factor */
    void prepare_score(const vect_t &src, weight_t escape = 0);
    /** Return score for a given destination, using a previous preparation
       (also reuse previously given escape factor) */
    weight_t get_score(const vect_t &dst);

  private:
    /** Add an obstacle on the field */
    void add_obstacle(const vect_t &c, uint16_t r, const int nodes);

    /** Number of possible obstacles. */
    static const int PATH_OBSTACLES_NB = (4+1/*cake*/);
    /** Number of points per standard obstacle. */
    static const int PATH_OBSTACLES_NAVPOINTS_NB = 8;
#ifdef playground_2013_hh
    /** Number of points for the cake */
    static const int PATH_CAKE_NAVPOINTS_NB = 14;
#endif
    /** Number of reserved points for the 2 endpoints  */
    static const int PATH_RESERVED_NAVPOINTS_NB = 2;
    /** Number of navigation points layers for each obstacle. */
    static const int PATH_NAVPOINTS_LAYERS = 2;
    /** Number of navigation points. */
    static const int PATH_NAVPOINTS_NB = (PATH_RESERVED_NAVPOINTS_NB +
#ifdef playground_2013_hh
                                           PATH_NAVPOINTS_LAYERS * (PATH_CAKE_NAVPOINTS_NB - PATH_OBSTACLES_NAVPOINTS_NB) +
#endif
                                           PATH_NAVPOINTS_LAYERS * (PATH_OBSTACLES_NB * PATH_OBSTACLES_NAVPOINTS_NB));
    /** Navigation points weight precision (2^-n). */
    static const int PATH_WEIGHT_PRECISION = 4;
    /** Navigation points weight step (2^-n). */
    static const int PATH_WEIGHT_STEP = 5;

    /** Borders, any point outside borders is eliminated. */
    const uint16_t border_xmin, border_ymin, border_xmax, border_ymax;
    /** Escape factor, 0 if none. */
    weight_t escape_factor;
    /** List of obstacles. */
    path_obstacle_t obstacles[PATH_OBSTACLES_NB];
    /** Number of obstacles */
    int obstacles_nb;
    /** List of navigation points coordonates */
    vect_t navpoints[PATH_NAVPOINTS_NB];
    /** List of navigation points weights */
    weight_t navweights[PATH_NAVPOINTS_NB];
    /** Number of navigation points */
    int navpoints_nb;
    /** List of nodes used for A*. */
    struct astar_node_t astar_nodes[PATH_NAVPOINTS_NB];
    /** Which node to look at for next step. */
    int next_node;
    /** TRUE when a path has been found */
    bool path_found;
    /** TRUE to allow last movement to enter an obstacle
     * This may be used to target the center of an obstacle
     * and stop closed to it */
    bool force_move;
};

#endif // path_hh