NEUROMANTIC

自分でC/C++/UE4/Graphics/ゲームなどをやったことをメモするブログ

グラフ・最短距離アルゴリズムのまとめメモ(3)

序文

「グラフ・最短距離アルゴリズムのまとめメモ(2)」に引き続きJPS、JPS+、GoalBoundingアルゴリズム(短距離アルゴリズム)を書きたいと思います。 日本語下手ですので用語について指摘して頂けましたらありがとうございます。


本文

JPS(Jump Point Search)

http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf
Jump point search - Wikipedia
zerowidth.com

A. 論文分析(用語説明)

  • {\vec{d}}:あるノード {n_i}から4方向または8方向への隣接ノードに移動する単位ベクトルを示す。
    • もし{\vec{d}}が斜め線である場合には、{\vec{d} = \vec{d_1} + \vec{d_2}}にもなれる。
  • {y = x + k\vec{d}}:グラフのノード {x}から {y}まで直線の {\vec{d}}方向に {k}歩移動出来る。
  • {\pi = \left\langle n_0, n_1, ..., n_k \right\rangle}:ノード {n_0}から {n_k}までの経路を表す。
  • {\pi \backslash x}:経路 {\pi}にノード {x}は存在しない。
  • {p(x)}:ノード {x}の親ノードを表す。
  • {neighbours(x)}:ノード {x}から隣接したノード達を表す。
  • この論文では各ノードの4方向移動時の重みを {1}、そして斜め移動を \sqrt{2}として規定している。
  • 論文では8方向移動を前提とする。

B. Jump Pointsとは

f:id:neuliliilli:20181124165533p:plain

(a)では親 {p(x)}から {x}へ探索を続けているが、{x}の周りの灰色のノードを通して探索できることを表している。単純なA*ならば普通にノード {x}に移動を終えて {x.f(x)}を更新しているはずだが、JPSではそうしない。

  1. 代わりに、右へとずっと移動し続けて {y}に到達するようにする。何故なら {z}に移動をするつもりだからである。そして {y.\pi = x}にする。({p(x)}ではない)
  2. {g(y)}を更新する。{g(y) = g(x) + \delta(x, y)}だ。
  3. この過程を反復してもし障害物にぶつかったらこの方向への探索は終わる。(障害物とはたんなる障害物または重みが重い辺で繋がらノードを指す。)

Neighbour Pruning Rule

  • Pruning Ruleとは「枝刈りアルゴリズム」で、グラフまたはグラフのように今の状態から次への状態に移動できるイベント(枝)が指数的に急増する場合演算の量を減らしながら適切な解答を見つけるようにするアルゴリズムまたは規則を表す。
  • Neighbour Pruning Rule(NPR)では、目標に到達する為にノード {x}の隣接ノードリスト {neighbours(x)}の子ノード {n}が計算されるかないかを判断する。
  • もし {x}がスターティングポイントならNPRは適用しない。

f:id:neuliliilli:20181124175508p:plain

