16 PathFinder::PathFinder(NavMesh nav_mesh) {
20 PathFinder::~PathFinder() {
29 _dest_node = dest_node;
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);
53 while(_open_list.size() > 1) {
57 Node* nxt_node = _open_list[1];
59 if(nxt_node->_grid_x == _dest_node->_grid_x &&
60 nxt_node->_grid_y == _dest_node->_grid_y) {
77 std::cout <<
"DESTINATION NOT REACHABLE MATE!" << std::endl;
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) {
94 parent_node->_neighbours[i]->_prv_node = parent_node;
111 nd->_score = nd->_cost + nd->_heuristic;
121 Node *start_node = nd;
122 while(start_node->_prv_node != _src_node) {
129 start_node = start_node->_prv_node;
147 int row_diff = abs(_dest_node->_grid_x - nd->_grid_x);
148 int col_diff = abs(_dest_node->_grid_y - nd->_grid_y);
150 int heuristic = 10 * (row_diff + col_diff);
159 float row_diff = nd->_grid_x - nd->_prv_node->_grid_x;
160 float col_diff = nd->_grid_y - nd->_prv_node->_grid_y;
163 if(row_diff == 0 || col_diff == 0) {
177 Node *child_node, *parent_node;
178 int child_idx, parent_idx;
181 nd->_status = nd->open;
183 _open_list.push_back(nd);
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];
197 while(_open_list[child_idx]->_score <= _open_list[parent_idx]->_score) {
199 _open_list[parent_idx] = child_node;
200 _open_list[child_idx] = parent_node;
203 child_idx = parent_idx;
204 parent_idx = child_idx / 2;
207 child_node = _open_list[child_idx];
208 parent_node = _open_list[parent_idx];
221 Node *child_node, *child_node_1, *child_node_2;
222 int child_idx, child_idx_1, child_idx_2;
226 _open_list.erase(_open_list.begin() + 1);
228 if(_open_list.size() > 1) {
230 Node *temp_node = _open_list[_open_list.size() - 1];
234 for(
int i = _open_list.size() - 1; i > 1; --i) {
235 _open_list[i] = _open_list[i - 1];
239 _open_list[1] = temp_node;
247 if((k * 2 + 1) < _open_list.size()) {
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];
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) {
257 _open_list[child_idx_1] = _open_list[k];
258 _open_list[k] = child_node_1;
268 if(_open_list[k]->_score > _open_list[child_idx_2]->_score) {
270 _open_list[child_idx_2] = _open_list[k];
271 _open_list[k] = child_node_2;
281 else if((k * 2) < _open_list.size()) {
284 child_node = _open_list[child_idx];
286 if(_open_list[k]->_score > _open_list[child_idx]->_score) {
288 _open_list[child_idx] = _open_list[k];
289 _open_list[k] = child_node;
314 nd->_status = nd->close;
316 _closed_list.push_back(nd);
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);
337 int size = grid_size;
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]);
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.
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.