Panda3D

aiPathFinder.cxx
Go to the documentation of this file.
1 /**
2 * PANDA 3D SOFTWARE
3 * Copyright (c) Carnegie Mellon University. All rights reserved.
4 *
5 * All use of this software is subject to the terms of the revised BSD
6 * license. You should have received a copy of this license along
7 * with this source code in a file named "LICENSE."
8 *
9 * @file aiPathFinder.cxx
10 * @author Deepak, John, Navin
11 * @date 2009-11-10
12 */
13
14 #include "aiPathFinder.h"
15
16 PathFinder::PathFinder(NavMesh nav_mesh) {
17 _grid = nav_mesh;
18 }
19
20 PathFinder::~PathFinder() {
21 }
22
23 /**
24 * This function initializes the pathfinding process by accepting the source
25 * and destination nodes. It then calls the generate_path().
26 */
27 void PathFinder::find_path(Node *src_node, Node *dest_node) {
28 _src_node = src_node;
29 _dest_node = dest_node;
30
31 // Add a dummy node as the first element of the open list with score = -1.
32 // Inorder to implement a binary heap the index of the elements should never
33 // be 0.
34 Node *_dummy_node = new Node(-1, -1, LVecBase3(0.0, 0.0, 0.0), 0, 0, 0);
35 _dummy_node->_status = _dummy_node->open;
36 _dummy_node->_score = -1;
37 _open_list.push_back(_dummy_node);
38
39 // Add the source node to the open list.
40 add_to_olist(_src_node);
41
42 // Generate the path.
43 generate_path();
44 }
45
46 /**
47 * This function performs the pathfinding process using the A* algorithm. It
48 * updates the openlist and closelist.
49 */
50 void PathFinder::generate_path() {
51 // All the A* algorithm is implemented here. The check is > 1 due to the
52 // existence of the dummy node.
53 while(_open_list.size() > 1) {
54 // The first element of the open list will always be the optimal node.
55 // This is because the open list is a binary heap with element having the
56 // smallest score at the top of the heap.
57 Node* nxt_node = _open_list[1];
58
59 if(nxt_node->_grid_x == _dest_node->_grid_x &&
60 nxt_node->_grid_y == _dest_node->_grid_y) {
61 // Remove the optimal node from the top of the heap.
62 remove_from_olist();
63
64 // add the used node to the closed list.
65 add_to_clist(nxt_node);
66
67 // At this point the destination is reached.
68 return;
69 }
70 else {
71 identify_neighbors(nxt_node);
72
73 // add the used node to the closed list.
74 add_to_clist(nxt_node);
75 }
76 }
77 std::cout << "DESTINATION NOT REACHABLE MATE!" << std::endl;
78 _closed_list.clear();
79 }
80
81 /**
82 * This function traverses through the 8 neigbors of the parent node and then
83 * adds the neighbors to the _open_list based on A* criteria.
84 */
85 void PathFinder::identify_neighbors(Node *parent_node) {
86 // Remove the parent node from the open_list so that it is not considered
87 // while adding new nodes to the open list heap.
88 remove_from_olist();
89 for(int i = 0; i < 8; ++i) {
90 if(parent_node->_neighbours[i] != nullptr) {
91 if(parent_node->_neighbours[i]->_status == parent_node->_neighbours[i]->neutral
92 && parent_node->_neighbours[i]->_type == true) {
93 // Link the neighbor to the parent node.
94 parent_node->_neighbours[i]->_prv_node = parent_node;
95 // Calculate and update the score for the node.
96 calc_node_score(parent_node->_neighbours[i]);
97 // Add the neighbor to the open list.
98 add_to_olist(parent_node->_neighbours[i]);
99 }
100 }
101 }
102 }
103
104 /**
105 * This function calculates the score of each node. Score = Cost +
106 * Heuristics.
107 */
108 void PathFinder::calc_node_score(Node *nd) {
109 nd->_cost = calc_cost_frm_src(nd);
110 nd->_heuristic = calc_heuristic(nd);
111 nd->_score = nd->_cost + nd->_heuristic;
112 }
113
114 /**
115 * This function calculates the cost of each node by finding out the number of
116 * node traversals required to reach the source node. Diagonal traversals
117 * have cost = 14. Horizontal and vertical traversals have cost = 10.
118 */
119 int PathFinder::calc_cost_frm_src(Node *nd) {
120 int cost = 0;
121 Node *start_node = nd;
122 while(start_node->_prv_node != _src_node) {
123 if(is_diagonal_node(start_node)) {
124 cost += 14;
125 }
126 else {
127 cost += 10;
128 }
129 start_node = start_node->_prv_node;
130 }
131 // Add the cost of traversal to the source node also.
132 if(is_diagonal_node(start_node)) {
133 cost += 14;
134 }
135 else {
136 cost += 10;
137 }
138 return cost;
139 }
140
141 /**
142 * This function calculates the heuristic of the nodes using Manhattan method.
143 * All it does is predict the number of node traversals required to reach the
144 * target node. No diagonal traversals are allowed in this technique.
145 */
146 int PathFinder::calc_heuristic(Node *nd) {
147 int row_diff = abs(_dest_node->_grid_x - nd->_grid_x);
148 int col_diff = abs(_dest_node->_grid_y - nd->_grid_y);
149
150 int heuristic = 10 * (row_diff + col_diff);
151 return heuristic;
152 }
153
154 /**
155 * This function checks if the traversal from a node is diagonal.
156 */
157 bool PathFinder::is_diagonal_node(Node *nd) {
158 // Calculate the row and column differences between child and parent nodes.
159 float row_diff = nd->_grid_x - nd->_prv_node->_grid_x;
160 float col_diff = nd->_grid_y - nd->_prv_node->_grid_y;
161
162 // Check if the relationship between child and parent node is diagonal.
163 if(row_diff == 0 || col_diff == 0) {
164 return false;
165 }
166 else {
167 return true;
168 }
169 }
170
171 /**
172 * This function adds a node to the open list heap. A binay heap is
173 * maintained to improve the search.
174 */
175 void PathFinder::add_to_olist(Node *nd) {
176 // Variables required to search the binary heap.
177 Node *child_node, *parent_node;
178 int child_idx, parent_idx;
179
180 // Set the status as open.
181 nd->_status = nd->open;
182 // Add the node to the open list.
183 _open_list.push_back(nd);
184
185 // Find the parent and child nodes and create temporary nodes out of them.
186 // In a binary heap the children of a parent node are always i*2 and i*2 +
187 // 1, where i is the index of the parent node in the heap. And hence, the
188 // parent of a node can be easily found out by dividing by 2 and rounding
189 // it.
190 child_idx = _open_list.size() - 1;
191 parent_idx = child_idx / 2;
192 child_node = _open_list[child_idx];
193 parent_node = _open_list[parent_idx];
194
195 // Keep traversing the heap until the lowest element in the list is bubbled
196 // to the top of the heap.
197 while(_open_list[child_idx]->_score <= _open_list[parent_idx]->_score) {
198 // Swap the parent node and the child node.
199 _open_list[parent_idx] = child_node;
200 _open_list[child_idx] = parent_node;
201
202 // Update the new child and parent indices.
203 child_idx = parent_idx;
204 parent_idx = child_idx / 2;
205
206 // Update the new child and parent nodes.
207 child_node = _open_list[child_idx];
208 parent_node = _open_list[parent_idx];
209 }
210
211 // At this point the Node with the smallest score will be at the top of the
212 // heap.
213 }
214
215 /**
216 * This function removes a node from the open list. During the removal the
217 * binary heap is maintained.
218 */
219 void PathFinder::remove_from_olist() {
220 // Variables for maintaining the binary heap.
221 Node *child_node, *child_node_1, *child_node_2;
222 int child_idx, child_idx_1, child_idx_2;
223
224 // Remove the Node at index 1 from the open list binary heap. Note: Node at
225 // index 0 of open list is a dummy node.
226 _open_list.erase(_open_list.begin() + 1);
227
228 if(_open_list.size() > 1) {
229 // Store the last element in the open list to a temp_node.
230 Node *temp_node = _open_list[_open_list.size() - 1];
231
232 // Shift the elements of the open list to the right by 1 element
233 // circularly, excluding element at 0 index.
234 for(int i = _open_list.size() - 1; i > 1; --i) {
235 _open_list[i] = _open_list[i - 1];
236 }
237
238 // Assign the temp_node to 1st element in the open list.
239 _open_list[1] = temp_node;
240
241 // Set the iterator for traversing the node from index 1 in the heap.
242 unsigned int k = 1;
243
244 // This loop traverses down the open list till the node reaches the
245 // correct position in the binary heap.
246 while(true) {
247 if((k * 2 + 1) < _open_list.size()) {
248 // Two children exists for the parent node.
249 child_idx_1 = k * 2;
250 child_idx_2 = k * 2 + 1;
251 child_node_1 = _open_list[child_idx_1];
252 child_node_2 = _open_list[child_idx_2];
253
254 if(_open_list[child_idx_1]->_score < _open_list[child_idx_2]->_score) {
255 if(_open_list[k]->_score > _open_list[child_idx_1]->_score) {
256 // Swap the parent node and the child node.
257 _open_list[child_idx_1] = _open_list[k];
258 _open_list[k] = child_node_1;
259
260 // Update the parent node index.
261 k = child_idx_1;
262 }
263 else {
264 break;
265 }
266 }
267 else {
268 if(_open_list[k]->_score > _open_list[child_idx_2]->_score) {
269 // Swap the parent node and the child node.
270 _open_list[child_idx_2] = _open_list[k];
271 _open_list[k] = child_node_2;
272
273 // Update the parent node index.
274 k = child_idx_2;
275 }
276 else {
277 break;
278 }
279 }
280 }
281 else if((k * 2) < _open_list.size()) {
282 // Only one child exists for the parent node.
283 child_idx = k * 2;
284 child_node = _open_list[child_idx];
285
286 if(_open_list[k]->_score > _open_list[child_idx]->_score) {
287 // Swap the parent node and the child node.
288 _open_list[child_idx] = _open_list[k];
289 _open_list[k] = child_node;
290
291 // Update the parent node index.
292 k = child_idx;
293 }
294 else {
295 break;
296 }
297 }
298 else {
299 // No children exists.
300 break;
301 }
302 }
303 }
304
305 // At this point the Node was succesfully removed and the binary heap re-
306 // arranged.
307 }
308
309 /**
310 * This function adds a node to the closed list.
311 */
312 void PathFinder::add_to_clist(Node *nd) {
313 // Set the status as closed.
314 nd->_status = nd->close;
315 // Add the node to the close list.
316 _closed_list.push_back(nd);
317 }
318
319 /**
320 * This function removes a node from the closed list.
321 */
322 void PathFinder::remove_from_clist(int r, int c) {
323 for(unsigned int i = 0; i < _closed_list.size(); ++i) {
324 if(_closed_list[i]->_grid_x == r && _closed_list[i]->_grid_y == c) {
325 _closed_list.erase(_closed_list.begin() + i);
326 break;
327 }
328 }
329 }
330
331 /**
332 * This function allows the user to pass a position and it returns the
333 * corresponding node on the navigation mesh. A very useful function as it
334 * allows for dynamic updation of the mesh based on position.
335 */
336 Node* find_in_mesh(NavMesh nav_mesh, LVecBase3 pos, int grid_size) {
337 int size = grid_size;
338 float x = pos[0];
339 float y = pos[1];
340
341 for(int i = 0; i < size; ++i) {
342 for(int j = 0; j < size; ++j) {
343 if(nav_mesh[i][j] != nullptr && nav_mesh[i][j]->contains(x, y)) {
344 return(nav_mesh[i][j]);
345 }
346 }
347 }
348 return nullptr;
349 }
find_in_mesh
Node * find_in_mesh(NavMesh nav_mesh, LVecBase3 pos, int grid_size)
This function allows the user to pass a position and it returns the corresponding node on the navigat...
Definition: aiPathFinder.cxx:336
aiPathFinder.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
Node
This class is used to assign the nodes on the mesh.
Definition: meshNode.h:16
PathFinder::remove_from_clist
void remove_from_clist(int r, int c)
This function removes a node from the closed list.
Definition: aiPathFinder.cxx:322
PathFinder::is_diagonal_node
bool is_diagonal_node(Node *nd)
This function checks if the traversal from a node is diagonal.
Definition: aiPathFinder.cxx:157
PathFinder::add_to_clist
void add_to_clist(Node *nd)
This function adds a node to the closed list.
Definition: aiPathFinder.cxx:312
PathFinder::calc_cost_frm_src
int calc_cost_frm_src(Node *nd)
This function calculates the cost of each node by finding out the number of node traversals required ...
Definition: aiPathFinder.cxx:119
PathFinder::add_to_olist
void add_to_olist(Node *nd)
This function adds a node to the open list heap.
Definition: aiPathFinder.cxx:175
PathFinder::generate_path
void generate_path()
This function performs the pathfinding process using the A* algorithm.
Definition: aiPathFinder.cxx:50
PathFinder::calc_heuristic
int calc_heuristic(Node *nd)
This function calculates the heuristic of the nodes using Manhattan method.
Definition: aiPathFinder.cxx:146
PathFinder::identify_neighbors
void identify_neighbors(Node *nd)
This function traverses through the 8 neigbors of the parent node and then adds the neighbors to the ...
Definition: aiPathFinder.cxx:85
PathFinder::calc_node_score
void calc_node_score(Node *nd)
This function calculates the score of each node.
Definition: aiPathFinder.cxx:108
PathFinder::remove_from_olist
void remove_from_olist()
This function removes a node from the open list.
Definition: aiPathFinder.cxx:219
PathFinder::find_path
void find_path(Node *src_node, Node *dest_node)
This function initializes the pathfinding process by accepting the source and destination nodes.
Definition: aiPathFinder.cxx:27