NPRは下の手順で進行する。

  1. {(\pi =  \left\langle p(x), ..., x, n \right\rangle)}{(\pi' \ \backslash \ x \ | \ \pi =  \left\langle p(x), ..., n \right\rangle)}の重みの長さを比較する。
  2. {\delta(p(x), x)}(1)直線移動をするか(2)斜め移動をするかによってまた分かれる。
  3. 直線移動:次の数式を満たしている隣接したノード {n}を枝刈りする。
    {len({\left\langle} p(x), ..., n {\right\rangle} \ \backslash \ x) \le len(\left\langle p(x), x, n \right\rangle)}
    Fig. 2 (a)を見ると4番から {x}へ移動した時、次となる {n}は条件を満たしていない5番ノードになる。何故なら上の数式を適用した所、{2\sqrt{2} \le 2}になるからだ。
  4. 斜め移動:次の数式を満たしている隣接したノード {n}を枝刈りする。
    {len(\left\langle p(x), ..., n \right\rangle \ \backslash \ x) < len(\left\langle p(x), x, n \right\rangle)}
    Fig. 2 (c)を見ると8番から {x}へ移動した時、次となる {n}は条件を満たさない2,3,5番になる。
  5. もし {neighbours(x)}が障害物がなければ、灰色の隣接ノードは {x}Natural neighbourと呼ぶ。

  6. 定義1Fig. 2 (b) Fig. 2(d)の様に障害物があって、障害物が無い場合には既存の数式を満たすにも関わらずに {n}として選択されるノードをForced(強いられている)と言う。

    • {n}は以下の条件によって {neighbours(x)}によって強いられる。(Forced) {len(\left\langle p(x), x, n \right\rangle) < len(\left\langle p(x), ..., n \right\rangle \ \backslash \ x)}
      よってFig. 2(b)では障害物がなければ5番ノードだけ選択するはずのものを3番が強いられて選択するようにした。
  7. {len(\left\langle p(x), ..., n \right\rangle \ \backslash \ x)}を求めるのはさらなる部分的な最短経路問題になれる。

Arithmetic Description

  • 定義2{y = x + k\vec{d}}yxのジャンプポイントとしたら、そのyは3つの条件で少なくとも1つ以上を満足する。
    1. yはゴールだ。
    2. y{neighbours(y)}のノードで少なくとも1つ以上が障害物によってForcedしたノードを持つ。
    3. {\vec{d}}は斜め移動である時、(1)か(2)の条件を満足するさらなる {z = y + k_i\vec{d_i}}が存在する。

Fig 1. (b)定義2を満足する図を見せる。 1. p(x)から出発する。p(x)スターティングポイントは【定義2:3】を満足する。何故なら次の子ノードとなるxが【定義2:2】満足するからだ。 * 真上のノードはxよりヒューリスティックじゃない為、廃棄する。 2. xから真上のノードか、真横のノードか、それともyに方向を展開する。しかし真上と真横は {neighbours(x')}が無い為、排除する。そしてyが採択される。 3. 【定義2:1】を満足するためにzへと移動する。 4. 終わり。

このアルゴリズムは下のPseudo codeから確認できる。

// Identify successors
Required: x <= current node, s <= start position, g <= goal position
successors(x) = {};
n_neighbours(x) = prune(x, neighbours(x))
foreach n in n_neighbours(x) do
  n = jump(x, direction_unit(x, n), s, g)
  add n to successors(x)
return successors(x)

そしてJump関数は下のようになっている。

// Function `jump`
Required: x <= initial node, d: direction, s: start, g: goal
n := step(x, d)
if n is obstacle or oob then return null
if n == g then return n
if n' := neighbours(n) and n' is forced then return n
if d is diagonal then
  for all i := {1, 2} do
    if jump(n, d(i), s, g) is not null then return n
return jump(n, d, s, g)

実装

コード

長いです。

Relaxation

// @brief Relaxation function. 
// compare distance from start and push neighbor to minHeap.
void Relax(const DDirection& dir, SNode& cursor, SNode& successor, TMinHeap& heap, float distance) {
  if (successor.mDistantFromStart > cursor.mDistantFromStart + distance) {
    // Update distance from start and parent node in subgraph.
    successor.mDistantFromStart = cursor.mDistantFromStart + distance;
    successor.mParentPtr        = &cursor;

    // Set direction to parent node in predecessor subgraph.
    SetDirectionToParent(successor, dir);
    if (successor.status == ENodeStatus::Idle) { heap.push(&successor); }
    ++count;
  }
};

Prune (Neighbour Pruning Rule)

/// @brief  Find neighbour node list from cursor.
/// @param  cursor Point.
/// @return valid (natural and forced) neighbour node list from cursor.
[[nodiscard]] TNeighbourPairList Prune(SNode& cursor) 
{ 
  /// 0 1 /
  /// 3 x <-
  /// 6 7 /
  static auto DoPruneStraight = [](SNode& cursor, const int recCount) -> TNeighbourPairList {
    static constexpr std::array<std::pair<int, DDirection>, 5> checkList = {
        std::make_pair(0, gDirectionLU), std::make_pair(1, gDirection_U), 
        std::make_pair(3, gDirectionL_), std::make_pair(6, gDirectionLD), 
        std::make_pair(7, gDirection_D)
    };
    static constexpr std::array<std::pair<int, int>, 2> forcedCheckList = {
        std::make_pair(1, 0), std::make_pair(7, 6),
    };

    std::array<DDirection, 8> directionList = {};
    std::array<bool, 8>       obstacleFlag  = {};
    for (const auto& [id, dir] : checkList) {
      directionList[id] = GetRotatedDirectionCW(dir, recCount);
      if (cursor.GetEdge(directionList[id]).node == nullptr) { obstacleFlag[id] = true; }
    }

    TNeighbourPairList returnList = {};
    if (obstacleFlag[3] == false) { 
      returnList.emplace_back(directionList[3], cursor.GetEdge(directionList[3]).node);
    };

    for (const auto& [obs, flr] : forcedCheckList) {
      if (obstacleFlag[obs] == true && obstacleFlag[flr] == false) {
        returnList.emplace_back(directionList[flr], cursor.GetEdge(directionList[flr]).node);
      } 
    }

    return returnList;
  };
 
  /// 0 1 2
  /// 3 x 5
  /// 6 7 ↖
  static auto DoPruneDiagonal = [](SNode& cursor, const int recCount) -> TNeighbourPairList {
    static constexpr std::array<std::pair<int, DDirection>, 7> checkList = {
        std::make_pair(0, gDirectionLU), std::make_pair(1, gDirection_U), 
        std::make_pair(2, gDirectionRU), std::make_pair(3, gDirectionL_), 
        std::make_pair(5, gDirectionR_), std::make_pair(6, gDirectionLD),
        std::make_pair(7, gDirection_D)
    };
    static constexpr std::array<std::pair<int, int>, 2> forcedCheckList = {
        std::make_pair(5, 2), std::make_pair(7, 6),
    };

    std::array<DDirection, 8> directionList = {};
    std::array<bool, 8>       obstacleFlag  = {};
    for (const auto& [id, dir] : checkList) {
      directionList[id] = GetRotatedDirectionCW(dir, recCount);
      if (cursor.GetEdge(directionList[id]).node == nullptr) { obstacleFlag[id] = true; }
    }

    TNeighbourPairList returnList = {};
    static constexpr std::array<int, 3> generalCheckList = {0, 1, 3};
    for (const auto& id : generalCheckList) {
      if (obstacleFlag[id] == false) {
        returnList.emplace_back(directionList[id], cursor.GetEdge(directionList[id]).node);
      }
    }
    for (const auto& [obs, flr] : forcedCheckList) {
      if (obstacleFlag[obs] == true && obstacleFlag[flr] == false) {
        returnList.emplace_back(directionList[flr], cursor.GetEdge(directionList[flr]).node);
      } 
    }
    return returnList;
  };

  // Validity test
  assert(cursor.mParentPtr != nullptr);
  // Function body
  const DDirection& dirFromParent = GetReverseDirection(cursor.mParentDirection);
  if (IsDirectionDiagonal(dirFromParent) == true) 
  {
    const auto recursionCount = GetDirectionIndexFrom(gDirectionList4Diagonal, dirFromParent) * 2;
    assert(recursionCount != -2);
    return DoPruneDiagonal(cursor, recursionCount);
  }
  else
  {
    const auto recursionCount = GetDirectionIndexFrom(gDirectionList4, dirFromParent) * 2;
    assert(recursionCount != -2);
    return DoPruneStraight(cursor, recursionCount);
  }
};

Check Forced

/// @brief
/// @param  cursor
/// @param  direction
/// @return If there is any neighbour node which is forced, return true or false.
[[nodiscard]] bool IsForced(const SNode& cursor, const DDirection& direction) {
  /// 0 1 /
  /// 3 x <-
  /// 6 7 /
  static auto DoCheckStraight = [](const SNode& cursor, const int recCount) -> bool {
    static constexpr std::array<std::pair<int, DDirection>, 4> checkList = {
        std::make_pair(0, gDirectionLU), std::make_pair(1, gDirection_U), 
        std::make_pair(6, gDirectionLD), std::make_pair(7, gDirection_D)
    };
    static constexpr std::array<std::pair<int, int>, 2> forcedCheckList = {
        std::make_pair(1, 0), std::make_pair(7, 6),
    };

    std::array<DDirection, 8> directionList = {};
    std::array<bool, 8>       obstacleFlag  = {};
    for (const auto& [id, dir] : checkList) {
      directionList[id] = GetRotatedDirectionCW(dir, recCount);
      const auto& edge = cursor.GetEdge(directionList[id]);
      if (edge.node == nullptr) { obstacleFlag[id] = true; }
    }

    for (const auto& [obs, flr] : forcedCheckList) {
      if (obstacleFlag[obs] == true && obstacleFlag[flr] == false) { return true; } 
    }
    return false;
  };
 
  /// 0 1 2
  /// 3 x 5
  /// 6 7 ↖
  static auto DoCheckDiagonal = [](const SNode& cursor, const int recCount) -> bool {
    static constexpr std::array<std::pair<int, DDirection>, 4> checkList = {
        std::make_pair(2, gDirectionRU), std::make_pair(5, gDirectionR_), 
        std::make_pair(6, gDirectionLD), std::make_pair(7, gDirection_D)
    };
    static constexpr std::array<std::pair<int, int>, 2> forcedCheckList = {
        std::make_pair(5, 2), std::make_pair(7, 6),
    };

    std::array<DDirection, 8> directionList = {};
    std::array<bool, 8>       obstacleFlag  = {};
    for (const auto& [id, dir] : checkList) {
      directionList[id] = GetRotatedDirectionCW(dir, recCount);
      if (cursor.GetEdge(directionList[id]).node == nullptr) { obstacleFlag[id] = true; }
    }

    for (const auto& [obs, flr] : forcedCheckList) {
      if (obstacleFlag[obs] == true && obstacleFlag[flr] == false) { return true; } 
    }
    return false;
  };

  // Function body
  if (IsDirectionDiagonal(direction) == true) {
    const auto recursionCount = GetDirectionIndexFrom(gDirectionList4Diagonal, direction) * 2;
    assert(recursionCount >= 0);
    return DoCheckDiagonal(cursor, recursionCount);
  }
  else {
    const auto recursionCount = GetDirectionIndexFrom(gDirectionList4, direction) * 2;
    assert(recursionCount >= 0);
    return DoCheckStraight(cursor, recursionCount);
  }
}

Jump

[[nodiscard]] std::pair<SNode*, float> Jump(
    SNode& cursor, const DDirection& direction, 
    const SNode& goal, float distance = 0.0f) 
{
  SEdge& neighbourEdge = cursor.GetEdge(direction);
  const auto distanceToNeighbour = distance + neighbourEdge.mWeight;

  if (neighbourEdge.node == nullptr) { return {nullptr, 0}; }

  SNode& neighbourNode = *neighbourEdge.node;  

  if (neighbourNode.mPosition == goal.mPosition)  { return {&neighbourNode, distanceToNeighbour}; }
  if (IsForced(neighbourNode, direction) == true) { return {&neighbourNode, distanceToNeighbour}; }
  if (IsDirectionDiagonal(direction) == true) 
  {
    if (Jump(neighbourNode, direction.mH, goal, distanceToNeighbour).first != nullptr) 
    { return {&neighbourNode, distanceToNeighbour}; } 
    if (Jump(neighbourNode, direction.mV, goal, distanceToNeighbour).first != nullptr) 
    { return {&neighbourNode, distanceToNeighbour}; } 
  }
  return Jump(neighbourNode, direction, goal, distanceToNeighbour);
}

Get Successors

///
/// @brief  Get successors (next search nodes) from cursor.
/// @param  cursor
/// @return Valid node pointers list.
///
[[nodiscard]] TGetSuccessorsResult GetSuccessors(SNode& cursor, const SNode& goal) { 
  // Try prune or just get valid neighbours from cursor ( or start point ) 
  // If there is previous one (parent), do prune.
  TNeighbourPairList neighbourList = {};
  if (cursor.mParentPtr != nullptr) { neighbourList = Prune(cursor); }
  else { // Get neighbour from start point (s).
    for (const auto& direction : gDirectionList8) {
      const SEdge& edge = cursor.GetEdge(direction);
      if (edge.node == nullptr) { continue; }
      neighbourList.emplace_back(std::make_pair(direction, edge.node));
    }
  }

  // Try jump to all valid neighbour nodes and get successors for these.
  TGetSuccessorsResult returnList = {};
  for (const auto& [direction, neighbour] : neighbourList) {
    auto [successorNode, distance] = Jump(cursor, direction, goal, 0);
    if (successorNode == nullptr) { continue; }

    successorNode->mParentPtr = &cursor;
    SetDirectionToParent(*successorNode, direction);
    returnList.emplace_back(direction, distance, successorNode);
  }
  return returnList;
}

Main

void JPS(TMazeContainer& map, const DVector2& start, const DVector2& goalPos) {
  // Update huristic functions.
  for (auto& row : map) {
    for (auto& node : row) {
      if (node.type == ENodeType::Floor) { node.UpdateHuristicValueToGoal(goalPos); }
    }
  }

  // Start point setting.
  auto& startNode = map[start.y][start.x]; 
  startNode.mDistantFromStart = 0.0f;      
  TMinHeap minHeap{MinHeapCmp};       
  minHeap.push(&startNode);

  // Do JPS
  while (minHeap.empty() == false) {  
    SNode& cursor = *minHeap.top(); minHeap.pop();    // Extract-Min
    cursor.status = decltype(SNode::status)::Visited; // Move into `S` set.
    if (cursor.mPosition == goalPos) { return; }      // Check goal point.

    auto successors = GetSuccessors(cursor, map[goalPos.y][goalPos.x]);
    for (auto& successorInstnace : successors) {
      const auto& direction = successorInstnace.mDirection;
      const auto  distance  = successorInstnace.mDistance;
      auto&       successor = *successorInstnace.mSuccessor;
      Relax(direction, cursor, successor, minHeap, distance);
    }
  }
}

結論

f:id:neuliliilli:20181126002802p:plain

既存のA*よりすごく速くなりました。理論上2倍から4倍くらいの速さを見せるそうです。 ですが、ここで前処理を行って実行するJPS+を使うとA*よりなんと60倍くらい速くなるそうです。

しかしまた分量調整大失敗で、JPS+とGoalBoundingは(4)に分けて書きたいと思います。 すみません。。。うげー