Panda3D
|
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 }