Panda3D
Loading...
Searching...
No Matches
aiPathFinder.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 aiPathFinder.cxx
10 * @author Deepak, John, Navin
11 * @date 2009-11-10
12 */
13
14#include "aiPathFinder.h"
15
16PathFinder::PathFinder(NavMesh nav_mesh) {
17 _grid = nav_mesh;
18}
19
20PathFinder::~PathFinder() {
21}
22
23/**
24 * This function initializes the pathfinding process by accepting the source
25 * and destination nodes. It then calls the generate_path().
26 */
27void PathFinder::find_path(Node *src_node, Node *dest_node) {
28 _src_node = src_node;
29 _dest_node = dest_node;
30
31 // Add a dummy node as the first element of the open list with score = -1.
32 // Inorder to implement a binary heap the index of the elements should never
33 // be 0.
34 Node *_dummy_node = new Node(-1, -1, LVecBase3(0.0, 0.0, 0.0), 0, 0, 0);
35 _dummy_node->_status = _dummy_node->open;
36 _dummy_node->_score = -1;
37 _open_list.push_back(_dummy_node);
38
39 // Add the source node to the open list.
40 add_to_olist(_src_node);
41
42 // Generate the path.
44}
45
46/**
47 * This function performs the pathfinding process using the A* algorithm. It
48 * updates the openlist and closelist.
49 */
51 // All the A* algorithm is implemented here. The check is > 1 due to the
52 // existence of the dummy node.
53 while(_open_list.size() > 1) {
54 // The first element of the open list will always be the optimal node.
55 // This is because the open list is a binary heap with element having the
56 // smallest score at the top of the heap.
57 Node* nxt_node = _open_list[1];
58
59 if(nxt_node->_grid_x == _dest_node->_grid_x &&
60 nxt_node->_grid_y == _dest_node->_grid_y) {
61 // Remove the optimal node from the top of the heap.
63
64 // add the used node to the closed list.
65 add_to_clist(nxt_node);
66
67 // At this point the destination is reached.
68 return;
69 }
70 else {
71 identify_neighbors(nxt_node);
72
73 // add the used node to the closed list.
74 add_to_clist(nxt_node);
75 }
76 }
77 std::cout << "DESTINATION NOT REACHABLE MATE!" << std::endl;
78 _closed_list.clear();
79}
80
81/**
82 * This function traverses through the 8 neigbors of the parent node and then
83 * adds the neighbors to the _open_list based on A* criteria.
84 */
86 // Remove the parent node from the open_list so that it is not considered
87 // while adding new nodes to the open list heap.
89 for(int i = 0; i < 8; ++i) {
90 if(parent_node->_neighbours[i] != nullptr) {
91 if(parent_node->_neighbours[i]->_status == parent_node->_neighbours[i]->neutral
92 && parent_node->_neighbours[i]->_type == true) {
93 // Link the neighbor to the parent node.
94 parent_node->_neighbours[i]->_prv_node = parent_node;
95 // Calculate and update the score for the node.
96 calc_node_score(parent_node->_neighbours[i]);
97 // Add the neighbor to the open list.
98 add_to_olist(parent_node->_neighbours[i]);
99 }
100 }
101 }
102}
103
104/**
105 * This function calculates the score of each node. Score = Cost +
106 * Heuristics.
107 */
109 nd->_cost = calc_cost_frm_src(nd);
110 nd->_heuristic = calc_heuristic(nd);
111 nd->_score = nd->_cost + nd->_heuristic;
112}
113
114/**
115 * This function calculates the cost of each node by finding out the number of
116 * node traversals required to reach the source node. Diagonal traversals
117 * have cost = 14. Horizontal and vertical traversals have cost = 10.
118 */
120 int cost = 0;
121 Node *start_node = nd;
122 while(start_node->_prv_node != _src_node) {
123 if(is_diagonal_node(start_node)) {
124 cost += 14;
125 }
126 else {
127 cost += 10;
128 }
129 start_node = start_node->_prv_node;
130 }
131 // Add the cost of traversal to the source node also.
132 if(is_diagonal_node(start_node)) {
133 cost += 14;
134 }
135 else {
136 cost += 10;
137 }
138 return cost;
139}
140
141/**
142 * This function calculates the heuristic of the nodes using Manhattan method.
143 * All it does is predict the number of node traversals required to reach the
144 * target node. No diagonal traversals are allowed in this technique.
145 */
147 int row_diff = abs(_dest_node->_grid_x - nd->_grid_x);
148 int col_diff = abs(_dest_node->_grid_y - nd->_grid_y);
149
150 int heuristic = 10 * (row_diff + col_diff);
151 return heuristic;
152}
153
154/**
155 * This function checks if the traversal from a node is diagonal.
156 */
158 // Calculate the row and column differences between child and parent nodes.
159 float row_diff = nd->_grid_x - nd->_prv_node->_grid_x;
160 float col_diff = nd->_grid_y - nd->_prv_node->_grid_y;
161
162 // Check if the relationship between child and parent node is diagonal.
163 if(row_diff == 0 || col_diff == 0) {
164 return false;
165 }
166 else {
167 return true;
168 }
169}
170
171/**
172 * This function adds a node to the open list heap. A binay heap is
173 * maintained to improve the search.
174 */
176 // Variables required to search the binary heap.
177 Node *child_node, *parent_node;
178 int child_idx, parent_idx;
179
180 // Set the status as open.
181 nd->_status = nd->open;
182 // Add the node to the open list.
183 _open_list.push_back(nd);
184
185 // Find the parent and child nodes and create temporary nodes out of them.
186 // In a binary heap the children of a parent node are always i*2 and i*2 +
187 // 1, where i is the index of the parent node in the heap. And hence, the
188 // parent of a node can be easily found out by dividing by 2 and rounding
189 // it.
190 child_idx = _open_list.size() - 1;
191 parent_idx = child_idx / 2;
192 child_node = _open_list[child_idx];
193 parent_node = _open_list[parent_idx];
194
195 // Keep traversing the heap until the lowest element in the list is bubbled
196 // to the top of the heap.
197 while(_open_list[child_idx]->_score <= _open_list[parent_idx]->_score) {
198 // Swap the parent node and the child node.
199 _open_list[parent_idx] = child_node;
200 _open_list[child_idx] = parent_node;
201
202 // Update the new child and parent indices.
203 child_idx = parent_idx;
204 parent_idx = child_idx / 2;
205
206 // Update the new child and parent nodes.
207 child_node = _open_list[child_idx];
208 parent_node = _open_list[parent_idx];
209 }
210
211 // At this point the Node with the smallest score will be at the top of the
212 // heap.
213}
214
215/**
216 * This function removes a node from the open list. During the removal the
217 * binary heap is maintained.
218 */
220 // Variables for maintaining the binary heap.
221 Node *child_node, *child_node_1, *child_node_2;
222 int child_idx, child_idx_1, child_idx_2;
223
224 // Remove the Node at index 1 from the open list binary heap. Note: Node at
225 // index 0 of open list is a dummy node.
226 _open_list.erase(_open_list.begin() + 1);
227
228 if(_open_list.size() > 1) {
229 // Store the last element in the open list to a temp_node.
230 Node *temp_node = _open_list[_open_list.size() - 1];
231
232 // Shift the elements of the open list to the right by 1 element
233 // circularly, excluding element at 0 index.
234 for(int i = _open_list.size() - 1; i > 1; --i) {
235 _open_list[i] = _open_list[i - 1];
236 }
237
238 // Assign the temp_node to 1st element in the open list.
239 _open_list[1] = temp_node;
240
241 // Set the iterator for traversing the node from index 1 in the heap.
242 unsigned int k = 1;
243
244 // This loop traverses down the open list till the node reaches the
245 // correct position in the binary heap.
246 while(true) {
247 if((k * 2 + 1) < _open_list.size()) {
248 // Two children exists for the parent node.
249 child_idx_1 = k * 2;
250 child_idx_2 = k * 2 + 1;
251 child_node_1 = _open_list[child_idx_1];
252 child_node_2 = _open_list[child_idx_2];
253
254 if(_open_list[child_idx_1]->_score < _open_list[child_idx_2]->_score) {
255 if(_open_list[k]->_score > _open_list[child_idx_1]->_score) {
256 // Swap the parent node and the child node.
257 _open_list[child_idx_1] = _open_list[k];
258 _open_list[k] = child_node_1;
259
260 // Update the parent node index.
261 k = child_idx_1;
262 }
263 else {
264 break;
265 }
266 }
267 else {
268 if(_open_list[k]->_score > _open_list[child_idx_2]->_score) {
269 // Swap the parent node and the child node.
270 _open_list[child_idx_2] = _open_list[k];
271 _open_list[k] = child_node_2;
272
273 // Update the parent node index.
274 k = child_idx_2;
275 }
276 else {
277 break;
278 }
279 }
280 }
281 else if((k * 2) < _open_list.size()) {
282 // Only one child exists for the parent node.
283 child_idx = k * 2;
284 child_node = _open_list[child_idx];
285
286 if(_open_list[k]->_score > _open_list[child_idx]->_score) {
287 // Swap the parent node and the child node.
288 _open_list[child_idx] = _open_list[k];
289 _open_list[k] = child_node;
290
291 // Update the parent node index.
292 k = child_idx;
293 }
294 else {
295 break;
296 }
297 }
298 else {
299 // No children exists.
300 break;
301 }
302 }
303 }
304
305 // At this point the Node was succesfully removed and the binary heap re-
306 // arranged.
307}
308
309/**
310 * This function adds a node to the closed list.
311 */
313 // Set the status as closed.
314 nd->_status = nd->close;
315 // Add the node to the close list.
316 _closed_list.push_back(nd);
317}
318
319/**
320 * This function removes a node from the closed list.
321 */
323 for(unsigned int i = 0; i < _closed_list.size(); ++i) {
324 if(_closed_list[i]->_grid_x == r && _closed_list[i]->_grid_y == c) {
325 _closed_list.erase(_closed_list.begin() + i);
326 break;
327 }
328 }
329}
330
331/**
332 * This function allows the user to pass a position and it returns the
333 * corresponding node on the navigation mesh. A very useful function as it
334 * allows for dynamic updation of the mesh based on position.
335 */
336Node* find_in_mesh(NavMesh nav_mesh, LVecBase3 pos, int grid_size) {
337 int size = grid_size;
338 float x = pos[0];
339 float y = pos[1];
340
341 for(int i = 0; i < size; ++i) {
342 for(int j = 0; j < size; ++j) {
343 if(nav_mesh[i][j] != nullptr && nav_mesh[i][j]->contains(x, y)) {
344 return(nav_mesh[i][j]);
345 }
346 }
347 }
348 return nullptr;
349}
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...
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
This class is used to assign the nodes on the mesh.
Definition meshNode.h:16
void remove_from_clist(int r, int c)
This function removes a node from the closed list.
bool is_diagonal_node(Node *nd)
This function checks if the traversal from a node is diagonal.
void add_to_clist(Node *nd)
This function adds a node to the closed list.
int calc_cost_frm_src(Node *nd)
This function calculates the cost of each node by finding out the number of node traversals required ...
void add_to_olist(Node *nd)
This function adds a node to the open list heap.
void generate_path()
This function performs the pathfinding process using the A* algorithm.
int calc_heuristic(Node *nd)
This function calculates the heuristic of the nodes using Manhattan method.
void identify_neighbors(Node *nd)
This function traverses through the 8 neigbors of the parent node and then adds the neighbors to the ...
void calc_node_score(Node *nd)
This function calculates the score of each node.
void remove_from_olist()
This function removes a node from the open list.
void find_path(Node *src_node, Node *dest_node)
This function initializes the pathfinding process by accepting the source and destination nodes.