summaryrefslogtreecommitdiffhomepage
path: root/digital/avr/modules/path/astar/astar.c
blob: 4103682c8efc54d054118e99f029c403b93786be (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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
/* astar.c */
/* avr.path.astar - A* path finding module. {{{
 *
 * Copyright (C) 2010 Nicolas Schodet
 *
 * 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 "common.h"
#include "astar.h"

uint8_t
astar (struct astar_node_t *nodes, uint8_t nodes_nb, uint8_t initial,
       uint8_t goal)
{
    uint8_t i;
    /* Initialise all nodes. */
    for (i = 0; i < nodes_nb; i++)
      {
	nodes[i].score = ASTAR_NODE_SCORE_UNVISITED;
	nodes[i].heuristic = 0;
      }
    /* Start with the initial node. */
    nodes[initial].prev = 0xff; /* Not really read, no previous node. */
    nodes[initial].score = 0;
    nodes[initial].heuristic = AC_ASTAR_HEURISTIC_CALLBACK (initial);
    /* Loop until no node to consider. */
    while (1)
      {
	/* Find the node with the lowest total weight (score + heuristic) in
	 * the open set.  The score field special values has been chosen so
	 * that there is no extra test to select only nodes in the open set.
	 *
	 * If this code is too slow, this search should be replaced using a
	 * proper data structure (sorted list, heap...). */
	uint8_t lowest_node = 0;
	uint16_t lowest_weight = nodes[0].score + nodes[0].heuristic;
	for (i = 1; i < nodes_nb; i++)
	  {
	    if (nodes[i].score + nodes[i].heuristic < lowest_weight)
	      {
		lowest_node = i;
		lowest_weight = nodes[i].score + nodes[i].heuristic;
	      }
	  }
	/* If "found" node is not in the open set, there was no node found,
	 * abort. */
	uint16_t lowest_score = nodes[lowest_node].score;
	if (!ASTAR_NODE_SCORE_OPEN (lowest_score))
	    return 0;
	/* If this is the goal, report our success. */
	if (lowest_node == goal)
	    return 1;
	/* OK, there is some work, move this node to the closed set, it will
	 * never be consider again. */
	nodes[lowest_node].score = ASTAR_NODE_SCORE_CLOSED;
	nodes[lowest_node].heuristic = 0;
	/* Now, process all its neighbors. */
	struct astar_neighbor_t neighbors[nodes_nb - 1];
	uint8_t neighbors_nb = AC_ASTAR_NEIGHBOR_CALLBACK (lowest_node,
							   neighbors);
	for (i = 0; i < neighbors_nb; i++)
	  {
	    uint8_t neighbor = neighbors[i].node;
	    /* Never consider nodes in the closed set. */
	    if (nodes[neighbor].score == ASTAR_NODE_SCORE_CLOSED)
		continue;
	    /* See if our lowest_node is better to arrive to this neighbor
	     * node (node not considered yet, or new score is better).  Note
	     * that due to the score assigned to unvisited nodes, there is
	     * only one test. */
	    uint16_t tentative_score = lowest_score + neighbors[i].weight;
	    if (tentative_score < nodes[neighbor].score)
	      {
		nodes[neighbor].prev = lowest_node;
		/* Assign the new score, which as the side effect to put this
		 * node in the open set. */
		nodes[neighbor].score = tentative_score;
		/* Update heuristic if not done yet. */
		if (!nodes[neighbor].heuristic)
		    nodes[neighbor].heuristic =
			AC_ASTAR_HEURISTIC_CALLBACK (neighbor);
	      }
	  }
      }
}

/* Warning!
 * The heuristic field is used to known if a node has been visited. */
#define unvisited heuristic

void
astar_dijkstra_prepare (struct astar_node_t *nodes, uint8_t nodes_nb,
			uint8_t initial, uint8_t goal)
{
    /* Warning!
     * What is following is NOT the A* algorithm.  This look more like a
     * Dijkstra algorithm as every nodes are processed.  This is done because
     * goal node information (and therefore heuristic) is unknown for now. */
    uint8_t i;
    /* Initialise all nodes. */
    for (i = 0; i < nodes_nb; i++)
      {
	nodes[i].score = ASTAR_NODE_SCORE_UNVISITED;
	nodes[i].unvisited = 1;
      }
    /* Start with the initial node and explore every single node. */
    nodes[initial].prev = 0xff; /* Not really read, no previous node. */
    nodes[initial].score = 0;
    uint8_t lowest_node = initial;
    uint16_t lowest_score = 0;
    /* Loop until no node to consider. */
    while (1)
      {
	/* Mark the current node as visited, it will never be considered
	 * again. */
	nodes[lowest_node].unvisited = 0;
	/* Now, process all its neighbors. */
	struct astar_neighbor_t neighbors[nodes_nb - 1];
	uint8_t neighbors_nb = AC_ASTAR_NEIGHBOR_CALLBACK (lowest_node,
							   neighbors);
	for (i = 0; i < neighbors_nb; i++)
	  {
	    uint8_t neighbor = neighbors[i].node;
	    /* Never consider visited nodes. */
	    if (!nodes[neighbor].unvisited)
		continue;
	    /* See if the current node is better to arrive to this neighbor
	     * node. */
	    uint16_t tentative_score = lowest_score + neighbors[i].weight;
	    if (tentative_score < nodes[neighbor].score)
	      {
		nodes[neighbor].prev = lowest_node;
		nodes[neighbor].score = tentative_score;
	      }
	  }
	/* Find the next unvisited node with the lowest score. */
	lowest_node = 0xff;
	lowest_score = ASTAR_NODE_SCORE_UNVISITED;
	for (i = 0; i < nodes_nb; i++)
	  {
	    if (nodes[i].unvisited && i != goal
		&& nodes[i].score < lowest_score)
	      {
		lowest_node = i;
		lowest_score = nodes[i].score;
	      }
	  }
	/* Found node is visited or not reachable from initial node, abort. */
	if (lowest_node == 0xff)
	    break;
      }
}

uint16_t
astar_dijkstra_finish (struct astar_node_t *nodes, uint8_t nodes_nb,
		       uint8_t goal)
{
    uint8_t i;
    /* Find the shortest path from all of its neighbors. */
    struct astar_neighbor_t neighbors[nodes_nb - 1];
    uint8_t neighbors_nb = AC_ASTAR_NEIGHBOR_CALLBACK (goal, neighbors);
    uint16_t best_score = (uint16_t) -1;
    for (i = 0; i < neighbors_nb; i++)
      {
	uint8_t neighbor = neighbors[i].node;
	/* Only consider reachable neighbors. */
	if (ASTAR_NODE_SCORE_OPEN (nodes[neighbor].score))
	  {
	    uint16_t neighbor_score = nodes[neighbor].score
		+ neighbors[i].weight;
	    if (neighbor_score < best_score)
	      {
		best_score = neighbor_score;
		nodes[goal].prev = neighbor;
	      }
	  }
      }
    return best_score;
}

#undef unvisited