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);
void clear_path()
Helper function to restore the path and mesh to its initial state.
void clear_previous_obstacles()
Helper function to reset the collisions if the obstacle is not on the node anymore.
void add_to_path(LVecBase3 pos)
This function adds positions to the path to follow.
Node * find_in_mesh(NavMesh nav_mesh, LVecBase3 pos, int grid_size)
This function allows the user to pass a position and it returns the corresponding node on the navigat...
void dynamic_avoid(NodePath obstacle)
This function starts the pathfinding obstacle navigation for the passed in obstacle.
This class is used to assign the nodes on the mesh.
This defines a bounding sphere, consisting of a center and a radius.
This class implements pathfinding using A* algorithm.
void create_nav_mesh(const char *navmesh_filename)
This function recreates the navigation mesh from the .csv file.
GeomNode * create(bool dynamic=false)
Creates a new GeomNode that will render the series of line segments and points described via calls to...
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
void set_color(PN_stdfloat r, PN_stdfloat g, PN_stdfloat b, PN_stdfloat a=1.0f)
Establishes the color that will be assigned to all vertices created by future calls to move_to() and ...
void set_thickness(PN_stdfloat thick)
Establishes the line thickness or point size in pixels that will be assigned to all lines and points ...
void find_path(Node *src_node, Node *dest_node)
This function initializes the pathfinding process by accepting the source and destination nodes.
void move_to(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Moves the pen to the given point without drawing a line.
void remove_ai(std::string ai_type)
This function removes individual or all the AIs.
Encapsulates creation of a series of connected or disconnected line segments or points,...
void path_find(LVecBase3 pos, std::string type="normal")
This function checks for the source and target in the navigation mesh for its availability and then f...
NodePath attach_new_node(PandaNode *node, int sort=0, Thread *current_thread=Thread::get_current_thread()) const
Attaches a new node, with or without existing parents, to the scene graph below the referenced node o...
void add_obstacle_to_mesh(NodePath obstacle)
This function allows the user to dynamically add obstacles to the game environment.
void start_follow(std::string type="normal")
This function starts the path follower.
void assign_neighbor_nodes(const char *navmesh_filename)
This function assigns the neighbor nodes for each main node present in _nav_mesh.
LPoint3 get_pos() const
Retrieves the translation component of the transform.
void draw_to(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Draws a line segment from the pen's last position (the last call to move_to or draw_to) to the indica...
void trace_path(Node *src)
This function is the function which sends the path information one by one to the path follower so tha...
void path_follow(float follow_wt)
This function activates path following.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
void do_dynamic_avoid()
This function does the updation of the collisions to the mesh based on the new positions of the obsta...
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
A node that holds Geom objects, renderable pieces of geometry.
void set_path_find(const char *navmesh_filename)
This function starts the path finding process after reading the given navigation mesh.