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