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);
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 path_follow(float follow_wt=1.0f)
This function activates path following.
void start_follow(std::string type="normal")
This function starts the path follower.
void remove_ai(std::string ai_type)
This function removes individual or all the AIs.
void add_to_path(LVecBase3 pos)
This function adds positions to the path to follow.
This defines a bounding sphere, consisting of a center and a radius.
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
A node that holds Geom objects, renderable pieces of geometry.
Encapsulates creation of a series of connected or disconnected line segments or points,...
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 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 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 ...
GeomNode * create(bool dynamic=false)
Creates a new GeomNode that will render the series of line segments and points described via calls to...
void move_to(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Moves the pen to the given point without drawing a line.
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
LPoint3 get_pos() const
Retrieves the translation component of the transform.
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...
This class is used to assign the nodes on the mesh.
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...
void add_obstacle_to_mesh(NodePath obstacle)
This function allows the user to dynamically add obstacles to the game environment.
void create_nav_mesh(const char *navmesh_filename)
This function recreates the navigation mesh from the .csv file.
void dynamic_avoid(NodePath obstacle)
This function starts the pathfinding obstacle navigation for the passed in obstacle.
void clear_path()
Helper function to restore the path and mesh to its initial state.
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 set_path_find(const char *navmesh_filename)
This function starts the path finding process after reading the given navigation mesh.
void clear_previous_obstacles()
Helper function to reset the collisions if the obstacle is not on the node anymore.
void do_dynamic_avoid()
This function does the updation of the collisions to the mesh based on the new positions of the obsta...
void assign_neighbor_nodes(const char *navmesh_filename)
This function assigns the neighbor nodes for each main node present in _nav_mesh.
This class implements pathfinding using A* algorithm.
void find_path(Node *src_node, Node *dest_node)
This function initializes the pathfinding process by accepting the source and destination nodes.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.