aiPathFinder.cxx

Go to the documentation of this file.

/**
 * PANDA 3D SOFTWARE
 * Copyright (c) Carnegie Mellon University.  All rights reserved.
 *
 * All use of this software is subject to the terms of the revised BSD
 * license.  You should have received a copy of this license along
 * with this source code in a file named "LICENSE."
 *
 * @file aiPathFinder.cxx
 * @author Deepak, John, Navin
 * @date 2009-11-10
 */

#include "aiPathFinder.h"

PathFinder::PathFinder(NavMesh nav_mesh) {
  _grid = nav_mesh;
}

PathFinder::~PathFinder() {
}

/**
 * This function initializes the pathfinding process by accepting the source
 * and destination nodes.  It then calls the generate_path().
 */
void PathFinder::find_path(Node *src_node, Node *dest_node) {
  _src_node = src_node;
  _dest_node = dest_node;

  // Add a dummy node as the first element of the open list with score = -1.
  // Inorder to implement a binary heap the index of the elements should never
  // be 0.
  Node *_dummy_node = new Node(-1, -1, LVecBase3(0.0, 0.0, 0.0), 0, 0, 0);
  _dummy_node->_status = _dummy_node->open;
  _dummy_node->_score = -1;
  _open_list.push_back(_dummy_node);

  // Add the source node to the open list.
  add_to_olist(_src_node);

  // Generate the path.
  generate_path();
}

/**
 * This function performs the pathfinding process using the A* algorithm.  It
 * updates the openlist and closelist.
 */
void PathFinder::generate_path() {
  // All the A* algorithm is implemented here.  The check is > 1 due to the
  // existence of the dummy node.
  while(_open_list.size() > 1) {
    // The first element of the open list will always be the optimal node.
    // This is because the open list is a binary heap with element having the
    // smallest score at the top of the heap.
    Node* nxt_node = _open_list[1];

    if(nxt_node->_grid_x == _dest_node->_grid_x &&
              nxt_node->_grid_y == _dest_node->_grid_y) {
      // Remove the optimal node from the top of the heap.
      remove_from_olist();

      // add the used node to the closed list.
      add_to_clist(nxt_node);

      // At this point the destination is reached.
      return;
    }
    else {
      identify_neighbors(nxt_node);

      // add the used node to the closed list.
      add_to_clist(nxt_node);
    }
  }
  std::cout << "DESTINATION NOT REACHABLE MATE!" << std::endl;
  _closed_list.clear();
}

/**
 * This function traverses through the 8 neigbors of the parent node and then
 * adds the neighbors to the _open_list based on A* criteria.
 */
void PathFinder::identify_neighbors(Node *parent_node) {
  // Remove the parent node from the open_list so that it is not considered
  // while adding new nodes to the open list heap.
  remove_from_olist();
  for(int i = 0; i < 8; ++i) {
    if(parent_node->_neighbours[i] != nullptr) {
      if(parent_node->_neighbours[i]->_status == parent_node->_neighbours[i]->neutral
        && parent_node->_neighbours[i]->_type == true) {
        // Link the neighbor to the parent node.
        parent_node->_neighbours[i]->_prv_node = parent_node;
        // Calculate and update the score for the node.
        calc_node_score(parent_node->_neighbours[i]);
        // Add the neighbor to the open list.
        add_to_olist(parent_node->_neighbours[i]);
      }
    }
  }
}

/**
 * This function calculates the score of each node.  Score = Cost +
 * Heuristics.
 */
void PathFinder::calc_node_score(Node *nd) {
  nd->_cost = calc_cost_frm_src(nd);
  nd->_heuristic = calc_heuristic(nd);
  nd->_score = nd->_cost + nd->_heuristic;
}

/**
 * This function calculates the cost of each node by finding out the number of
 * node traversals required to reach the source node.  Diagonal traversals
 * have cost = 14.  Horizontal and vertical traversals have cost = 10.
 */
int PathFinder::calc_cost_frm_src(Node *nd) {
  int cost = 0;
  Node *start_node = nd;
  while(start_node->_prv_node != _src_node) {
    if(is_diagonal_node(start_node)) {
      cost += 14;
    }
    else {
      cost += 10;
    }
    start_node = start_node->_prv_node;
  }
  // Add the cost of traversal to the source node also.
  if(is_diagonal_node(start_node)) {
    cost += 14;
  }
  else {
    cost += 10;
  }
  return cost;
}

/**
 * This function calculates the heuristic of the nodes using Manhattan method.
 * All it does is predict the number of node traversals required to reach the
 * target node.  No diagonal traversals are allowed in this technique.
 */
int PathFinder::calc_heuristic(Node *nd) {
  int row_diff = abs(_dest_node->_grid_x - nd->_grid_x);
  int col_diff = abs(_dest_node->_grid_y - nd->_grid_y);

  int heuristic = 10 * (row_diff + col_diff);
  return heuristic;
}

/**
 * This function checks if the traversal from a node is diagonal.
 */
bool PathFinder::is_diagonal_node(Node *nd) {
  // Calculate the row and column differences between child and parent nodes.
  float row_diff = nd->_grid_x - nd->_prv_node->_grid_x;
  float col_diff = nd->_grid_y - nd->_prv_node->_grid_y;

  // Check if the relationship between child and parent node is diagonal.
  if(row_diff == 0 || col_diff == 0) {
    return false;
  }
  else {
    return true;
  }
}

