Panda3D
 All Classes Functions Variables Enumerations
pathFind.cxx
1 
2 #include "pathFind.h"
3 
4 PathFind::PathFind(AICharacter *ai_ch) {
5  _ai_char = ai_ch;
6 
7  _parent = new GeomNode("parent");
8  _ai_char->_window_render.attach_new_node(_parent);
9 
10  _pen = new LineSegs("pen");
11  _pen->set_color(1.0, 0.0, 0.0);
12  _pen->set_thickness(2.0);
13 
14  _path_finder_obj = NULL;
15  _dynamic_avoid = false;
16 }
17 
18 PathFind::~PathFind() {
19 }
20 
21 /////////////////////////////////////////////////////////////////////////////////
22 //
23 // Function : create_nav_mesh
24 // Description : This function recreates the navigation mesh from the .csv file
25 
26 /////////////////////////////////////////////////////////////////////////////////
27 
28 void PathFind::create_nav_mesh(const char* navmesh_filename) {
29  // Stage variables.
30  int grid_x, grid_y;
31  float l, w, h;
32  LVecBase3 position;
33 
34  // Variable to hold line data read from file.
35  string line;
36 
37  // Array for storing data members obtained from each line of the file.
38  string fields[10];
39 
40  // Open data file for reading.
41  ifstream nav_mesh_file (navmesh_filename);
42 
43  if(nav_mesh_file.is_open()) {
44  // Capture the grid size from the file.
45  getline(nav_mesh_file, line);
46  int pos = line.find(",");
47  _grid_size = atoi((line.substr(pos + 1)).c_str());
48 
49  // Initialize the stage mesh with NULL nodes.
50  for(int r = 0; r < _grid_size; ++r) {
51  _nav_mesh.push_back(vector<Node*>());
52  for(int c = 0; c < _grid_size; ++c) {
53  _nav_mesh[r].push_back(NULL);
54  }
55  }
56 
57  // Ignore the header of the navmesh.csv file.
58  getline(nav_mesh_file, line);
59 
60  // Begin reading data from the file.
61  while(!nav_mesh_file.eof()) {
62  getline(nav_mesh_file, line);
63  stringstream linestream (line);
64 
65  // Stores all the data members in the line to the array.
66  // Data structure: NULL,NodeType,GridX,GridY,Length,Width,Height,PosX,PosY,PosZ
67  for(int i = 0; i < 10; ++i) {
68  getline(linestream, fields[i], ',');
69  }
70 
71  // Populate the main nodes into stage mesh.
72  if(fields[0] == "0" && fields[1] == "0") {
73  grid_x = atoi(fields[2].c_str());
74  grid_y = atoi(fields[3].c_str());
75  l = atof(fields[4].c_str());
76  w = atof(fields[5].c_str());
77  h = atof(fields[6].c_str());
78  position = LVecBase3(atof(fields[7].c_str()), atof(fields[8].c_str()), atof(fields[9].c_str()));
79 
80  Node *stage_node = new Node(grid_x, grid_y, position, w, l, h);
81 
82 
83  _nav_mesh[grid_y][grid_x] = stage_node;
84  }
85  else if(fields[0] == "") {
86  // End of file reached at this point.
87  nav_mesh_file.close();
88 
89  // Assign the neighbor nodes for each of the main nodes that just got populated into the stage mesh.
90  assign_neighbor_nodes(navmesh_filename);
91  }
92  }
93  }
94  else {
95  cout<<"error opening navmesh.csv file!"<<endl;
96  }
97 }
98 
99 /////////////////////////////////////////////////////////////////////////////////
100 //
101 // Function : assign_neighbor_nodes
102 // Description : This function assigns the neighbor nodes for each main node present in
103 // _nav_mesh.
104 
105 /////////////////////////////////////////////////////////////////////////////////
106 
107 void PathFind::assign_neighbor_nodes(const char* navmesh_filename){
108  ifstream nav_mesh_file (navmesh_filename);
109 
110  // Stage variables.
111  int gd_x, gd_y, gd_xn, gd_yn;
112  string ln;
113  string fields[10];
114  string fields_n[10];
115 
116  if(nav_mesh_file.is_open()) {
117  getline(nav_mesh_file, ln); // Get rid of grid size line.
118  getline(nav_mesh_file, ln); // Get rid of the header.
119 
120  while(!nav_mesh_file.eof()) {
121  getline(nav_mesh_file, ln); // Gets main node data only. No neighbor nodes.
122  stringstream linestream (ln);
123  for(int i = 0; i < 10; ++i) {
124  getline(linestream, fields[i], ',');
125  }
126  if(fields[0] == "0" && fields[1] == "0") {
127  // Usable main node.
128  gd_x = atoi(fields[2].c_str());
129  gd_y = atoi(fields[3].c_str());
130  for(int i = 0; i < 8; ++i) {
131  getline(nav_mesh_file, ln); // Gets neighbor node data only. No main nodes.
132  stringstream linestream_n (ln);
133  for(int j = 0; j < 10; ++j) {
134  getline(linestream_n, fields_n[j], ',');
135  }
136  gd_xn = atoi(fields_n[2].c_str());
137  gd_yn = atoi(fields_n[3].c_str());
138 
139  if(fields_n[0] == "0" && fields_n[1] == "1") {
140  // Usable neighbor for main node.
141  // TODO: The indices of the vector are inverted when compared to the values of the nodes on actual grid. Fix this!
142  _nav_mesh[gd_y][gd_x]->_neighbours[i] = _nav_mesh[gd_yn][gd_xn];
143  }
144  else if(fields_n[0] == "1" && fields_n[1] == "1") {
145  // NULL neighbor.
146  _nav_mesh[gd_y][gd_x]->_neighbours[i] = NULL;
147  }
148  else {
149  cout<<"Warning: Corrupt data!"<<endl;
150  }
151  }
152  }
153  else if(fields[0] == "") {
154  // End of file reached at this point.
155  nav_mesh_file.close();
156  }
157  }
158  }
159  else {
160  cout<<"error opening navmesh.csv file!"<<endl;
161  }
162 }
163 
164 ///////////////////////////////////////////////////////////////////////////////////////
165 //
166 // Function : set_path_find
167 // Description : This function starts the path finding process after reading the given
168 // navigation mesh.
169 
170 ///////////////////////////////////////////////////////////////////////////////////////
171 
172 
173 void PathFind::set_path_find(const char* navmesh_filename) {
174  create_nav_mesh(navmesh_filename);
175 
176  if(_ai_char->_steering->_path_follow_obj) {
177  _ai_char->_steering->remove_ai("pathfollow");
178  }
179 
180  _ai_char->_steering->path_follow(1.0f);
181 
182  if(_path_finder_obj) {
183  delete _path_finder_obj;
184  _path_finder_obj = NULL;
185  }
186 
187  _path_finder_obj = new PathFinder(_nav_mesh);
188 }
189 
190 ///////////////////////////////////////////////////////////////////////////////////////
191 //
192 // Function : path_find (for pathfinding towards a static position)
193 // Description : This function checks for the source and target in the navigation mesh
194 // for its availability and then finds the best path via the A* algorithm
195 // Then it calls the path follower to make the object follow the path.
196 
197 ///////////////////////////////////////////////////////////////////////////////////////
198 
199 
200 void PathFind::path_find(LVecBase3 pos, string type) {
201  if(type == "addPath") {
202  if(_ai_char->_steering->_path_follow_obj) {
203  _ai_char->_steering->remove_ai("pathfollow");
204  }
205 
206  _ai_char->_steering->path_follow(1.0f);
207  }
208 
209  clear_path();
210 
211  Node* src = find_in_mesh(_nav_mesh, _ai_char->_ai_char_np.get_pos(_ai_char->_window_render), _grid_size);
212 
213  if(src == NULL) {
214  cout<<"couldnt find source"<<endl;
215  }
216 
217  Node* dst = find_in_mesh(_nav_mesh, pos, _grid_size);
218 
219  if(dst == NULL) {
220  cout<<"couldnt find destination"<<endl;
221  }
222 
223  if(src != NULL && dst != NULL) {
224  _path_finder_obj->find_path(src, dst);
225  trace_path(src);
226  }
227 
228  if(!_ai_char->_steering->_path_follow_obj->_start) {
229  _ai_char->_steering->start_follow();
230  }
231 }
232 
233 ///////////////////////////////////////////////////////////////////////////////////////
234 //
235 // Function : path_find (for pathfinding towards a moving target (a NodePath))
236 // Description : This function checks for the source and target in the navigation mesh
237 // for its availability and then finds the best path via the A* algorithm
238 // Then it calls the path follower to make the object follow the path.
239 
240 ///////////////////////////////////////////////////////////////////////////////////////
241 
242 void PathFind::path_find(NodePath target, string type) {
243  if(type == "addPath") {
244  if(_ai_char->_steering->_path_follow_obj) {
245  _ai_char->_steering->remove_ai("pathfollow");
246  }
247 
248  _ai_char->_steering->path_follow(1.0f);
249  }
250 
251  clear_path();
252 
253  _path_find_target = target;
254  _prev_position = target.get_pos(_ai_char->_window_render);
255 
256  Node* src = find_in_mesh(_nav_mesh, _ai_char->_ai_char_np.get_pos(_ai_char->_window_render), _grid_size);
257 
258  if(src == NULL) {
259  cout<<"couldnt find source"<<endl;
260  }
261 
262  Node* dst = find_in_mesh(_nav_mesh, _prev_position, _grid_size);
263 
264  if(dst == NULL) {
265  cout<<"couldnt find destination"<<endl;
266  }
267 
268  if(src != NULL && dst != NULL) {
269  _path_finder_obj->find_path(src, dst);
270  trace_path(src);
271  }
272 
273  if(_ai_char->_steering->_path_follow_obj!=NULL) {
274  if(!_ai_char->_steering->_path_follow_obj->_start) {
275  _ai_char->_steering->start_follow("pathfind");
276  }
277  }
278 }
279 
280 ///////////////////////////////////////////////////////////////////////////////////////
281 //
282 // Function : clear_path
283 // Description : Helper function to restore the path and mesh to its initial state
284 
285 ///////////////////////////////////////////////////////////////////////////////////////
286 
288  // Initialize to zero
289  for(int i = 0; i < _grid_size; ++i) {
290  for(int j = 0; j < _grid_size; ++j) {
291  if(_nav_mesh[i][j] != NULL) {
292  _nav_mesh[i][j]->_status = _nav_mesh[i][j]->neutral;
293  _nav_mesh[i][j]->_cost = 0;
294  _nav_mesh[i][j]->_heuristic = 0;
295  _nav_mesh[i][j]->_score = 0;
296  _nav_mesh[i][j]->_prv_node = NULL;
297  }
298  }
299  }
300 
301  if(_path_finder_obj) {
302  _path_finder_obj->_open_list.clear();
303  _path_finder_obj->_closed_list.clear();
304  }
305 }
306 
307 ///////////////////////////////////////////////////////////////////////////////////////
308 //
309 // Function : trace_path
310 // Description : This function is the function which sends the path information one by
311 // one to the path follower so that it can store the path needed to be
312 // traversed by the pathfinding object
313 
314 ///////////////////////////////////////////////////////////////////////////////////////
315 
317  if(_ai_char->_pf_guide) {
318  _parent->remove_all_children();
319  }
320  else {
321  _parent->remove_all_children();
322  }
323 
324  if(_path_finder_obj->_closed_list.size() > 0) {
325  Node *traversor = _path_finder_obj->_closed_list[_path_finder_obj->_closed_list.size() - 0.5];
326  while(traversor != src) {
327  if(_ai_char->_pf_guide) {
328  _pen->move_to(traversor->_position.get_x(), traversor->_position.get_y(), 1);
329  _pen->draw_to(traversor->_prv_node->_position.get_x(), traversor->_prv_node->_position.get_y(), 0.5);
330  PT(GeomNode) gnode = _pen->create();
331  _parent->add_child(gnode);
332  }
333  _ai_char->_steering->add_to_path(traversor->_position);
334  traversor = traversor->_prv_node;
335  }
336  }
337 }
338 
339 ///////////////////////////////////////////////////////////////////////////////////////
340 //
341 // Function : add_obstacle_to_mesh
342 // Description : This function allows the user to dynamically add obstacles to the
343 // game environment. The function will update the nodes within the
344 // bounding volume of the obstacle as non-traversable. Hence will not be
345 // considered by the pathfinding algorithm.
346 
347 ///////////////////////////////////////////////////////////////////////////////////////
348 
350  PT(BoundingVolume) np_bounds = obstacle.get_bounds();
351  CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere();
352 
353  Node* temp = find_in_mesh(_nav_mesh, obstacle.get_pos(), _grid_size);
354 
355  if(temp != NULL) {
356  float left = temp->_position.get_x() - np_sphere->get_radius();
357  float right = temp->_position.get_x() + np_sphere->get_radius();
358  float top = temp->_position.get_y() + np_sphere->get_radius();
359  float down = temp->_position.get_y() - np_sphere->get_radius();
360 
361  for(int i = 0; i < _grid_size; ++i) {
362  for(int j = 0; j < _grid_size; ++j) {
363  if(_nav_mesh[i][j] != NULL && _nav_mesh[i][j]->_type == true) {
364  if(_nav_mesh[i][j]->_position.get_x() >= left && _nav_mesh[i][j]->_position.get_x() <= right &&
365  _nav_mesh[i][j]->_position.get_y() >= down && _nav_mesh[i][j]->_position.get_y() <= top) {
366  _nav_mesh[i][j]->_type = false;
367  _previous_obstacles.insert(_previous_obstacles.end(), i);
368  _previous_obstacles.insert(_previous_obstacles.end(), j);
369  }
370  }
371  }
372  }
373  }
374 }
375 
376 ///////////////////////////////////////////////////////////////////////////////////////
377 //
378 // Function : do_dynamic_avoid()
379 // Description : This function does the updation of the collisions to the mesh based
380 // on the new positions of the obstacles.
381 
382 ///////////////////////////////////////////////////////////////////////////////////////
383 
386  _previous_obstacles.clear();
387  for(unsigned int i = 0; i < _dynamic_obstacle.size(); ++i) {
388  add_obstacle_to_mesh(_dynamic_obstacle[i]);
389  }
390 }
391 
392 ///////////////////////////////////////////////////////////////////////////////////////
393 //
394 // Function : clear_previous_obstacles()
395 // Description : Helper function to reset the collisions if the obstacle is not on the
396 // node anymore
397 
398 ///////////////////////////////////////////////////////////////////////////////////////
399 
401  for(unsigned int i = 0; i < _previous_obstacles.size(); i = i + 2) {
402  _nav_mesh[_previous_obstacles[i]][_previous_obstacles[i + 1]]->_type = true;
403  }
404 }
405 
406 ///////////////////////////////////////////////////////////////////////////////////////
407 //
408 // Function : dynamic_avoid
409 // Description : This function starts the pathfinding obstacle navigation for the
410 // passed in obstacle.
411 
412 ///////////////////////////////////////////////////////////////////////////////////////
413 
415  _dynamic_avoid = true;
416  _dynamic_obstacle.insert(_dynamic_obstacle.end(), obstacle);
417 }
void clear_path()
Helper function to restore the path and mesh to its initial state.
Definition: pathFind.cxx:287
void clear_previous_obstacles()
Helper function to reset the collisions if the obstacle is not on the node anymore.
Definition: pathFind.cxx:400
void add_to_path(LVecBase3 pos)
This function adds positions to the path to follow.
This is the base class for all three-component vectors and points.
Definition: lvecBase3.h:105
void dynamic_avoid(NodePath obstacle)
This function starts the pathfinding obstacle navigation for the passed in obstacle.
Definition: pathFind.cxx:414
This class is used to assign the nodes on the mesh.
Definition: meshNode.h:18
This defines a bounding sphere, consisting of a center and a radius.
This class implements pathfinding using A* algorithm.
Definition: aiPathFinder.h:36
void create_nav_mesh(const char *navmesh_filename)
This function recreates the navigation mesh from the .csv file.
Definition: pathFind.cxx:28
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:137
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 ...
Definition: lineSegs.I:68
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:92
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.
Definition: lineSegs.I:105
Encapsulates creation of a series of connected or disconnected line segments or points, for drawing paths or rays.
Definition: lineSegs.h:36
LPoint3 get_pos() const
Retrieves the translation component of the transform.
Definition: nodePath.cxx:1178
void add_obstacle_to_mesh(NodePath obstacle)
This function allows the user to dynamically add obstacles to the game environment.
Definition: pathFind.cxx:349
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:107
void path_find(LVecBase3 pos, string type="normal")
This function checks for the source and target in the navigation mesh for its availability and then f...
Definition: pathFind.cxx:200
void remove_ai(string ai_type)
This function removes individual or all the AIs.
void draw_to(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Draws a line segment from the pen&#39;s last position (the last call to move_to or draw_to) to the indica...
Definition: lineSegs.I:119
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:316
void path_follow(float follow_wt)
This function activates path following.
void start_follow(string type="normal")
This function starts the path follower.
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:384
void add_child(PandaNode *child_node, int sort=0, Thread *current_thread=Thread::get_current_thread())
Adds a new child to the node.
Definition: pandaNode.cxx:654
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition: nodePath.h:165
A node that holds Geom objects, renderable pieces of geometry.
Definition: geomNode.h:37
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:723
void set_path_find(const char *navmesh_filename)
This function starts the path finding process after reading the given navigation mesh.
Definition: pathFind.cxx:173