| 76 | } |
| 77 | |
| 78 | void PathFinder::initAStar() { |
| 79 | auto heuristicCostFn = [this](Node const& fromNode, Node const& toNode) -> float { |
| 80 | return heuristicCost(fromNode.position, toNode.position); |
| 81 | }; |
| 82 | auto goalReachedFn = [this](Node const& node) -> bool { |
| 83 | if (m_searchParams.mustEndOnGround && (!onGround(node.position) || node.velocity.isValid())) |
| 84 | return false; |
| 85 | return distance(node.position, m_searchTo) < NodeGranularity; |
| 86 | }; |
| 87 | auto neighborsFn = [this](Node const& node, List<Edge>& result) { |
| 88 | auto neighborFilter = [this](Edge const& edge) -> bool { |
| 89 | return distance(edge.source.position, m_searchFrom) <= m_searchParams.maxDistance.value(DefaultMaxDistance); |
| 90 | }; |
| 91 | neighbors(node, result); |
| 92 | result.filter(neighborFilter); |
| 93 | }; |
| 94 | auto validateEndFn = [this](Edge const& edge) -> bool { |
| 95 | if (!m_searchParams.mustEndOnGround) |
| 96 | return true; |
| 97 | return onGround(edge.target.position) && edge.action != Action::Jump; |
| 98 | }; |
| 99 | |
| 100 | Vec2F roundedFrom = roundToNode(m_searchFrom); |
| 101 | Vec2F roundedTo = roundToNode(m_searchTo); |
| 102 | |
| 103 | m_astar = AStar::Search<Edge, Node>(heuristicCostFn, |
| 104 | neighborsFn, |
| 105 | goalReachedFn, |
| 106 | m_searchParams.returnBest, |
| 107 | {validateEndFn}, |
| 108 | m_searchParams.maxFScore, |
| 109 | m_searchParams.maxNodesToSearch); |
| 110 | m_astar->start(Node{roundedFrom, {}}, Node{roundedTo, {}}); |
| 111 | } |
| 112 | |
| 113 | float PathFinder::heuristicCost(Vec2F const& fromPosition, Vec2F const& toPosition) const { |
| 114 | // This function is used to estimate the cost of travel between two nodes. |