Code/Engine/Utilities/PathFinding/Implementation/GraphSearch_inl.h - dev - redgpu/ezEngine

Code/Engine/Utilities/PathFinding/Implementation/GraphSearch_inl.h
219 строк7 KB
Jan Krassnigg
Minor AI code improvements
24 июл 2026, 17:25
24 июл 2026, 17:25ca18f47
#pragma once

template <typename PathStateType>
void ezPathSearch<PathStateType>::ClearPathStates()
{
  m_PathStates.Clear();
  m_StateQueue.Clear();
}

template <typename PathStateType>
ezInt64 ezPathSearch<PathStateType>::FindBestNodeToExpand(PathStateType*& out_pPathState)
{
  float fLowestEstimation = ezMath::Infinity<float>();
  out_pPathState = nullptr;
  ezUInt32 iBestInQueue = 0;

  for (ezUInt32 i = 0; i < m_StateQueue.GetCount(); ++i)
  {
    const ezInt64 iNodeIndex = m_StateQueue[i];

    PathStateType* pState = &m_PathStates[iNodeIndex];

    if (pState->m_fEstimatedCostToTarget < fLowestEstimation)
    {
      fLowestEstimation = pState->m_fEstimatedCostToTarget;
      out_pPathState = pState;
      iBestInQueue = i;
    }
  }

  EZ_ASSERT_DEV(out_pPathState != nullptr, "Implementation Error");

  const ezInt64 iBestNodeIndex = m_StateQueue[iBestInQueue];
  m_StateQueue.RemoveAtAndSwap(iBestInQueue);

  return iBestNodeIndex;
}

template <typename PathStateType>
void ezPathSearch<PathStateType>::FillOutPathResult(ezInt64 iEndNodeIndex, ezDeque<PathResultData>& out_Path)
{
  out_Path.Clear();

  while (true)
  {
    const PathStateType* pCurState = &m_PathStates[iEndNodeIndex];

    PathResultData r;
    r.m_iNodeIndex = iEndNodeIndex;
    r.m_pPathState = pCurState;

    out_Path.PushFront(r);

    if (iEndNodeIndex == pCurState->m_iReachedThroughNode)
      return;

    iEndNodeIndex = pCurState->m_iReachedThroughNode;
  }
}

template <typename PathStateType>
void ezPathSearch<PathStateType>::AddPathNode(ezInt64 iNodeIndex, const PathStateType& NewState)
{
  EZ_ASSERT_DEV(NewState.m_fCostToNode > m_CurState.m_fCostToNode,
    "The costs must grow from one node to the next.\nStart Node Costs: {0}\nAdjacent Node Costs: {1}", ezArgF(m_CurState.m_fCostToNode, 2),
    ezArgF(NewState.m_fCostToNode, 2));
  // EZ_ASSERT_DEV(NewState.m_fEstimatedCostToTarget >=  m_CurState.m_fEstimatedCostToTarget, "The estimated path costs cannot go down, the
  // heuristic must be 'optimistic' regarding to the real costs.\nEstimated Costs from Current: {0}\nEstimated Costs from Adjacent: {1}",
  // ezArgF(m_pCurPathState->m_fEstimatedCostToTarget, 2), ezArgF(NewState.m_fEstimatedCostToTarget, 2));
  EZ_ASSERT_DEV(NewState.m_fEstimatedCostToTarget >= NewState.m_fCostToNode, "Unrealistic expectations will get you nowhere.");

  PathStateType* pExistingState;

  if (m_PathStates.TryGetValue(iNodeIndex, pExistingState))
  {
    // state already exists in the hash table, and has a lower cost -> ignore the new state
    if (pExistingState->m_fCostToNode <= NewState.m_fCostToNode)
      return;

    // incoming state is better than the existing state -> update existing state
    *pExistingState = NewState;
    pExistingState->m_iReachedThroughNode = m_iCurNodeIndex;
    return;
  }

  // the state has not been reached before -> insert it
  pExistingState = &m_PathStates[iNodeIndex];

  *pExistingState = NewState;
  pExistingState->m_iReachedThroughNode = m_iCurNodeIndex;

  // put it into the queue of states that still need to be expanded
  m_StateQueue.PushBack(iNodeIndex);
}

