Panda3D
pathFind.cxx
Go to the documentation of this file.
1 /**
2  * PANDA 3D SOFTWARE
3  * Copyright (c) Carnegie Mellon University. All rights reserved.
4  *
5  * All use of this software is subject to the terms of the revised BSD
6  * license. You should have received a copy of this license along
7  * with this source code in a file named "LICENSE."
8  *
9  * @file pathFind.cxx
10  * @author Deepak, John, Navin
11  * @date 2009-10-12
12  */
13 
14 #include "pathFind.h"
15 
16 #include "pathFollow.h"
17 
18 using std::cout;
19 using std::endl;
20 using std::string;
21 
22 PathFind::PathFind(AICharacter *ai_ch) {
23  _ai_char = ai_ch;
24 
25  _parent = new GeomNode("parent");
26  _ai_char->_window_render.attach_new_node(_parent);
27 
28  _pen = new LineSegs("pen");
29  _pen->set_color(1.0, 0.0, 0.0);
30  _pen->set_thickness(2.0);
31 
32  _path_finder_obj = nullptr;
33  _dynamic_avoid = false;
34 }
35 
36 PathFind::~PathFind() {
37 }
38 
39 /**
40  * This function recreates the navigation mesh from the .csv file
41  */
42 void PathFind::create_nav_mesh(const char* navmesh_filename) {
43  // Stage variables.
44  int grid_x, grid_y;
45  float l, w, h;
46  LVecBase3 position;
47 
48  // Variable to hold line data read from file.
49  string line;
50 
51  // Array for storing data members obtained from each line of the file.
52  string fields[10];
53 
54  // Open data file for reading.
55  std::ifstream nav_mesh_file (navmesh_filename);
56 
57  if(nav_mesh_file.is_open()) {
58  // Capture the grid size from the file.
59  getline(nav_mesh_file, line);
60  int pos = line.find(",");
61  _grid_size = atoi((line.substr(pos + 1)).c_str());
62 
63  // Initialize the stage mesh with NULL nodes.
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);
68  }
69  }
70 
71  // Ignore the header of the navmesh.csv file.
72  getline(nav_mesh_file, line);
73 
74  // Begin reading data from the file.
75  while(!nav_mesh_file.eof()) {
76  getline(nav_mesh_file, line);
77  std::stringstream linestream (line);
78 
79  // Stores all the data members in the line to the array. Data
80  // structure:
81  // NULL,NodeType,GridX,GridY,Length,Width,Height,PosX,PosY,PosZ
82  for(int i = 0; i < 10; ++i) {
83  getline(linestream, fields[i], ',');
84  }
85 
86  // Populate the main nodes into stage mesh.
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()));
94 
95  Node *stage_node = new Node(grid_x, grid_y, position, w, l, h);
96 
97 
98  _nav_mesh[grid_y][grid_x] = stage_node;
99  }
100  else if(fields[0] == "") {
101  // End of file reached at this point.
102  nav_mesh_file.close();
103 
104  // Assign the neighbor nodes for each of the main nodes that just got
105  // populated into the stage mesh.
106  assign_neighbor_nodes(navmesh_filename);
107  }
108  }
109  }
110  else {
111  cout<<"error opening navmesh.csv file!"<<endl;
112  }
113 }
114 
115 /**
116  * This function assigns the neighbor nodes for each main node present in
117  * _nav_mesh.
118  */
119 void PathFind::assign_neighbor_nodes(const char* navmesh_filename){
120  std::ifstream nav_mesh_file (navmesh_filename);
121 
122  // Stage variables.
123  int gd_x, gd_y, gd_xn, gd_yn;
124  string ln;
125  string fields[10];
126  string fields_n[10];
127 
128  if(nav_mesh_file.is_open()) {
129  getline(nav_mesh_file, ln); // Get rid of grid size line.
130  getline(nav_mesh_file, ln); // Get rid of the header.
131 
132  while(!nav_mesh_file.eof()) {
133  getline(nav_mesh_file, ln); // Gets main node data only. No neighbor nodes.
134  std::stringstream linestream (ln);
135  for(int i = 0; i < 10; ++i) {
136  getline(linestream, fields[i], ',');
137  }
138  if(fields[0] == "0" && fields[1] == "0") {
139  // Usable main node.
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); // Gets neighbor node data only. No main nodes.
144  std::stringstream linestream_n (ln);
145  for(int j = 0; j < 10; ++j) {
146  getline(linestream_n, fields_n[j], ',');
147  }
148  gd_xn = atoi(fields_n[2].c_str());
149  gd_yn = atoi(fields_n[3].c_str());
150 
151  if(fields_n[0] == "0" && fields_n[1] == "1") {
152  // Usable neighbor for main node. TODO: The indices of the vector
153  // are inverted when compared to the values of the nodes on actual
154  // grid. Fix this!
155  _nav_mesh[gd_y][gd_x]->_neighbours[i] = _nav_mesh[gd_yn][gd_xn];
156  }
157  else if(fields_n[0] == "1" && fields_n[1] == "1") {
158  // NULL neighbor.
159  _nav_mesh[gd_y][gd_x]->_neighbours[i] = nullptr;
160  }
161  else {
162  cout<<"Warning: Corrupt data!"<<endl;
163  }
164  }
165  }
166  else if(fields[0] == "") {
167  // End of file reached at this point.
168  nav_mesh_file.close();
169  }
170  }
171  }
172  else {
173  cout<<"error opening navmesh.csv file!"<<endl;
174  }
175 }
176 
177 /**
178  * This function starts the path finding process after reading the given
179  * navigation mesh.
180  */
181 void PathFind::set_path_find(const char* navmesh_filename) {
182  create_nav_mesh(navmesh_filename);
183 
184  if(_ai_char->_steering->_path_follow_obj) {
185  _ai_char->_steering->remove_ai("pathfollow");
186  }
187 
188  _ai_char->_steering->path_follow(1.0f);
189 
190  if(_path_finder_obj) {
191  delete _path_finder_obj;
192  _path_finder_obj = nullptr;
193  }
194 
195  _path_finder_obj = new PathFinder(_nav_mesh);
196 }
197 
198 /**
199  * This function checks for the source and target in the navigation mesh for
200  * its availability and then finds the best path via the A* algorithm Then it
201  * calls the path follower to make the object follow the path.
202  */
203 void PathFind::path_find(LVecBase3 pos, string type) {
204  if(type == "addPath") {
205  if(_ai_char->_steering->_path_follow_obj) {
206  _ai_char->_steering->remove_ai("pathfollow");
207  }
208 
209  _ai_char->_steering->path_follow(1.0f);
210  }
211 
212  clear_path();
213 
214  Node* src = find_in_mesh(_nav_mesh, _ai_char->_ai_char_np.get_pos(_ai_char->_window_render), _grid_size);
215 
216  if(src == nullptr) {
217  cout<<"couldnt find source"<<endl;
218  }
219 
220  Node* dst = find_in_mesh(_nav_mesh, pos, _grid_size);
221 
222  if(dst == nullptr) {
223  cout<<"couldnt find destination"<<endl;
224  }
225 
226  if(src != nullptr && dst != nullptr) {
227  _path_finder_obj->find_path(src, dst);
228  trace_path(src);
229  }
230 
231  if(!_ai_char->_steering->_path_follow_obj->_start) {
232  _ai_char->_steering->start_follow();
233  }
234 }
235 
236 /**
237  * This function checks for the source and target in the navigation mesh for
238  * its availability and then finds the best path via the A* algorithm Then it
239  * calls the path follower to make the object follow the path.
240  */
241 void PathFind::path_find(NodePath target, string type) {
242  if(type == "addPath") {
243  if(_ai_char->_steering->_path_follow_obj) {
244  _ai_char->_steering->remove_ai("pathfollow");
245  }
246 
247  _ai_char->_steering->path_follow(1.0f);
248  }
249 
250  clear_path();
251 
252  _path_find_target = target;
253  _prev_position = target.get_pos(_ai_char->_window_render);
254 
255  Node* src = find_in_mesh(_nav_mesh, _ai_char->_ai_char_np.get_pos(_ai_char->_window_render), _grid_size);
256 
257  if(src == nullptr) {
258  cout<<"couldnt find source"<<endl;
259  }
260 
261  Node* dst = find_in_mesh(_nav_mesh, _prev_position, _grid_size);
262 
263  if(dst == nullptr) {
264  cout<<"couldnt find destination"<<endl;
265  }
266 
267  if(src != nullptr && dst != nullptr) {
268  _path_finder_obj->find_path(src, dst);
269  trace_path(src);
270  }
271 
272  if(_ai_char->_steering->_path_follow_obj!=nullptr) {
273  if(!_ai_char->_steering->_path_follow_obj->_start) {
274  _ai_char->_steering->start_follow("pathfind");
275  }
276  }
277 }
278 
279 /**
280  * Helper function to restore the path and mesh to its initial state
281  */
283  // Initialize to zero
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;
292  }
293  }
294  }
295 
296  if(_path_finder_obj) {
297  _path_finder_obj->_open_list.clear();
298  _path_finder_obj->_closed_list.clear();
299  }
300 }
301 
302 /**
303  * This function is the function which sends the path information one by one
304  * to the path follower so that it can store the path needed to be traversed
305  * by the pathfinding object
306  */
308  if(_ai_char->_pf_guide) {
309  _parent->remove_all_children();
310  }
311  else {
312  _parent->remove_all_children();
313  }
314 
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);
321  PT(GeomNode) gnode = _pen->create();
322  _parent->add_child(gnode);
323  }
324  _ai_char->_steering->add_to_path(traversor->_position);
325  traversor = traversor->_prv_node;
326  }
327  }
328 }
329 
330 /**
331  * This function allows the user to dynamically add obstacles to the game
332  * environment. The function will update the nodes within the bounding volume
333  * of the obstacle as non-traversable. Hence will not be considered by the
334  * pathfinding algorithm.
335  */
337  PT(BoundingVolume) np_bounds = obstacle.get_bounds();
338  CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere();
339 
340  Node* temp = find_in_mesh(_nav_mesh, obstacle.get_pos(), _grid_size);
341 
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();
347 
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);
356  }
357  }
358  }
359  }
360  }
361 }
362 
363 /**
364  * This function does the updation of the collisions to the mesh based on the
365  * new positions of the obstacles.
366  */
369  _previous_obstacles.clear();
370  for(unsigned int i = 0; i < _dynamic_obstacle.size(); ++i) {
371  add_obstacle_to_mesh(_dynamic_obstacle[i]);
372  }
373 }
374 
375 /**
376  * Helper function to reset the collisions if the obstacle is not on the node
377  * anymore
378  */
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;
382  }
383 }
384 
385 /**
386  * This function starts the pathfinding obstacle navigation for the passed in
387  * obstacle.
388  */
390  _dynamic_avoid = true;
391  _dynamic_obstacle.insert(_dynamic_obstacle.end(), obstacle);
392 }
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.
Definition: geomNode.h:34
Encapsulates creation of a series of connected or disconnected line segments or points,...
Definition: lineSegs.h:33
void set_thickness(PN_stdfloat thick)
Establishes the line thickness or point size in pixels that will be assigned to all lines and points ...
Definition: lineSegs.I:74
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...
Definition: lineSegs.I:94
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 ...
Definition: lineSegs.I:56
GeomNode * create(bool dynamic=false)
Creates a new GeomNode that will render the series of line segments and points described via calls to...
Definition: lineSegs.I:108
void move_to(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Moves the pen to the given point without drawing a line.
Definition: lineSegs.I:84
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition: nodePath.h:159
LPoint3 get_pos() const
Retrieves the translation component of the transform.
Definition: nodePath.cxx:1044
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...
Definition: nodePath.cxx:600
This class is used to assign the nodes on the mesh.
Definition: meshNode.h:16
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...
Definition: pathFind.cxx:203
void add_obstacle_to_mesh(NodePath obstacle)
This function allows the user to dynamically add obstacles to the game environment.
Definition: pathFind.cxx:336
void create_nav_mesh(const char *navmesh_filename)
This function recreates the navigation mesh from the .csv file.
Definition: pathFind.cxx:42
void dynamic_avoid(NodePath obstacle)
This function starts the pathfinding obstacle navigation for the passed in obstacle.
Definition: pathFind.cxx:389
void clear_path()
Helper function to restore the path and mesh to its initial state.
Definition: pathFind.cxx:282
void trace_path(Node *src)
This function is the function which sends the path information one by one to the path follower so tha...
Definition: pathFind.cxx:307
void set_path_find(const char *navmesh_filename)
This function starts the path finding process after reading the given navigation mesh.
Definition: pathFind.cxx:181
void clear_previous_obstacles()
Helper function to reset the collisions if the obstacle is not on the node anymore.
Definition: pathFind.cxx:379
void do_dynamic_avoid()
This function does the updation of the collisions to the mesh based on the new positions of the obsta...
Definition: pathFind.cxx:367
void assign_neighbor_nodes(const char *navmesh_filename)
This function assigns the neighbor nodes for each main node present in _nav_mesh.
Definition: pathFind.cxx:119
This class implements pathfinding using A* algorithm.
Definition: aiPathFinder.h:31
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.