Revision 93210a92
ID | 93210a926207fbd1d30002efc0af47ee56f5356b |
BFS for NavigationMap prototype, lots of TODOs
scout/libscout/src/behaviors/navigationMap.cpp | ||
---|---|---|
104 | 104 |
navigationMap::shortest_path(State target_state) |
105 | 105 |
{ |
106 | 106 |
// BFS algorithm |
107 |
State curr_state = get_state(); |
|
108 |
char visited[MAX_NODES+1]; |
|
109 |
//FAKE QUEUE OH NO |
|
110 |
//TODO: real implementation |
|
111 |
Queue queue; |
|
112 |
queue.enqueue(curr_state); |
|
113 |
|
|
114 |
// 1 == visited, 0 == not visited |
|
115 |
visited[curr_state] = 1; |
|
116 |
|
|
117 |
while (!queue.is_empty) |
|
118 |
{ |
|
119 |
State state = queue.dequeue(); |
|
120 |
if (state == target_state) |
|
121 |
{ |
|
122 |
//TODO: implement moves_list |
|
123 |
State i; |
|
124 |
|
|
125 |
while(i != curr_state) |
|
126 |
{ |
|
127 |
i = visited[i]; |
|
128 |
|
|
129 |
} |
|
130 |
return moves_list; |
|
131 |
} |
|
132 |
Edge* edges = get_outbound_edges(state); |
|
133 |
int length = sizeof(edges) / sizeof(Edge); |
|
134 |
for (int i = 0; i < length; i++) |
|
135 |
{ |
|
136 |
State new_state = GET_EDGE_STATE(edges[i]); |
|
137 |
if (!visited[new_state]) |
|
138 |
{ |
|
139 |
visited[new_state] = state; |
|
140 |
queue.enqueue(new_state); |
|
141 |
} |
|
142 |
} |
|
143 |
} |
|
107 | 144 |
} |
108 | 145 |
|
109 | 146 |
navigationMap::make_edge(Turn dir, State state, int dist) |
Also available in: Unified diff