16 #include "pathFollow.h"
32 _path_finder_obj =
nullptr;
33 _dynamic_avoid =
false;
36 PathFind::~PathFind() {
55 std::ifstream nav_mesh_file (navmesh_filename);
57 if(nav_mesh_file.is_open()) {
59 getline(nav_mesh_file, line);
60 int pos = line.find(
",");
61 _grid_size = atoi((line.substr(pos + 1)).c_str());
64 for(
int r = 0; r < _grid_size; ++r) {
65 _nav_mesh.push_back(std::vector<Node*>());
66 for(
int c = 0; c < _grid_size; ++c) {
67 _nav_mesh[r].push_back(
nullptr);
72 getline(nav_mesh_file, line);
75 while(!nav_mesh_file.eof()) {
76 getline(nav_mesh_file, line);
77 std::stringstream linestream (line);
82 for(
int i = 0; i < 10; ++i) {
83 getline(linestream, fields[i],
',');
87 if(fields[0] ==
"0" && fields[1] ==
"0") {
88 grid_x = atoi(fields[2].c_str());
89 grid_y = atoi(fields[3].c_str());
90 l = atof(fields[4].c_str());
91 w = atof(fields[5].c_str());
92 h = atof(fields[6].c_str());
93 position = LVecBase3(atof(fields[7].c_str()), atof(fields[8].c_str()), atof(fields[9].c_str()));
95 Node *stage_node =
new Node(grid_x, grid_y, position, w, l, h);
98 _nav_mesh[grid_y][grid_x] = stage_node;
100 else if(fields[0] ==
"") {
102 nav_mesh_file.close();
111 cout<<
"error opening navmesh.csv file!"<<endl;
120 std::ifstream nav_mesh_file (navmesh_filename);
123 int gd_x, gd_y, gd_xn, gd_yn;
128 if(nav_mesh_file.is_open()) {
129 getline(nav_mesh_file, ln);
130 getline(nav_mesh_file, ln);
132 while(!nav_mesh_file.eof()) {
133 getline(nav_mesh_file, ln);
134 std::stringstream linestream (ln);
135 for(
int i = 0; i < 10; ++i) {
136 getline(linestream, fields[i],
',');
138 if(fields[0] ==
"0" && fields[1] ==
"0") {
140 gd_x = atoi(fields[2].c_str());
141 gd_y = atoi(fields[3].c_str());
142 for(
int i = 0; i < 8; ++i) {
143 getline(nav_mesh_file, ln);
144 std::stringstream linestream_n (ln);
145 for(
int j = 0; j < 10; ++j) {
146 getline(linestream_n, fields_n[j],
',');
148 gd_xn = atoi(fields_n[2].c_str());
149 gd_yn = atoi(fields_n[3].c_str());
151 if(fields_n[0] ==
"0" && fields_n[1] ==
"1") {
155 _nav_mesh[gd_y][gd_x]->_neighbours[i] = _nav_mesh[gd_yn][gd_xn];
157 else if(fields_n[0] ==
"1" && fields_n[1] ==
"1") {
159 _nav_mesh[gd_y][gd_x]->_neighbours[i] =
nullptr;
162 cout<<
"Warning: Corrupt data!"<<endl;
166 else if(fields[0] ==
"") {
168 nav_mesh_file.close();
173 cout<<
"error opening navmesh.csv file!"<<endl;
184 if(_ai_char->_steering->_path_follow_obj) {
185 _ai_char->_steering->
remove_ai(
"pathfollow");
190 if(_path_finder_obj) {
191 delete _path_finder_obj;
192 _path_finder_obj =
nullptr;
204 if(type ==
"addPath") {
205 if(_ai_char->_steering->_path_follow_obj) {
206 _ai_char->_steering->
remove_ai(
"pathfollow");
217 cout<<
"couldnt find source"<<endl;
223 cout<<
"couldnt find destination"<<endl;
226 if(src !=
nullptr && dst !=
nullptr) {
231 if(!_ai_char->_steering->_path_follow_obj->_start) {
242 if(type ==
"addPath") {
243 if(_ai_char->_steering->_path_follow_obj) {
244 _ai_char->_steering->
remove_ai(
"pathfollow");
252 _path_find_target = target;
253 _prev_position = target.
get_pos(_ai_char->_window_render);
258 cout<<
"couldnt find source"<<endl;
264 cout<<
"couldnt find destination"<<endl;
267 if(src !=
nullptr && dst !=
nullptr) {
272 if(_ai_char->_steering->_path_follow_obj!=
nullptr) {
273 if(!_ai_char->_steering->_path_follow_obj->_start) {
284 for(
int i = 0; i < _grid_size; ++i) {
285 for(
int j = 0; j < _grid_size; ++j) {
286 if(_nav_mesh[i][j] !=
nullptr) {
287 _nav_mesh[i][j]->_status = _nav_mesh[i][j]->neutral;
288 _nav_mesh[i][j]->_cost = 0;
289 _nav_mesh[i][j]->_heuristic = 0;
290 _nav_mesh[i][j]->_score = 0;
291 _nav_mesh[i][j]->_prv_node =
nullptr;
296 if(_path_finder_obj) {
297 _path_finder_obj->_open_list.clear();
298 _path_finder_obj->_closed_list.clear();
308 if(_ai_char->_pf_guide) {
309 _parent->remove_all_children();
312 _parent->remove_all_children();
315 if(_path_finder_obj->_closed_list.size() > 0) {
316 Node *traversor = _path_finder_obj->_closed_list[_path_finder_obj->_closed_list.size() - 0.5];
317 while(traversor != src) {
318 if(_ai_char->_pf_guide) {
319 _pen->
move_to(traversor->_position.get_x(), traversor->_position.get_y(), 1);
320 _pen->
draw_to(traversor->_prv_node->_position.get_x(), traversor->_prv_node->_position.get_y(), 0.5);
322 _parent->add_child(gnode);
324 _ai_char->_steering->
add_to_path(traversor->_position);
325 traversor = traversor->_prv_node;
342 if(temp !=
nullptr) {
343 float left = temp->_position.get_x() - np_sphere->get_radius();
344 float right = temp->_position.get_x() + np_sphere->get_radius();
345 float top = temp->_position.get_y() + np_sphere->get_radius();
346 float down = temp->_position.get_y() - np_sphere->get_radius();
348 for(
int i = 0; i < _grid_size; ++i) {
349 for(
int j = 0; j < _grid_size; ++j) {
350 if(_nav_mesh[i][j] !=
nullptr && _nav_mesh[i][j]->_type ==
true) {
351 if(_nav_mesh[i][j]->_position.get_x() >= left && _nav_mesh[i][j]->_position.get_x() <= right &&
352 _nav_mesh[i][j]->_position.get_y() >= down && _nav_mesh[i][j]->_position.get_y() <= top) {
353 _nav_mesh[i][j]->_type =
false;
354 _previous_obstacles.insert(_previous_obstacles.end(), i);
355 _previous_obstacles.insert(_previous_obstacles.end(), j);
369 _previous_obstacles.clear();
370 for(
unsigned int i = 0; i < _dynamic_obstacle.size(); ++i) {
380 for(
unsigned int i = 0; i < _previous_obstacles.size(); i = i + 2) {
381 _nav_mesh[_previous_obstacles[i]][_previous_obstacles[i + 1]]->_type =
true;
390 _dynamic_avoid =
true;
391 _dynamic_obstacle.insert(_dynamic_obstacle.end(), obstacle);