Panda3D

pathFind.cxx

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 }
 All Classes Functions Variables Enumerations