Panda3D
|
00001 00002 #include "pathFind.h" 00003 00004 PathFind::PathFind(AICharacter *ai_ch) { 00005 _ai_char = ai_ch; 00006 00007 _parent = new GeomNode("parent"); 00008 _ai_char->_window_render.attach_new_node(_parent); 00009 00010 _pen = new LineSegs("pen"); 00011 _pen->set_color(1.0, 0.0, 0.0); 00012 _pen->set_thickness(2.0); 00013 00014 _path_finder_obj = NULL; 00015 _dynamic_avoid = false; 00016 } 00017 00018 PathFind::~PathFind() { 00019 } 00020 00021 ///////////////////////////////////////////////////////////////////////////////// 00022 // 00023 // Function : create_nav_mesh 00024 // Description : This function recreates the navigation mesh from the .csv file 00025 00026 ///////////////////////////////////////////////////////////////////////////////// 00027 00028 void PathFind::create_nav_mesh(const char* navmesh_filename) { 00029 // Stage variables. 00030 int grid_x, grid_y; 00031 float l, w, h; 00032 LVecBase3f position; 00033 00034 // Variable to hold line data read from file. 00035 string line; 00036 00037 // Array for storing data members obtained from each line of the file. 00038 string fields[10]; 00039 00040 // Open data file for reading. 00041 ifstream nav_mesh_file (navmesh_filename); 00042 00043 if(nav_mesh_file.is_open()) { 00044 // Capture the grid size from the file. 00045 getline(nav_mesh_file, line); 00046 int pos = line.find(","); 00047 _grid_size = atoi((line.substr(pos + 1)).c_str()); 00048 00049 // Initialize the stage mesh with NULL nodes. 00050 for(int r = 0; r < _grid_size; ++r) { 00051 _nav_mesh.push_back(vector<Node*>()); 00052 for(int c = 0; c < _grid_size; ++c) { 00053 _nav_mesh[r].push_back(NULL); 00054 } 00055 } 00056 00057 // Ignore the header of the navmesh.csv file. 00058 getline(nav_mesh_file, line); 00059 00060 // Begin reading data from the file. 00061 while(!nav_mesh_file.eof()) { 00062 getline(nav_mesh_file, line); 00063 stringstream linestream (line); 00064 00065 // Stores all the data members in the line to the array. 00066 // Data structure: NULL,NodeType,GridX,GridY,Length,Width,Height,PosX,PosY,PosZ 00067 for(int i = 0; i < 10; ++i) { 00068 getline(linestream, fields[i], ','); 00069 } 00070 00071 // Populate the main nodes into stage mesh. 00072 if(fields[0] == "0" && fields[1] == "0") { 00073 grid_x = atoi(fields[2].c_str()); 00074 grid_y = atoi(fields[3].c_str()); 00075 l = atof(fields[4].c_str()); 00076 w = atof(fields[5].c_str()); 00077 h = atof(fields[6].c_str()); 00078 position = LVecBase3f(atof(fields[7].c_str()), atof(fields[8].c_str()), atof(fields[9].c_str())); 00079 00080 Node *stage_node = new Node(grid_x, grid_y, position, w, l, h); 00081 00082 00083 _nav_mesh[grid_y][grid_x] = stage_node; 00084 } 00085 else if(fields[0] == "") { 00086 // End of file reached at this point. 00087 nav_mesh_file.close(); 00088 00089 // Assign the neighbor nodes for each of the main nodes that just got populated into the stage mesh. 00090 assign_neighbor_nodes(navmesh_filename); 00091 } 00092 } 00093 } 00094 else { 00095 cout<<"error opening navmesh.csv file!"<<endl; 00096 } 00097 } 00098 00099 ///////////////////////////////////////////////////////////////////////////////// 00100 // 00101 // Function : assign_neighbor_nodes 00102 // Description : This function assigns the neighbor nodes for each main node present in 00103 // _nav_mesh. 00104 00105 ///////////////////////////////////////////////////////////////////////////////// 00106 00107 void PathFind::assign_neighbor_nodes(const char* navmesh_filename){ 00108 ifstream nav_mesh_file (navmesh_filename); 00109 00110 // Stage variables. 00111 int gd_x, gd_y, gd_xn, gd_yn; 00112 string ln; 00113 string fields[10]; 00114 string fields_n[10]; 00115 00116 if(nav_mesh_file.is_open()) { 00117 getline(nav_mesh_file, ln); // Get rid of grid size line. 00118 getline(nav_mesh_file, ln); // Get rid of the header. 00119 00120 while(!nav_mesh_file.eof()) { 00121 getline(nav_mesh_file, ln); // Gets main node data only. No neighbor nodes. 00122 stringstream linestream (ln); 00123 for(int i = 0; i < 10; ++i) { 00124 getline(linestream, fields[i], ','); 00125 } 00126 if(fields[0] == "0" && fields[1] == "0") { 00127 // Usable main node. 00128 gd_x = atoi(fields[2].c_str()); 00129 gd_y = atoi(fields[3].c_str()); 00130 for(int i = 0; i < 8; ++i) { 00131 getline(nav_mesh_file, ln); // Gets neighbor node data only. No main nodes. 00132 stringstream linestream_n (ln); 00133 for(int j = 0; j < 10; ++j) { 00134 getline(linestream_n, fields_n[j], ','); 00135 } 00136 gd_xn = atoi(fields_n[2].c_str()); 00137 gd_yn = atoi(fields_n[3].c_str()); 00138 00139 if(fields_n[0] == "0" && fields_n[1] == "1") { 00140 // Usable neighbor for main node. 00141 // TODO: The indices of the vector are inverted when compared to the values of the nodes on actual grid. Fix this! 00142 _nav_mesh[gd_y][gd_x]->_neighbours[i] = _nav_mesh[gd_yn][gd_xn]; 00143 } 00144 else if(fields_n[0] == "1" && fields_n[1] == "1") { 00145 // NULL neighbor. 00146 _nav_mesh[gd_y][gd_x]->_neighbours[i] = NULL; 00147 } 00148 else { 00149 cout<<"Warning: Corrupt data!"<<endl; 00150 } 00151 } 00152 } 00153 else if(fields[0] == "") { 00154 // End of file reached at this point. 00155 nav_mesh_file.close(); 00156 } 00157 } 00158 } 00159 else { 00160 cout<<"error opening navmesh.csv file!"<<endl; 00161 } 00162 } 00163 00164 /////////////////////////////////////////////////////////////////////////////////////// 00165 // 00166 // Function : set_path_find 00167 // Description : This function starts the path finding process after reading the given 00168 // navigation mesh. 00169 00170 /////////////////////////////////////////////////////////////////////////////////////// 00171 00172 00173 void PathFind::set_path_find(const char* navmesh_filename) { 00174 create_nav_mesh(navmesh_filename); 00175 00176 if(_ai_char->_steering->_path_follow_obj) { 00177 _ai_char->_steering->remove_ai("pathfollow"); 00178 } 00179 00180 _ai_char->_steering->path_follow(1.0f); 00181 00182 if(_path_finder_obj) { 00183 delete _path_finder_obj; 00184 _path_finder_obj = NULL; 00185 } 00186 00187 _path_finder_obj = new PathFinder(_nav_mesh); 00188 } 00189 00190 /////////////////////////////////////////////////////////////////////////////////////// 00191 // 00192 // Function : path_find (for pathfinding towards a static position) 00193 // Description : This function checks for the source and target in the navigation mesh 00194 // for its availability and then finds the best path via the A* algorithm 00195 // Then it calls the path follower to make the object follow the path. 00196 00197 /////////////////////////////////////////////////////////////////////////////////////// 00198 00199 00200 void PathFind::path_find(LVecBase3f pos, string type) { 00201 if(type == "addPath") { 00202 if(_ai_char->_steering->_path_follow_obj) { 00203 _ai_char->_steering->remove_ai("pathfollow"); 00204 } 00205 00206 _ai_char->_steering->path_follow(1.0f); 00207 } 00208 00209 clear_path(); 00210 00211 Node* src = find_in_mesh(_nav_mesh, _ai_char->_ai_char_np.get_pos(_ai_char->_window_render), _grid_size); 00212 00213 if(src == NULL) { 00214 cout<<"couldnt find source"<<endl; 00215 } 00216 00217 Node* dst = find_in_mesh(_nav_mesh, pos, _grid_size); 00218 00219 if(dst == NULL) { 00220 cout<<"couldnt find destination"<<endl; 00221 } 00222 00223 if(src != NULL && dst != NULL) { 00224 _path_finder_obj->find_path(src, dst); 00225 trace_path(src); 00226 } 00227 00228 if(!_ai_char->_steering->_path_follow_obj->_start) { 00229 _ai_char->_steering->start_follow(); 00230 } 00231 } 00232 00233 /////////////////////////////////////////////////////////////////////////////////////// 00234 // 00235 // Function : path_find (for pathfinding towards a moving target (a NodePath)) 00236 // Description : This function checks for the source and target in the navigation mesh 00237 // for its availability and then finds the best path via the A* algorithm 00238 // Then it calls the path follower to make the object follow the path. 00239 00240 /////////////////////////////////////////////////////////////////////////////////////// 00241 00242 void PathFind::path_find(NodePath target, string type) { 00243 if(type == "addPath") { 00244 if(_ai_char->_steering->_path_follow_obj) { 00245 _ai_char->_steering->remove_ai("pathfollow"); 00246 } 00247 00248 _ai_char->_steering->path_follow(1.0f); 00249 } 00250 00251 clear_path(); 00252 00253 _path_find_target = target; 00254 _prev_position = target.get_pos(_ai_char->_window_render); 00255 00256 Node* src = find_in_mesh(_nav_mesh, _ai_char->_ai_char_np.get_pos(_ai_char->_window_render), _grid_size); 00257 00258 if(src == NULL) { 00259 cout<<"couldnt find source"<<endl; 00260 } 00261 00262 Node* dst = find_in_mesh(_nav_mesh, _prev_position, _grid_size); 00263 00264 if(dst == NULL) { 00265 cout<<"couldnt find destination"<<endl; 00266 } 00267 00268 if(src != NULL && dst != NULL) { 00269 _path_finder_obj->find_path(src, dst); 00270 trace_path(src); 00271 } 00272 00273 if(_ai_char->_steering->_path_follow_obj!=NULL) { 00274 if(!_ai_char->_steering->_path_follow_obj->_start) { 00275 _ai_char->_steering->start_follow("pathfind"); 00276 } 00277 } 00278 } 00279 00280 /////////////////////////////////////////////////////////////////////////////////////// 00281 // 00282 // Function : clear_path 00283 // Description : Helper function to restore the path and mesh to its initial state 00284 00285 /////////////////////////////////////////////////////////////////////////////////////// 00286 00287 void PathFind::clear_path() { 00288 // Initialize to zero 00289 for(int i = 0; i < _grid_size; ++i) { 00290 for(int j = 0; j < _grid_size; ++j) { 00291 if(_nav_mesh[i][j] != NULL) { 00292 _nav_mesh[i][j]->_status = _nav_mesh[i][j]->neutral; 00293 _nav_mesh[i][j]->_cost = 0; 00294 _nav_mesh[i][j]->_heuristic = 0; 00295 _nav_mesh[i][j]->_score = 0; 00296 _nav_mesh[i][j]->_prv_node = NULL; 00297 } 00298 } 00299 } 00300 00301 if(_path_finder_obj) { 00302 _path_finder_obj->_open_list.clear(); 00303 _path_finder_obj->_closed_list.clear(); 00304 } 00305 } 00306 00307 /////////////////////////////////////////////////////////////////////////////////////// 00308 // 00309 // Function : trace_path 00310 // Description : This function is the function which sends the path information one by 00311 // one to the path follower so that it can store the path needed to be 00312 // traversed by the pathfinding object 00313 00314 /////////////////////////////////////////////////////////////////////////////////////// 00315 00316 void PathFind::trace_path(Node* src) { 00317 if(_ai_char->_pf_guide) { 00318 _parent->remove_all_children(); 00319 } 00320 else { 00321 _parent->remove_all_children(); 00322 } 00323 00324 if(_path_finder_obj->_closed_list.size() > 0) { 00325 Node *traversor = _path_finder_obj->_closed_list[_path_finder_obj->_closed_list.size() - 0.5]; 00326 while(traversor != src) { 00327 if(_ai_char->_pf_guide) { 00328 _pen->move_to(traversor->_position.get_x(), traversor->_position.get_y(), 1); 00329 _pen->draw_to(traversor->_prv_node->_position.get_x(), traversor->_prv_node->_position.get_y(), 0.5); 00330 PT(GeomNode) gnode = _pen->create(); 00331 _parent->add_child(gnode); 00332 } 00333 _ai_char->_steering->add_to_path(traversor->_position); 00334 traversor = traversor->_prv_node; 00335 } 00336 } 00337 } 00338 00339 /////////////////////////////////////////////////////////////////////////////////////// 00340 // 00341 // Function : add_obstacle_to_mesh 00342 // Description : This function allows the user to dynamically add obstacles to the 00343 // game environment. The function will update the nodes within the 00344 // bounding volume of the obstacle as non-traversable. Hence will not be 00345 // considered by the pathfinding algorithm. 00346 00347 /////////////////////////////////////////////////////////////////////////////////////// 00348 00349 void PathFind::add_obstacle_to_mesh(NodePath obstacle) { 00350 PT(BoundingVolume) np_bounds = obstacle.get_bounds(); 00351 CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere(); 00352 00353 Node* temp = find_in_mesh(_nav_mesh, obstacle.get_pos(), _grid_size); 00354 00355 if(temp != NULL) { 00356 float left = temp->_position.get_x() - np_sphere->get_radius(); 00357 float right = temp->_position.get_x() + np_sphere->get_radius(); 00358 float top = temp->_position.get_y() + np_sphere->get_radius(); 00359 float down = temp->_position.get_y() - np_sphere->get_radius(); 00360 00361 for(int i = 0; i < _grid_size; ++i) { 00362 for(int j = 0; j < _grid_size; ++j) { 00363 if(_nav_mesh[i][j] != NULL && _nav_mesh[i][j]->_type == true) { 00364 if(_nav_mesh[i][j]->_position.get_x() >= left && _nav_mesh[i][j]->_position.get_x() <= right && 00365 _nav_mesh[i][j]->_position.get_y() >= down && _nav_mesh[i][j]->_position.get_y() <= top) { 00366 _nav_mesh[i][j]->_type = false; 00367 _previous_obstacles.insert(_previous_obstacles.end(), i); 00368 _previous_obstacles.insert(_previous_obstacles.end(), j); 00369 } 00370 } 00371 } 00372 } 00373 } 00374 } 00375 00376 /////////////////////////////////////////////////////////////////////////////////////// 00377 // 00378 // Function : do_dynamic_avoid() 00379 // Description : This function does the updation of the collisions to the mesh based 00380 // on the new positions of the obstacles. 00381 00382 /////////////////////////////////////////////////////////////////////////////////////// 00383 00384 void PathFind::do_dynamic_avoid() { 00385 clear_previous_obstacles(); 00386 _previous_obstacles.clear(); 00387 for(unsigned int i = 0; i < _dynamic_obstacle.size(); ++i) { 00388 add_obstacle_to_mesh(_dynamic_obstacle[i]); 00389 } 00390 } 00391 00392 /////////////////////////////////////////////////////////////////////////////////////// 00393 // 00394 // Function : clear_previous_obstacles() 00395 // Description : Helper function to reset the collisions if the obstacle is not on the 00396 // node anymore 00397 00398 /////////////////////////////////////////////////////////////////////////////////////// 00399 00400 void PathFind::clear_previous_obstacles(){ 00401 for(unsigned int i = 0; i < _previous_obstacles.size(); i = i + 2) { 00402 _nav_mesh[_previous_obstacles[i]][_previous_obstacles[i + 1]]->_type = true; 00403 } 00404 } 00405 00406 /////////////////////////////////////////////////////////////////////////////////////// 00407 // 00408 // Function : dynamic_avoid 00409 // Description : This function starts the pathfinding obstacle navigation for the 00410 // passed in obstacle. 00411 00412 /////////////////////////////////////////////////////////////////////////////////////// 00413 00414 void PathFind::dynamic_avoid(NodePath obstacle) { 00415 _dynamic_avoid = true; 00416 _dynamic_obstacle.insert(_dynamic_obstacle.end(), obstacle); 00417 }