BVB Source Codes

CRYENGINE Show GenericAStarSolver.h Source code

Return Download CRYENGINE: download GenericAStarSolver.h Source code - Download CRYENGINE Source code - Type:.h
  1. // Copyright 2001-2016 Crytek GmbH / Crytek Group. All rights reserved.
  2.  
  3. #ifndef GENERICASTARSOLVER_H
  4. #define GENERICASTARSOLVER_H
  5.  
  6. //GenericAStarSolver.h
  7. //By Jeremy Gross
  8.  
  9. #include <vector>
  10. #include <list>
  11. #include <map>
  12. #include <set>
  13. #include <algorithm>
  14. #include <float.h>
  15.  
  16. /////////////////////////////////////////////////////////////////////////////
  17. //DISCLAIMER/////////////////////////////////////////////////////////////////
  18. //This is not really THAT generic of an A*.  Is it missing your needed feature?
  19. //      =>Likely!!
  20. //Please feel encouraged as reader/user of this file to make it more generic.
  21. //
  22. //Only Requests:
  23. //KEEP IT CLEAN!
  24. //      (And make sure it stays backwards compatible for whoever is using)
  25. //
  26. //DISCLAIMER/////////////////////////////////////////////////////////////////
  27. /////////////////////////////////////////////////////////////////////////////
  28.  
  29. /////////////////////////////////////////////////////////////////////////////
  30. //Static polymorphic interface: GraphTraits//////////////////////////////////
  31. template<
  32.   typename YourGraph
  33.   , typename YourReferencePoint
  34.   , typename YourIndexType//    <=HIGHLY recommend this be int
  35.   >
  36. class GraphTraits
  37. {
  38. public:
  39.         typedef YourReferencePoint ReferencePointType;
  40.         typedef YourIndexType      IndexType;
  41.  
  42.         IndexType          GetNumNodes() const;
  43.  
  44.         IndexType          GetBestNodeIndex(ReferencePointType& referencePoint) const;
  45.         ReferencePointType GetNodeReferencePoint(IndexType nodeIndex) const;
  46.  
  47.         int                GetNumLinks(IndexType nodeIndex) const;
  48.         IndexType          GetNextNodeIndex(IndexType graphNodeIndex, int linkIndex) const;
  49. };
  50. //Static polymorphic interfaces: GraphTraits/////////////////////////////////
  51. /////////////////////////////////////////////////////////////////////////////
  52.  
  53. /////////////////////////////////////////////////////////////////////////////
  54. //Static polymorphic interface: HeuristicTraits//////////////////////////////
  55. template<typename GraphTraits, typename IndexType = typename GraphTraits::IndexType>
  56. class HeuristicTraits
  57. {
  58. public:
  59.         float H(const GraphTraits* graph, IndexType currentNodeIndex, IndexType endNodeIndex) const;
  60. };
  61. //Static polymorphic interface: HeuristicTraits//////////////////////////////
  62. /////////////////////////////////////////////////////////////////////////////
  63.  
  64. /////////////////////////////////////////////////////////////////////////////
  65. //Static polymorphic interface: LinkTraversalCostCalculatorTraits////////////
  66. template<typename GraphTraits, typename IndexType = typename GraphTraits::IndexType>
  67. class LinkTraversalCostCalculatorTraits
  68. {
  69. public:
  70.         float GetCost(const GraphTraits* graph, IndexType currentNodeIndex, IndexType nextNodeIndex) const;
  71. };
  72. //Static polymorphic interface: LinkTraversalCostCalculatorTraits////////////
  73. /////////////////////////////////////////////////////////////////////////////
  74.  
  75. /////////////////////////////////////////////////////////////////////////////
  76. //AStar OpenList Node////////////////////////////////////////////////////////
  77. template<typename IndexType = int>
  78. struct AStarNode
  79. {
  80.         float                       G, F;
  81.         const IndexType             graphNodeIndex;
  82.         const AStarNode<IndexType>* parentNode;
  83.  
  84.         AStarNode(IndexType theGraphNodeIndex)
  85.                 : G(FLT_MAX)
  86.                 , F(FLT_MAX)
  87.                 , graphNodeIndex(theGraphNodeIndex)
  88.                 , parentNode(NULL)
  89.         {}
  90.  
  91.         bool operator<(const AStarNode& other) const
  92.         {
  93.                 return F < other.F;
  94.         }
  95.  
  96.         bool operator>=(const AStarNode& other) const
  97.         {
  98.                 return F >= other.F;
  99.         }
  100. };
  101. //AStar OpenList Node////////////////////////////////////////////////////////
  102. /////////////////////////////////////////////////////////////////////////////
  103.  
  104. /////////////////////////////////////////////////////////////////////////////
  105. //Optional OpenList Imp: List////////////////////////////////////////////////
  106. template<typename IndexType = int>
  107. class DefaultOpenList
  108. {
  109.         std::list<const AStarNode<IndexType>*> openList;
  110.  
  111. public:
  112.  
  113.         void Add(const AStarNode<IndexType>* node)
  114.         {
  115.                 typename std::list<const AStarNode<IndexType>*>::iterator iter = openList.begin();
  116.                 while (iter != openList.end())
  117.                 {
  118.                         if (**iter >= *node)
  119.                         {
  120.                                 break;
  121.                         }
  122.                         ++iter;
  123.                 }
  124.                 openList.insert(iter, node);
  125.         }
  126.  
  127.         const AStarNode<IndexType>* GetNext()
  128.         {
  129.                 const AStarNode<IndexType>* node = openList.front();
  130.                 openList.pop_front();
  131.                 return node;
  132.         }
  133.  
  134.         void Clear()
  135.         {
  136.                 openList.clear();
  137.         }
  138.  
  139.         bool IsEmpty() const
  140.         {
  141.                 return openList.empty() != 0;
  142.         }
  143.  
  144.         //For users debug benefit they may template specialize this function:
  145.         void Print() const;
  146. };
  147. //Optional OpenList Imp: List////////////////////////////////////////////////
  148. /////////////////////////////////////////////////////////////////////////////
  149.  
  150. /////////////////////////////////////////////////////////////////////////////
  151. //DefaultClosedList//////////////////////////////////////////////////////////
  152. #define BITS_PER_BYTE 8
  153. #define ROUND_TO_MIN_BYTES(theBits)        (((theBits) + (BITS_PER_BYTE - 1)) / BITS_PER_BYTE)
  154. #define BYTES_PER_INT sizeof(int)
  155. #define ROUND_TO_MIN_INTS(theBits)         ((ROUND_TO_MIN_BYTES(theBits) + (BYTES_PER_INT - 1)) / BYTES_PER_INT)
  156. #define TO_BIT_ARRAY_VALUE_SHIFT(theValue) ((theValue) & (BYTES_PER_INT * BITS_PER_BYTE - 1))
  157. #define TO_BIT_ARRAY_INDEX(theValue)       ((theValue) >> (BYTES_PER_INT * BITS_PER_BYTE - 1))
  158.  
  159. template<typename IndexType>
  160. class DefaultClosedList
  161. {
  162.         std::set<IndexType> closedList;
  163. public:
  164.  
  165.         void Resize(IndexType size)
  166.         {
  167.                 closedList.clear();
  168.         }
  169.  
  170.         void Close(IndexType index)
  171.         {
  172.                 closedList.insert(index);
  173.         }
  174.  
  175.         bool IsClosed(IndexType index) const
  176.         {
  177.                 return closedList.find(index) != closedList.end();
  178.         }
  179.  
  180.         //For users debug benefit they may template specialize this function:
  181.         void Print() const;
  182. };
  183.  
  184. template<>
  185. class DefaultClosedList<int>
  186. {
  187.         std::vector<int> BitArray;
  188.         int              ArraySize;
  189. public:
  190.  
  191.         void Resize(int size)
  192.         {
  193.                 BitArray.clear();
  194.                 BitArray.resize(ROUND_TO_MIN_INTS(size), 0);
  195.                 ArraySize = size;
  196.         }
  197.  
  198.         void Close(int index)
  199.         {
  200.                 BitArray[TO_BIT_ARRAY_INDEX(index)] |= (1 << TO_BIT_ARRAY_VALUE_SHIFT(index));
  201.         }
  202.  
  203.         bool IsClosed(int index) const
  204.         {
  205.                 return (BitArray[TO_BIT_ARRAY_INDEX(index)] & (1 << TO_BIT_ARRAY_VALUE_SHIFT(index))) != 0;
  206.         }
  207.  
  208.         void Print() const
  209.         {
  210.                 printf("Closed List:\n");
  211.                 for (int i = 0; i < ArraySize; ++i)
  212.                 {
  213.                         if (IsClosed(i))
  214.                         {
  215.                                 printf("1 ");
  216.                         }
  217.                         else
  218.                         {
  219.                                 printf("0 ");
  220.                         }
  221.                 }
  222.                 printf("\n");
  223.         }
  224. };
  225. //DefaultClosedList//////////////////////////////////////////////////////////
  226. /////////////////////////////////////////////////////////////////////////////
  227.  
  228. /////////////////////////////////////////////////////////////////////////////
  229. //DefaultNodeContainer///////////////////////////////////////////////////////
  230. template<typename IndexType = int>
  231. class DefaultNodeContainer
  232. {
  233.         struct AStarNodeCompare
  234.         {
  235.                 bool operator()(const AStarNode<IndexType>& LHS, const AStarNode<IndexType>& RHS) const
  236.                 {
  237.                         return LHS.graphNodeIndex < RHS.graphNodeIndex;
  238.                 }
  239.         };
  240.  
  241.         std::set<AStarNode<IndexType>, AStarNodeCompare> aStarNodes;
  242.  
  243. public:
  244.  
  245.         AStarNode<IndexType>* GetAStarNode(IndexType nodeIndex)
  246.         {
  247.                 std::pair<typename std::set<AStarNode<IndexType>, AStarNodeCompare>::iterator, bool> result =
  248.                   aStarNodes.insert(AStarNode<IndexType>(nodeIndex));
  249.  
  250.                 return const_cast<AStarNode<IndexType>*>(&*result.first);
  251.         }
  252.  
  253.         void Clear()
  254.         {
  255.                 aStarNodes.clear();
  256.         }
  257. };
  258. //DefaultNodeContainer///////////////////////////////////////////////////////
  259. /////////////////////////////////////////////////////////////////////////////
  260.  
  261. /////////////////////////////////////////////////////////////////////////////
  262. //GenericAStarSolver/////////////////////////////////////////////////////////
  263. template<
  264.   typename GraphTraits
  265.   , typename Heuristic = HeuristicTraits<GraphTraits>
  266.   , typename LinkTraversalCostCalculator = LinkTraversalCostCalculatorTraits<GraphTraits>
  267.  
  268.   , typename OpenList = DefaultOpenList<typename GraphTraits::IndexType>
  269.   , typename ClosedList = DefaultClosedList<typename GraphTraits::IndexType>
  270.   , typename NodeContainer = DefaultNodeContainer<typename GraphTraits::IndexType>
  271.  
  272.   , typename ReferencePointType = typename GraphTraits::ReferencePointType
  273.   , typename IndexType = typename GraphTraits::IndexType
  274.   >
  275. class GenericAStarSolver
  276. {
  277.         const GraphTraits*                 graph;
  278.         const Heuristic*                   heuristic;
  279.         const LinkTraversalCostCalculator* costCalculator;
  280.  
  281.         OpenList                           openList;
  282.         ClosedList                         closedList;
  283.         NodeContainer                      nodeContainer;
  284.  
  285.         std::vector<IndexType>             finalPathNodeIndices;
  286.         IndexType                          goalNodeIndex;
  287.         ReferencePointType                 PathEndPoint;
  288.  
  289. public:
  290.  
  291.         void StartPathFind(
  292.           const GraphTraits* theGraph
  293.           , const Heuristic* theHeuristic
  294.           , const LinkTraversalCostCalculator* theCostCalculator
  295.           , ReferencePointType theStart
  296.           , ReferencePointType theEnd)
  297.         {
  298.                 graph = theGraph;
  299.                 heuristic = theHeuristic;
  300.                 costCalculator = theCostCalculator;
  301.  
  302.                 closedList.Resize(graph->GetNumNodes());
  303.                 nodeContainer.Clear();
  304.                 openList.Clear();
  305.                 finalPathNodeIndices.clear();
  306.  
  307.                 PathEndPoint = theEnd;
  308.  
  309.                 IndexType theStartNodeIndex = theGraph->GetBestNodeIndex(theStart);
  310.                 goalNodeIndex = theGraph->GetBestNodeIndex(theEnd);
  311.  
  312.                 const float G = 0;
  313.                 float F = G + heuristic->H(graph, theStartNodeIndex, goalNodeIndex);
  314.  
  315.                 AStarNode<IndexType>* startNodePtr = nodeContainer.GetAStarNode(theStartNodeIndex);
  316.                 startNodePtr->G = G;
  317.                 startNodePtr->F = F;
  318.                 startNodePtr->parentNode = NULL;
  319.  
  320.                 openList.Add(startNodePtr);
  321.         }
  322.  
  323.         int Update(int theMaxNumNodesToProcess)
  324.         {
  325.                 for (; 0 < theMaxNumNodesToProcess; --theMaxNumNodesToProcess)
  326.                 {
  327.                         if (openList.IsEmpty())
  328.                         {
  329.                                 break;
  330.                         }
  331.  
  332.                         const AStarNode<IndexType>* currentAStarNode = openList.GetNext();
  333.                         if (closedList.IsClosed(currentAStarNode->graphNodeIndex))
  334.                                 continue;
  335.  
  336.                         closedList.Close(currentAStarNode->graphNodeIndex);
  337.  
  338.                         if (currentAStarNode->graphNodeIndex != goalNodeIndex)
  339.                         {
  340.                                 int numLinks = graph->GetNumLinks(currentAStarNode->graphNodeIndex);
  341.                                 for (int linkIndex = 0; linkIndex < numLinks; ++linkIndex)
  342.                                 {
  343.                                         IndexType nextNodeIndex = graph->GetNextNodeIndex(currentAStarNode->graphNodeIndex, linkIndex);
  344.                                         if (closedList.IsClosed(nextNodeIndex))
  345.                                         {
  346.                                                 //this next node is already visited, ignore it.
  347.                                                 continue;
  348.                                         }
  349.  
  350.                                         float H = heuristic->H(graph, nextNodeIndex, goalNodeIndex);
  351.                                         float G = currentAStarNode->G + costCalculator->GetCost(graph, currentAStarNode->graphNodeIndex, nextNodeIndex);
  352.                                         float F = G + H;
  353.  
  354.                                         AStarNode<IndexType>* nextNodePtr = nodeContainer.GetAStarNode(nextNodeIndex);
  355.  
  356.                                         //only insert this if its cheaper than what was there already (or its not there)
  357.                                         if (F < nextNodePtr->F)
  358.                                         {
  359.                                                 nextNodePtr->F = F;
  360.                                                 nextNodePtr->G = G;
  361.                                                 nextNodePtr->parentNode = currentAStarNode;
  362.                                                 openList.Add(nextNodePtr);
  363.                                         }
  364.                                 }
  365.                         }
  366.                         else
  367.                         {
  368.                                 //Goal found, save the path and quit:
  369.                                 while (currentAStarNode != NULL)
  370.                                 {
  371.                                         finalPathNodeIndices.push_back(currentAStarNode->graphNodeIndex);
  372.                                         currentAStarNode = currentAStarNode->parentNode;
  373.                                 }
  374.                                 break;
  375.                         }
  376.                 }
  377.                 return theMaxNumNodesToProcess;
  378.         }
  379.  
  380.         void GetPath(std::vector<ReferencePointType>& pathReferencePoints)
  381.         {
  382.                 pathReferencePoints.reserve(finalPathNodeIndices.size());
  383.                 for (typename std::vector<IndexType>::reverse_iterator iter = finalPathNodeIndices.rbegin()
  384.                      ; iter != finalPathNodeIndices.rend()
  385.                      ; ++iter)
  386.                 {
  387.                         IndexType index = *iter;
  388.                         pathReferencePoints.push_back(graph->GetNodeReferencePoint(index));
  389.                 }
  390.                 pathReferencePoints.push_back(PathEndPoint);
  391.         }
  392.  
  393.         void GetPathReversed(std::vector<ReferencePointType>& pathReferencePoints) const
  394.         {
  395.                 pathReferencePoints.reserve(finalPathNodeIndices.size());
  396.                 for (typename std::vector<IndexType>::const_iterator iter = finalPathNodeIndices.begin()
  397.                      ; iter != finalPathNodeIndices.end()
  398.                      ; ++iter)
  399.                 {
  400.                         IndexType index = *iter;
  401.                         pathReferencePoints.push_back(graph->GetNodeReferencePoint(index));
  402.                 }
  403.                 pathReferencePoints.push_back(PathEndPoint);
  404.         }
  405.  
  406.         void GetPath(std::vector<IndexType>& pathNodeIndices)
  407.         {
  408.                 pathNodeIndices.reserve(finalPathNodeIndices.size());
  409.                 for (typename std::vector<IndexType>::reverse_iterator iter = finalPathNodeIndices.rbegin()
  410.                      ; iter != finalPathNodeIndices.rend()
  411.                      ; ++iter)
  412.                 {
  413.                         pathNodeIndices.push_back(*iter);
  414.                 }
  415.         }
  416.  
  417.         void GetPathReversed(std::vector<IndexType>& pathNodeIndices) const
  418.         {
  419.                 pathNodeIndices.reserve(finalPathNodeIndices.size());
  420.                 for (typename std::vector<IndexType>::const_iterator iter = finalPathNodeIndices.begin()
  421.                      ; iter != finalPathNodeIndices.end()
  422.                      ; ++iter)
  423.                 {
  424.                         pathNodeIndices.push_back(*iter);
  425.                 }
  426.         }
  427.  
  428.         //For users debug benefit
  429.         const OpenList& GetOpenList()
  430.         {
  431.                 return openList;
  432.         }
  433.  
  434.         //For users debug benefit
  435.         const ClosedList& GetClosedList()
  436.         {
  437.                 return closedList;
  438.         }
  439.  
  440.         void Reset()
  441.         {
  442.                 stl::free_container(openList);
  443.                 stl::free_container(closedList);
  444.                 stl::free_container(nodeContainer);
  445.                 stl::free_container(finalPathNodeIndices);
  446.         }
  447. };
  448. //GenericAStarSolver/////////////////////////////////////////////////////////
  449. /////////////////////////////////////////////////////////////////////////////
  450.  
  451. #endif //GENERICASTARSOLVER_H
  452.  
downloadGenericAStarSolver.h Source code - Download CRYENGINE Source code
Related Source Codes/Software:
postal - 2017-06-11
reactide - Reactide is the first dedicated IDE for React web ... 2017-06-11
rkt - rkt is a pod-native container engine for Linux. It... 2017-06-11
uWebSockets - Tiny WebSockets https://for... 2017-06-11
realworld - TodoMVC for the RealWorld - Exemplary fullstack Me... 2017-06-11
CRYENGINE - CRYENGINE is a powerful real-time game development... 2017-06-11
goreplay - GoReplay is an open-source tool for capturing and ... 2017-06-10
pyenv - Simple Python version management 2017-06-10
redux-saga - An alternative side effect model for Redux apps ... 2017-06-10
angular-starter - 2017-06-10

 Back to top