/**
 * This function adds a node to the open list heap.  A binay heap is
 * maintained to improve the search.
 */
void PathFinder::add_to_olist(Node *nd) {
  // Variables required to search the binary heap.
  Node *child_node, *parent_node;
  int child_idx, parent_idx;

  // Set the status as open.
  nd->_status = nd->open;
  // Add the node to the open list.
  _open_list.push_back(nd);

  // Find the parent and child nodes and create temporary nodes out of them.
  // In a binary heap the children of a parent node are always i*2 and i*2 +
  // 1, where i is the index of the parent node in the heap.  And hence, the
  // parent of a node can be easily found out by dividing by 2 and rounding
  // it.
  child_idx = _open_list.size() - 1;
  parent_idx = child_idx / 2;
  child_node = _open_list[child_idx];
  parent_node = _open_list[parent_idx];

  // Keep traversing the heap until the lowest element in the list is bubbled
  // to the top of the heap.
  while(_open_list[child_idx]->_score <= _open_list[parent_idx]->_score) {
    // Swap the parent node and the child node.
    _open_list[parent_idx] = child_node;
    _open_list[child_idx] = parent_node;

    // Update the new child and parent indices.
    child_idx = parent_idx;
    parent_idx = child_idx / 2;

    // Update the new child and parent nodes.
    child_node = _open_list[child_idx];
    parent_node = _open_list[parent_idx];
  }

  // At this point the Node with the smallest score will be at the top of the
  // heap.
}

/**
 * This function removes a node from the open list.  During the removal the
 * binary heap is maintained.
 */
void PathFinder::remove_from_olist() {
  // Variables for maintaining the binary heap.
  Node *child_node, *child_node_1, *child_node_2;
  int child_idx, child_idx_1, child_idx_2;

  // Remove the Node at index 1 from the open list binary heap.  Note: Node at
  // index 0 of open list is a dummy node.
  _open_list.erase(_open_list.begin() + 1);

  if(_open_list.size() > 1) {
    // Store the last element in the open list to a temp_node.
    Node *temp_node = _open_list[_open_list.size() - 1];

    // Shift the elements of the open list to the right by 1 element
    // circularly, excluding element at 0 index.
    for(int i = _open_list.size() - 1; i > 1; --i) {
      _open_list[i] = _open_list[i - 1];
    }

    // Assign the temp_node to 1st element in the open list.
    _open_list[1] = temp_node;

    // Set the iterator for traversing the node from index 1 in the heap.
    unsigned int k = 1;

    // This loop traverses down the open list till the node reaches the
    // correct position in the binary heap.
    while(true) {
      if((k * 2 + 1) < _open_list.size()) {
        // Two children exists for the parent node.
        child_idx_1 = k * 2;
        child_idx_2 = k * 2 + 1;
        child_node_1 = _open_list[child_idx_1];
        child_node_2 = _open_list[child_idx_2];

        if(_open_list[child_idx_1]->_score < _open_list[child_idx_2]->_score) {
          if(_open_list[k]->_score > _open_list[child_idx_1]->_score) {
            // Swap the parent node and the child node.
            _open_list[child_idx_1] = _open_list[k];
            _open_list[k] = child_node_1;

            // Update the parent node index.
            k = child_idx_1;
          }
          else {
            break;
          }
        }
        else {
          if(_open_list[k]->_score > _open_list[child_idx_2]->_score) {
            // Swap the parent node and the child node.
            _open_list[child_idx_2] = _open_list[k];
            _open_list[k] = child_node_2;

            // Update the parent node index.
            k = child_idx_2;
          }
          else {
            break;
          }
        }
      }
      else if((k * 2) < _open_list.size()) {
        // Only one child exists for the parent node.
        child_idx = k * 2;
        child_node = _open_list[child_idx];

        if(_open_list[k]->_score > _open_list[child_idx]->_score) {
          // Swap the parent node and the child node.
          _open_list[child_idx] = _open_list[k];
          _open_list[k] = child_node;

          // Update the parent node index.
          k = child_idx;
        }
        else {
          break;
        }
      }
      else {
        // No children exists.
        break;
      }
    }
  }

  // At this point the Node was succesfully removed and the binary heap re-
  // arranged.
}

/**
 * This function adds a node to the closed list.
 */
void PathFinder::add_to_clist(Node *nd) {
  // Set the status as closed.
  nd->_status = nd->close;
  // Add the node to the close list.
  _closed_list.push_back(nd);
}

/**
 * This function removes a node from the closed list.
 */
void PathFinder::remove_from_clist(int r, int c) {
  for(unsigned int i = 0; i < _closed_list.size(); ++i) {
    if(_closed_list[i]->_grid_x == r && _closed_list[i]->_grid_y == c) {
      _closed_list.erase(_closed_list.begin() + i);
      break;
    }
  }
}

/**
 * This function allows the user to pass a position and it returns the
 * corresponding node on the navigation mesh.  A very useful function as it
 * allows for dynamic updation of the mesh based on position.
 */
Node* find_in_mesh(NavMesh nav_mesh, LVecBase3 pos, int grid_size) {
  int size = grid_size;
  float x = pos[0];
  float y = pos[1];

  for(int i = 0; i < size; ++i) {
    for(int j = 0; j < size; ++j) {
      if(nav_mesh[i][j] != nullptr && nav_mesh[i][j]->contains(x, y)) {
        return(nav_mesh[i][j]);
      }
    }
  }
  return nullptr;
}