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
00024
00025
00026
00027
00028 void PathFind::create_nav_mesh(const char* navmesh_filename) {
00029
00030 int grid_x, grid_y;
00031 float l, w, h;
00032 LVecBase3f position;
00033
00034
00035 string line;
00036
00037
00038 string fields[10];
00039
00040
00041 ifstream nav_mesh_file (navmesh_filename);
00042
00043 if(nav_mesh_file.is_open()) {
00044
00045 getline(nav_mesh_file, line);
00046 int pos = line.find(",");
00047 _grid_size = atoi((line.substr(pos + 1)).c_str());
00048
00049
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
00058 getline(nav_mesh_file, line);
00059
00060
00061 while(!nav_mesh_file.eof()) {
00062 getline(nav_mesh_file, line);
00063 stringstream linestream (line);
00064
00065
00066
00067 for(int i = 0; i < 10; ++i) {
00068 getline(linestream, fields[i], ',');
00069 }
00070
00071
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
00087 nav_mesh_file.close();
00088
00089
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
00102
00103
00104
00105
00106
00107 void PathFind::assign_neighbor_nodes(const char* navmesh_filename){
00108 ifstream nav_mesh_file (navmesh_filename);
00109
00110
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);
00118 getline(nav_mesh_file, ln);
00119
00120 while(!nav_mesh_file.eof()) {
00121 getline(nav_mesh_file, ln);
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
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);
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
00141
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
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
00155 nav_mesh_file.close();
00156 }
00157 }
00158 }
00159 else {
00160 cout<<"error opening navmesh.csv file!"<<endl;
00161 }
00162 }
00163
00164
00165
00166
00167
00168
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
00193
00194
00195
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
00236
00237
00238
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
00283
00284
00285
00286
00287 void PathFind::clear_path() {
00288
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
00310
00311
00312
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
00342
00343
00344
00345
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
00379
00380
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
00395
00396
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
00409
00410
00411
00412
00413
00414 void PathFind::dynamic_avoid(NodePath obstacle) {
00415 _dynamic_avoid = true;
00416 _dynamic_obstacle.insert(_dynamic_obstacle.end(), obstacle);
00417 }