Panda3D
 All Classes Functions Variables Enumerations
aiPathFinder.cxx
00001 ////////////////////////////////////////////////////////////////////////
00002 // Filename    :  aiPathFinder.cxx
00003 // Created by  :  Deepak, John, Navin
00004 // Date        :  10 Nov 09
00005 ////////////////////////////////////////////////////////////////////
00006 //
00007 // PANDA 3D SOFTWARE
00008 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00009 //
00010 // All use of this software is subject to the terms of the revised BSD
00011 // license.  You should have received a copy of this license along
00012 // with this source code in a file named "LICENSE."
00013 //
00014 ////////////////////////////////////////////////////////////////////
00015 
00016 #include "aiPathFinder.h"
00017 
00018 PathFinder::PathFinder(NavMesh nav_mesh) {
00019   _grid = nav_mesh;
00020 }
00021 
00022 PathFinder::~PathFinder() {
00023 }
00024 
00025 /////////////////////////////////////////////////////////////////////////////////////////
00026 //
00027 // Function : find_path
00028 // Description : This function initializes the pathfinding process by accepting the
00029 //               source and destination nodes. It then calls the generate_path().
00030 
00031 /////////////////////////////////////////////////////////////////////////////////////////
00032 
00033 void PathFinder::find_path(Node *src_node, Node *dest_node) {
00034   _src_node = src_node;
00035   _dest_node = dest_node;
00036 
00037   // Add a dummy node as the first element of the open list with score = -1.
00038   // Inorder to implement a binary heap the index of the elements should never be 0.
00039   Node *_dummy_node = new Node(-1, -1, LVecBase3f(0.0, 0.0, 0.0), 0, 0, 0);
00040   _dummy_node->_status = _dummy_node->open;
00041   _dummy_node->_score = -1;
00042   _open_list.push_back(_dummy_node);
00043 
00044   // Add the source node to the open list.
00045   add_to_olist(_src_node);
00046 
00047   // Generate the path.
00048   generate_path();
00049 }
00050 
00051 /////////////////////////////////////////////////////////////////////////////////////////
00052 //
00053 // Function : generate_path
00054 // Description : This function performs the pathfinding process using the A* algorithm.
00055 //               It updates the openlist and closelist.
00056 
00057 /////////////////////////////////////////////////////////////////////////////////////////
00058 
00059 void PathFinder::generate_path() {
00060   // All the A* algorithm is implemented here.
00061   // The check is > 1 due to the existence of the dummy node.
00062   while(_open_list.size() > 1) {
00063     // The first element of the open list will always be the optimal node.
00064     // This is because the open list is a binary heap with element having the
00065     // smallest score at the top of the heap.
00066     Node* nxt_node = _open_list[1];
00067 
00068     if(nxt_node->_grid_x == _dest_node->_grid_x &&
00069               nxt_node->_grid_y == _dest_node->_grid_y) {
00070       // Remove the optimal node from the top of the heap.
00071       remove_from_olist();
00072 
00073       // add the used node to the closed list.
00074       add_to_clist(nxt_node);
00075 
00076       // At this point the destination is reached.
00077       return;
00078     }
00079     else {
00080       identify_neighbors(nxt_node);
00081 
00082       // add the used node to the closed list.
00083       add_to_clist(nxt_node);
00084     }
00085   }
00086   cout<<"DESTINATION NOT REACHABLE MATE!"<<endl;
00087   _closed_list.clear();
00088 }
00089 
00090 /////////////////////////////////////////////////////////////////////////////////////////
00091 //
00092 // Function : identify_neighbors
00093 // Description : This function traverses through the 8 neigbors of the parent node and
00094 //               then adds the neighbors to the _open_list based on A* criteria.
00095 
00096 /////////////////////////////////////////////////////////////////////////////////////////
00097 
00098 void PathFinder::identify_neighbors(Node *parent_node) {
00099   // Remove the parent node from the open_list so that it is not considered
00100   // while adding new nodes to the open list heap.
00101   remove_from_olist();
00102   for(int i = 0; i < 8; ++i) {
00103     if(parent_node->_neighbours[i] != NULL) {
00104       if(parent_node->_neighbours[i]->_status == parent_node->_neighbours[i]->neutral
00105         && parent_node->_neighbours[i]->_type == true) {
00106         // Link the neighbor to the parent node.
00107         parent_node->_neighbours[i]->_prv_node = parent_node;
00108         // Calculate and update the score for the node.
00109         calc_node_score(parent_node->_neighbours[i]);
00110         // Add the neighbor to the open list.
00111         add_to_olist(parent_node->_neighbours[i]);
00112       }
00113     }
00114   }
00115 }
00116 
00117 /////////////////////////////////////////////////////////////////////////////////////////
00118 //
00119 // Function : calc_node_score
00120 // Description : This function calculates the score of each node.
00121 //               Score = Cost + Heuristics.
00122 
00123 /////////////////////////////////////////////////////////////////////////////////////////
00124 
00125 void PathFinder::calc_node_score(Node *nd) {
00126   nd->_cost = calc_cost_frm_src(nd);
00127   nd->_heuristic = calc_heuristic(nd);
00128   nd->_score = nd->_cost + nd->_heuristic;
00129 }
00130 
00131 /////////////////////////////////////////////////////////////////////////////////////////
00132 //
00133 // Function : calc_cost_frm_src
00134 // Description : This function calculates the cost of each node by finding out
00135 // the number of node traversals required to reach the source node.
00136 // Diagonal traversals have cost = 14.
00137 // Horizontal / Vertical traversals have cost = 10.
00138 
00139 /////////////////////////////////////////////////////////////////////////////////////////
00140 
00141 int PathFinder::calc_cost_frm_src(Node *nd) {
00142   int cost = 0;
00143   Node *start_node = nd;
00144   while(start_node->_prv_node != _src_node) {
00145     if(is_diagonal_node(start_node)) {
00146       cost += 14;
00147     }
00148     else {
00149       cost += 10;
00150     }
00151     start_node = start_node->_prv_node;
00152   }
00153   // Add the cost of traversal to the source node also.
00154   if(is_diagonal_node(start_node)) {
00155     cost += 14;
00156   }
00157   else {
00158     cost += 10;
00159   }
00160   return cost;
00161 }
00162 
00163 /////////////////////////////////////////////////////////////////////////////////////////
00164 //
00165 // Function : calc_heuristic
00166 // Description : This function calculates the heuristic of the nodes using Manhattan method.
00167 // All it does is predict the number of node traversals required to reach the target node.
00168 // No diagonal traversals are allowed in this technique.
00169 
00170 /////////////////////////////////////////////////////////////////////////////////////////
00171 
00172 int PathFinder::calc_heuristic(Node *nd) {
00173   int row_diff = abs(_dest_node->_grid_x - nd->_grid_x);
00174   int col_diff = abs(_dest_node->_grid_y - nd->_grid_y);
00175 
00176   int heuristic = 10 * (row_diff + col_diff);
00177   return heuristic;
00178 }
00179 
00180 /////////////////////////////////////////////////////////////////////////////////////////
00181 //
00182 // Function : is_diagonal_node
00183 // Description : This function checks if the traversal from a node is diagonal.
00184 
00185 /////////////////////////////////////////////////////////////////////////////////////////
00186 
00187 bool PathFinder::is_diagonal_node(Node *nd) {
00188   // Calculate the row and column differences between child and parent nodes.
00189   float row_diff = nd->_grid_x - nd->_prv_node->_grid_x;
00190   float col_diff = nd->_grid_y - nd->_prv_node->_grid_y;
00191 
00192   // Check if the relationship between child and parent node is diagonal.
00193   if(row_diff == 0 || col_diff == 0) {
00194     return false;
00195   }
00196   else {
00197     return true;
00198   }
00199 }
00200 
00201 /////////////////////////////////////////////////////////////////////////////////////////
00202 //
00203 // Function : add_to_olist
00204 // Description : This function adds a node to the open list heap.
00205 //               A binay heap is maintained to improve the search.
00206 
00207 /////////////////////////////////////////////////////////////////////////////////////////
00208 
00209 
00210 void PathFinder::add_to_olist(Node *nd) {
00211   // Variables required to search the binary heap.
00212   Node *child_node, *parent_node;
00213   int child_idx, parent_idx;
00214 
00215   // Set the status as open.
00216   nd->_status = nd->open;
00217   // Add the node to the open list.
00218   _open_list.push_back(nd);
00219 
00220   // Find the parent and child nodes and create temporary nodes out of them.
00221   // In a binary heap the children of a parent node are always i*2 and i*2 + 1,
00222   // where i is the index of the parent node in the heap. And hence, the parent
00223   // of a node can be easily found out by dividing by 2 and rounding it.
00224   child_idx = _open_list.size() - 1;
00225   parent_idx = child_idx / 2;
00226   child_node = _open_list[child_idx];
00227   parent_node = _open_list[parent_idx];
00228 
00229   // Keep traversing the heap until the lowest element in the list is bubbled
00230   // to the top of the heap.
00231   while(_open_list[child_idx]->_score <= _open_list[parent_idx]->_score) {
00232     // Swap the parent node and the child node.
00233     _open_list[parent_idx] = child_node;
00234     _open_list[child_idx] = parent_node;
00235 
00236     // Update the new child and parent indices.
00237     child_idx = parent_idx;
00238     parent_idx = child_idx / 2;
00239 
00240     // Update the new child and parent nodes.
00241     child_node = _open_list[child_idx];
00242     parent_node = _open_list[parent_idx];
00243   }
00244 
00245   // At this point the Node with the smallest score will be at the top of the heap.
00246 }
00247 
00248 /////////////////////////////////////////////////////////////////////////////////////////
00249 //
00250 // Function : remove_from_olist
00251 // Description : This function removes a node from the open list.
00252 //               During the removal the binary heap is maintained.
00253 
00254 /////////////////////////////////////////////////////////////////////////////////////////
00255 
00256 void PathFinder::remove_from_olist() {
00257   // Variables for maintaining the binary heap.
00258   Node *child_node, *child_node_1, *child_node_2;
00259   int child_idx, child_idx_1, child_idx_2;
00260 
00261   // Remove the Node at index 1 from the open list binary heap.
00262   // Note: Node at index 0 of open list is a dummy node.
00263   _open_list.erase(_open_list.begin() + 1);
00264 
00265   if(_open_list.size() > 1) {
00266     // Store the last element in the open list to a temp_node.
00267     Node *temp_node = _open_list[_open_list.size() - 1];
00268 
00269     // Shift the elements of the open list to the right by 1 element circularly, excluding element at 0 index.
00270     for(int i = _open_list.size() - 1; i > 1; --i) {
00271       _open_list[i] = _open_list[i - 1];
00272     }
00273 
00274     // Assign the temp_node to 1st element in the open list.
00275     _open_list[1] = temp_node;
00276 
00277     // Set the iterator for traversing the node from index 1 in the heap.
00278     unsigned int k = 1;
00279 
00280     // This loop traverses down the open list till the node reaches the correct position in the binary heap.
00281     while(true) {
00282       if((k * 2 + 1) < _open_list.size()) {
00283         // Two children exists for the parent node.
00284         child_idx_1 = k * 2;
00285         child_idx_2 = k * 2 + 1;
00286         child_node_1 = _open_list[child_idx_1];
00287         child_node_2 = _open_list[child_idx_2];
00288 
00289         if(_open_list[child_idx_1]->_score < _open_list[child_idx_2]->_score) {
00290           if(_open_list[k]->_score > _open_list[child_idx_1]->_score) {
00291             // Swap the parent node and the child node.
00292             _open_list[child_idx_1] = _open_list[k];
00293             _open_list[k] = child_node_1;
00294 
00295             // Update the parent node index.
00296             k = child_idx_1;
00297           }
00298           else {
00299             break;
00300           }
00301         }
00302         else {
00303           if(_open_list[k]->_score > _open_list[child_idx_2]->_score) {
00304             // Swap the parent node and the child node.
00305             _open_list[child_idx_2] = _open_list[k];
00306             _open_list[k] = child_node_2;
00307 
00308             // Update the parent node index.
00309             k = child_idx_2;
00310           }
00311           else {
00312             break;
00313           }
00314         }
00315       }
00316       else if((k * 2) < _open_list.size()) {
00317         // Only one child exists for the parent node.
00318         child_idx = k * 2;
00319         child_node = _open_list[child_idx];
00320 
00321         if(_open_list[k]->_score > _open_list[child_idx]->_score) {
00322           // Swap the parent node and the child node.
00323           _open_list[child_idx] = _open_list[k];
00324           _open_list[k] = child_node;
00325 
00326           // Update the parent node index.
00327           k = child_idx;
00328         }
00329         else {
00330           break;
00331         }
00332       }
00333       else {
00334         // No children exists.
00335         break;
00336       }
00337     }
00338   }
00339 
00340   // At this point the Node was succesfully removed and the binary heap re-arranged.
00341 }
00342 
00343 /////////////////////////////////////////////////////////////////////////////////////////
00344 //
00345 // Function : add_to_clist
00346 // Description : This function adds a node to the closed list.
00347 
00348 /////////////////////////////////////////////////////////////////////////////////////////
00349 
00350 void PathFinder::add_to_clist(Node *nd) {
00351   // Set the status as closed.
00352   nd->_status = nd->close;
00353   // Add the node to the close list.
00354   _closed_list.push_back(nd);
00355 }
00356 
00357 /////////////////////////////////////////////////////////////////////////////////////////
00358 //
00359 // Function : remove_from_clist
00360 // Description : This function removes a node from the closed list.
00361 
00362 /////////////////////////////////////////////////////////////////////////////////////////
00363 
00364 void PathFinder::remove_from_clist(int r, int c) {
00365   for(unsigned int i = 0; i < _closed_list.size(); ++i) {
00366     if(_closed_list[i]->_grid_x == r && _closed_list[i]->_grid_y == c) {
00367       _closed_list.erase(_closed_list.begin() + i);
00368       break;
00369     }
00370   }
00371 }
00372 
00373 /////////////////////////////////////////////////////////////////////////////////////////
00374 //
00375 // Function : find_in_mesh
00376 // Description : This function allows the user to pass a position and it returns the
00377 //               corresponding node on the navigation mesh. A very useful function as
00378 //               it allows for dynamic updation of the mesh based on position.
00379 
00380 /////////////////////////////////////////////////////////////////////////////////////////
00381 
00382 Node* find_in_mesh(NavMesh nav_mesh, LVecBase3f pos, int grid_size) {
00383   int size = grid_size;
00384   float x = pos[0];
00385   float y = pos[1];
00386 
00387   for(int i = 0; i < size; ++i) {
00388     for(int j = 0; j < size; ++j) {
00389       if(nav_mesh[i][j] != NULL && nav_mesh[i][j]->contains(x, y)) {
00390         return(nav_mesh[i][j]);
00391       }
00392     }
00393   }
00394   return NULL;
00395 }
 All Classes Functions Variables Enumerations