Panda3D
Loading...
Searching...
No Matches
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
18using std::cout;
19using std::endl;
20using std::string;
21
22PathFind::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
36PathFind::~PathFind() {
37}
38
39/**
40 * This function recreates the navigation mesh from the .csv file
41 */
42void 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 */
119void 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 */
181void 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 */
203void 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 */
241void 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.
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:599
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.
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.