template <typename PathStateType>
ezResult ezPathSearch<PathStateType>::FindPath(ezInt64 iStartNodeIndex, const PathStateType& StartState, ezInt64 iTargetNodeIndex,
  ezDeque<PathResultData>& out_Path, float fMaxPathCost /* = Infinity */)
{
  EZ_ASSERT_DEV(m_pStateGenerator != nullptr, "No Path State Generator is set.");

  ClearPathStates();

  if (iStartNodeIndex == iTargetNodeIndex)
  {
    m_PathStates.Reserve(1);

    m_PathStates[iTargetNodeIndex] = StartState;

    PathResultData r;
    r.m_iNodeIndex = iTargetNodeIndex;
    r.m_pPathState = &m_PathStates[iTargetNodeIndex];

    out_Path.Clear();
    out_Path.PushBack(r);

    return EZ_SUCCESS;
  }

  m_PathStates.Reserve(10000);

  PathStateType& FirstState = m_PathStates[iStartNodeIndex];

  m_pStateGenerator->StartSearch(iStartNodeIndex, &FirstState, iTargetNodeIndex);

  // make sure the first state references itself, as that is a termination criterion
  FirstState = StartState;
  FirstState.m_iReachedThroughNode = iStartNodeIndex;

  // put the start state into the to-be-expanded queue
  m_StateQueue.PushBack(iStartNodeIndex);

  // while the queue is not empty, expand the next node and see where that gets us
  while (!m_StateQueue.IsEmpty())
  {
    PathStateType* pCurState;
    m_iCurNodeIndex = FindBestNodeToExpand(pCurState);

    // we have reached the target node, generate the final path result
    if (m_iCurNodeIndex == iTargetNodeIndex)
    {
      FillOutPathResult(m_iCurNodeIndex, out_Path);
      m_pStateGenerator->SearchFinished(EZ_SUCCESS);
      return EZ_SUCCESS;
    }

    // The heuristic may overestimate how much it takes to reach the destination
    // thus even though the heuristic tells us we may not be able to make it, we cannot rely on that, but need to look at
    // the actual costs
    if (pCurState->m_fCostToNode >= fMaxPathCost)
    {
      m_pStateGenerator->SearchFinished(EZ_FAILURE);
      return EZ_FAILURE;
    }

    m_CurState = *pCurState;

    // let the generate append all the nodes that we can reach from here
    m_pStateGenerator->GenerateAdjacentStates(m_iCurNodeIndex, m_CurState, this);
  }

  m_pStateGenerator->SearchFinished(EZ_FAILURE);
  return EZ_FAILURE;
}


template <typename PathStateType>
ezResult ezPathSearch<PathStateType>::FindClosest(
  ezInt64 iStartNodeIndex, const PathStateType& StartState, IsSearchedObjectCallback Callback, ezDeque<PathResultData>& out_Path, float fMaxPathCost)
{
  EZ_ASSERT_DEV(m_pStateGenerator != nullptr, "No Path State Generator is set.");

  ClearPathStates();

  m_PathStates.Reserve(10000);

  PathStateType& FirstState = m_PathStates[iStartNodeIndex];

  m_pStateGenerator->StartSearchForClosest(iStartNodeIndex, &FirstState);

  // make sure the first state references itself, as that is a termination criterion
  FirstState = StartState;
  FirstState.m_iReachedThroughNode = iStartNodeIndex;

  // put the start state into the to-be-expanded queue
  m_StateQueue.PushBack(iStartNodeIndex);

  // while the queue is not empty, expand the next node and see where that gets us
  while (!m_StateQueue.IsEmpty())
  {
    PathStateType* pCurState;
    m_iCurNodeIndex = FindBestNodeToExpand(pCurState);

    // we have reached the target node, generate the final path result
    if (Callback(m_iCurNodeIndex, *pCurState, m_pStateGenerator))
    {
      FillOutPathResult(m_iCurNodeIndex, out_Path);
      m_pStateGenerator->SearchFinished(EZ_SUCCESS);
      return EZ_SUCCESS;
    }

    // The heuristic may overestimate how much it takes to reach the destination
    // thus even though the heuristic tells us we may not be able to make it, we cannot rely on that, but need to look at
    // the actual costs
    if (pCurState->m_fCostToNode >= fMaxPathCost)
    {
      m_pStateGenerator->SearchFinished(EZ_FAILURE);
      return EZ_FAILURE;
    }

    m_CurState = *pCurState;

    // let the generate append all the nodes that we can reach from here
    m_pStateGenerator->GenerateAdjacentStates(m_iCurNodeIndex, m_CurState, this);
  }

  m_pStateGenerator->SearchFinished(EZ_FAILURE);
  return EZ_FAILURE;
}